diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000000000..cc55fefd258a8d --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,19 @@ +--- +Checks: ' +bugprone-*, +-bugprone-integer-division, +-bugprone-narrowing-conversions, +performance-*, +clang-analyzer-*, +misc-*, +-misc-unused-parameters, +modernize-*, +-modernize-avoid-c-arrays, +-modernize-deprecated-headers, +-modernize-use-auto, +-modernize-use-using, +-modernize-use-nullptr, +-modernize-use-trailing-return-type, +' +CheckOptions: +... diff --git a/.dir-locals.el b/.dir-locals.el new file mode 100644 index 00000000000000..53017c9fb2972f --- /dev/null +++ b/.dir-locals.el @@ -0,0 +1,3 @@ +((c++-mode (flycheck-gcc-language-standard . "c++11") + (flycheck-clang-language-standard . "c++11") + )) diff --git a/.dockerignore b/.dockerignore index 631ea048403903..4a19eb9c3f39e4 100644 --- a/.dockerignore +++ b/.dockerignore @@ -13,6 +13,26 @@ *.o-* *.os *.os-* +*.so +*.a -venv/ -.venv/ +notebooks +phone +massivemap +neos +installer +chffr/app2 +chffr/backend/env +selfdrive/nav +selfdrive/baseui +chffr/lib/vidindex/vidindex +selfdrive/test/simulator2 +**/cache_data +xx/chffr/lib/vidindex/vidindex +xx/plus +xx/community +xx/projects +!xx/projects/eon_testing_master +!xx/projects/map3d +xx/ops +xx/junk diff --git a/.editorconfig b/.editorconfig index d506433ecec172..879e6eebca7498 100644 --- a/.editorconfig +++ b/.editorconfig @@ -5,7 +5,7 @@ end_of_line = lf insert_final_newline = true trim_trailing_whitespace = true -[*.{py,pyx,pxd}] +[{*.py, *.pyx, *.pxd}] charset = utf-8 indent_style = space indent_size = 2 diff --git a/.gitattributes b/.gitattributes index 41a7367d84ae5d..7a21b223d0290d 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,23 +1,2 @@ -* text=auto - -# to move existing files into LFS: -# git add --renormalize . +*.dlc filter=lfs diff=lfs merge=lfs -text *.onnx filter=lfs diff=lfs merge=lfs -text -*.svg filter=lfs diff=lfs merge=lfs -text -*.png filter=lfs diff=lfs merge=lfs -text -*.gif filter=lfs diff=lfs merge=lfs -text -*.ttf filter=lfs diff=lfs merge=lfs -text -*.otf filter=lfs diff=lfs merge=lfs -text -*.wav filter=lfs diff=lfs merge=lfs -text - -selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text -system/hardware/tici/updater_weston filter=lfs diff=lfs merge=lfs -text -system/hardware/tici/updater_magic filter=lfs diff=lfs merge=lfs -text -third_party/**/*.a filter=lfs diff=lfs merge=lfs -text -third_party/**/*.so filter=lfs diff=lfs merge=lfs -text -third_party/**/*.so.* filter=lfs diff=lfs merge=lfs -text -third_party/**/*.dylib filter=lfs diff=lfs merge=lfs -text -third_party/acados/*/t_renderer filter=lfs diff=lfs merge=lfs -text -third_party/qt5/larch64/bin/lrelease filter=lfs diff=lfs merge=lfs -text -third_party/qt5/larch64/bin/lupdate filter=lfs diff=lfs merge=lfs -text -third_party/catch2/include/catch2/catch.hpp filter=lfs diff=lfs merge=lfs -text diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 47eb6c216fb852..b1a14076ea5f51 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -8,7 +8,6 @@ body: value: > Before creating a **bug report**, please check the following: * If the issue likely only affects your car model or make, go back and open a **car bug report** instead. - * If the issue is related to the driving or driver monitoring models, you should open a [discussion](https://github.com/commaai/openpilot/discussions/categories/model-feedback) instead. * Ensure you're running the latest openpilot release. * Ensure you're using officially supported hardware. Issues running on PCs have a different issue template. * Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue. diff --git a/.github/ISSUE_TEMPLATE/car_bug_report.yml b/.github/ISSUE_TEMPLATE/car_bug_report.yml new file mode 100644 index 00000000000000..7f368f11b49a76 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/car_bug_report.yml @@ -0,0 +1,53 @@ +name: Car bug report +description: For issues with a particular car make or model +labels: ["car", "bug"] +body: + + - type: markdown + attributes: + value: > + Before creating a **bug report**, please check the following: + * Ensure you're running the latest openpilot release. + * Ensure you're using officially supported hardware. Issues running on PCs have a different issue template. + * Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue. + * Ensure you're running stock openpilot. We cannot look into bug reports from forks. + + If you're unsure whether you've hit a bug, check out the #installation-help channel in the [community Discord server](https://discord.comma.ai). + + - type: textarea + attributes: + label: Describe the bug + description: Also include a description of how to reproduce the bug + validations: + required: true + + - type: input + id: car + attributes: + label: Which car does this affect? + placeholder: Toyota Prius 2017 + validations: + required: true + + - type: input + id: route + attributes: + label: Provide a route where the issue occurs + description: Ensure the route is fully uploaded at https://useradmin.comma.ai + placeholder: 77611a1fac303767|2020-05-11--16-37-07 + validations: + required: true + + - type: input + id: version + attributes: + label: openpilot version + description: If you're not on release, provide the commit hash + placeholder: 0.8.10 + validations: + required: true + + - type: textarea + attributes: + label: Additional info + diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 6efdc059a595e8..2c2deb17bada1b 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,14 +1,11 @@ blank_issues_enabled: false contact_links: - - name: Car bug report - url: https://github.com/commaai/opendbc/issues/new - about: For issues with a particular car make or model - - name: Join the Discord - url: https://discord.comma.ai - about: The community Discord is for both openpilot development and experience discussion - - name: Report driving behavior feedback - url: https://discord.com/channels/469524606043160576/1254834193066623017 - about: Feedback for the driving and driver monitoring models goes in the #driving-feedback in Discord + - name: Discussions + url: https://github.com/commaai/openpilot/discussions + about: For questions and discussion about openpilot - name: Community Wiki url: https://github.com/commaai/openpilot/wiki about: Check out our community wiki + - name: Community Discord + url: https://discord.comma.ai + about: Check out our community discord diff --git a/.github/ISSUE_TEMPLATE/pc_bug_report.yml b/.github/ISSUE_TEMPLATE/pc_bug_report.yml index 761c8b1a0ae923..db3eb22e5c4099 100644 --- a/.github/ISSUE_TEMPLATE/pc_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/pc_bug_report.yml @@ -24,7 +24,7 @@ body: id: os-version attributes: label: OS Version - placeholder: Ubuntu 24.04 + placeholder: Ubuntu 20.04 validations: required: true diff --git a/.github/PULL_REQUEST_TEMPLATE/bugfix.md b/.github/PULL_REQUEST_TEMPLATE/bugfix.md new file mode 100644 index 00000000000000..e28661db3b7ab3 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/bugfix.md @@ -0,0 +1,15 @@ +--- +name: Bug fix +about: For openpilot bug fixes +title: '' +labels: 'bugfix' +assignees: '' +--- + +**Description** + + + +**Verification** + + diff --git a/.github/PULL_REQUEST_TEMPLATE/car_bugfix.md b/.github/PULL_REQUEST_TEMPLATE/car_bugfix.md new file mode 100644 index 00000000000000..76c86346c85491 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/car_bugfix.md @@ -0,0 +1,19 @@ +--- +name: Car Bug fix +about: For vehicle/brand specific bug fixes +title: '' +labels: 'car bug fix' +assignees: '' +--- + +**Description** + + + +**Verification** + + + +**Route** + +Route: [a route with the bug fix] diff --git a/.github/PULL_REQUEST_TEMPLATE/car_port.md b/.github/PULL_REQUEST_TEMPLATE/car_port.md new file mode 100644 index 00000000000000..4264363ba29e8a --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/car_port.md @@ -0,0 +1,15 @@ +--- +name: Car port +about: For new car ports +title: '' +labels: 'car port' +assignees: '' +--- + +**Checklist** + +- [ ] added to README +- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py) +- [ ] route with openpilot: +- [ ] route with stock system: +- [ ] car harness used (if comma doesn't sell it, put N/A): diff --git a/.github/PULL_REQUEST_TEMPLATE/fingerprint.md b/.github/PULL_REQUEST_TEMPLATE/fingerprint.md new file mode 100644 index 00000000000000..466d4f98f4dd1f --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/fingerprint.md @@ -0,0 +1,11 @@ +--- +name: Fingerprint +about: For adding fingerprints to existing cars +title: '' +labels: 'fingerprint' +assignees: '' +--- + +Discord username: [] + +Route: [] diff --git a/.github/PULL_REQUEST_TEMPLATE/refactor.md b/.github/PULL_REQUEST_TEMPLATE/refactor.md new file mode 100644 index 00000000000000..1ee21c1bba1151 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/refactor.md @@ -0,0 +1,15 @@ +--- +name: Refactor +about: For code refactors +title: '' +labels: 'refactor' +assignees: '' +--- + +**Description** + + + +**Verification** + + diff --git a/.github/PULL_REQUEST_TEMPLATE/tuning.md b/.github/PULL_REQUEST_TEMPLATE/tuning.md new file mode 100644 index 00000000000000..05e43126992969 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/tuning.md @@ -0,0 +1,32 @@ +--- +name: Tuning +about: For openpilot tuning changes +title: '' +labels: 'tuning' +assignees: '' +--- + +**Description** + + + +**Verification** + + diff --git a/.github/labeler.yaml b/.github/labeler.yaml deleted file mode 100644 index 63d41d5b73506c..00000000000000 --- a/.github/labeler.yaml +++ /dev/null @@ -1,27 +0,0 @@ -CI / testing: - - changed-files: - - any-glob-to-all-files: "{.github/**,**/test_*,**/test/**,Jenkinsfile}" - -car: - - changed-files: - - any-glob-to-all-files: '{selfdrive/car/**,opendbc_repo}' - -simulation: - - changed-files: - - any-glob-to-all-files: 'tools/sim/**' - -ui: - - changed-files: - - any-glob-to-all-files: '{selfdrive/assets/**,selfdrive/ui/**,system/ui/**}' - -tools: - - changed-files: - - any-glob-to-all-files: 'tools/**' - -multilanguage: - - changed-files: - - any-glob-to-all-files: 'selfdrive/ui/translations/**' - -autonomy: - - changed-files: - - any-glob-to-all-files: "{selfdrive/modeld/models/**,selfdrive/test/process_replay/model_replay_ref_commit}" diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 2b4a5ed48f1b14..efa947a91a83d6 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,68 +1,38 @@ - - - - - - diff --git a/.github/workflows/auto-cache/action.yaml b/.github/workflows/auto-cache/action.yaml deleted file mode 100644 index 377b1eedcde4bb..00000000000000 --- a/.github/workflows/auto-cache/action.yaml +++ /dev/null @@ -1,58 +0,0 @@ -name: 'automatically cache based on current runner' - -inputs: - path: - description: 'path to cache' - required: true - key: - description: 'key' - required: true - restore-keys: - description: 'restore-keys' - required: true - save: - description: 'whether to save the cache' - default: 'true' - required: false -outputs: - cache-hit: - description: 'cache hit occurred' - value: ${{ (contains(runner.name, 'nsc') && steps.ns-cache.outputs.cache-hit) || - (!contains(runner.name, 'nsc') && inputs.save != 'false' && steps.gha-cache.outputs.cache-hit) || - (!contains(runner.name, 'nsc') && inputs.save == 'false' && steps.gha-cache-ro.outputs.cache-hit) }} - -runs: - using: "composite" - steps: - - name: setup namespace cache - id: ns-cache - if: ${{ contains(runner.name, 'nsc') }} - uses: namespacelabs/nscloud-cache-action@v1 - with: - path: ${{ inputs.path }} - - - name: setup github cache - id: gha-cache - if: ${{ !contains(runner.name, 'nsc') && inputs.save != 'false' }} - uses: 'actions/cache@v4' - with: - path: ${{ inputs.path }} - key: ${{ inputs.key }} - restore-keys: ${{ inputs.restore-keys }} - - - name: setup github cache - id: gha-cache-ro - if: ${{ !contains(runner.name, 'nsc') && inputs.save == 'false' }} - uses: 'actions/cache/restore@v4' - with: - path: ${{ inputs.path }} - key: ${{ inputs.key }} - restore-keys: ${{ inputs.restore-keys }} - - # make the directory manually in case we didn't get a hit, so it doesn't fail on future steps - - id: scons-cache-setup - shell: bash - run: | - mkdir -p ${{ inputs.path }} - sudo chmod -R 777 ${{ inputs.path }} - sudo chown -R $USER ${{ inputs.path }} diff --git a/.github/workflows/auto_pr_review.yaml b/.github/workflows/auto_pr_review.yaml deleted file mode 100644 index 725154d21fa8a1..00000000000000 --- a/.github/workflows/auto_pr_review.yaml +++ /dev/null @@ -1,52 +0,0 @@ -name: "PR review" -on: - pull_request_target: - types: [opened, reopened, synchronize, edited] - -jobs: - labeler: - name: review - permissions: - contents: read - pull-requests: write - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v6 - with: - submodules: false - - # Label PRs - - uses: actions/labeler@v6 - with: - dot: true - configuration-path: .github/labeler.yaml - - # Check PR target branch - - name: check branch - uses: Vankka/pr-target-branch-action@def32ec9d93514138d6ac0132ee62e120a72aed5 - if: github.repository == 'commaai/openpilot' - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - with: - target: /^(?!master$).*/ - exclude: /commaai:.*/ - change-to: ${{ github.base_ref }} - already-exists-action: close_this - already-exists-comment: "Your PR should be made against the `master` branch" - - # Welcome comment - - name: "First timers PR" - uses: actions/first-interaction@v3 - if: github.event.pull_request.head.repo.full_name != 'commaai/openpilot' - with: - repo-token: ${{ secrets.GITHUB_TOKEN }} - pr-message: | - - Thanks for contributing to openpilot! In order for us to review your PR as quickly as possible, check the following: - * Convert your PR to a draft unless it's ready to review - * Read the [contributing docs](https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md) - * Before marking as "ready for review", ensure: - * the goal is clearly stated in the description - * all the tests are passing - * the change is [something we merge](https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md#what-gets-merged) - * include a route or your device' dongle ID if relevant diff --git a/.github/workflows/badges.yaml b/.github/workflows/badges.yaml index 3f9c9c1c59009c..16edb45c21cf6b 100644 --- a/.github/workflows/badges.yaml +++ b/.github/workflows/badges.yaml @@ -7,25 +7,21 @@ on: env: BASE_IMAGE: openpilot-base DOCKER_REGISTRY: ghcr.io/commaai - RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $DOCKER_REGISTRY/$BASE_IMAGE:latest /bin/bash -c + RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $DOCKER_REGISTRY/$BASE_IMAGE:latest /bin/sh -c jobs: badges: name: create badges - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 if: github.repository == 'commaai/openpilot' - permissions: - contents: write steps: - - uses: actions/checkout@v6 + - uses: actions/checkout@v3 with: submodules: true - - uses: ./.github/workflows/setup-with-retry + - uses: ./.github/workflows/setup - name: Push badges run: | - ${{ env.RUN }} "python3 selfdrive/ui/translations/create_badges.py" - - rm .gitattributes + ${{ env.RUN }} "scons -j$(nproc) && python selfdrive/ui/translations/create_badges.py" git checkout --orphan badges git rm -rf --cached . diff --git a/.github/workflows/ci_weekly_report.yaml b/.github/workflows/ci_weekly_report.yaml deleted file mode 100644 index c7f5ec34f0b3c9..00000000000000 --- a/.github/workflows/ci_weekly_report.yaml +++ /dev/null @@ -1,101 +0,0 @@ -name: weekly CI test report -on: - schedule: - - cron: '37 9 * * 1' # 9:37AM UTC -> 2:37AM PST every monday - workflow_dispatch: - inputs: - ci_runs: - description: 'The amount of runs to trigger in CI test report' -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - -env: - CI_RUNS: ${{ github.event.inputs.ci_runs || '50' }} - -jobs: - setup: - if: github.repository == 'commaai/openpilot' - runs-on: ubuntu-latest - outputs: - ci_runs: ${{ steps.ci_runs_setup.outputs.matrix }} - steps: - - id: ci_runs_setup - name: CI_RUNS=${{ env.CI_RUNS }} - run: | - matrix=$(python3 -c "import json; print(json.dumps({ 'run_number' : list(range(${{ env.CI_RUNS }})) }))") - echo "matrix=$matrix" >> $GITHUB_OUTPUT - - ci_matrix_run: - needs: [ setup ] - strategy: - fail-fast: false - matrix: ${{fromJSON(needs.setup.outputs.ci_runs)}} - uses: commaai/openpilot/.github/workflows/ci_weekly_run.yaml@master - with: - run_number: ${{ matrix.run_number }} - - report: - needs: [ci_matrix_run] - runs-on: ubuntu-latest - if: always() && github.repository == 'commaai/openpilot' - steps: - - name: Get job results - uses: actions/github-script@v8 - id: get-job-results - with: - script: | - const jobs = await github - .paginate("GET /repos/{owner}/{repo}/actions/runs/{run_id}/attempts/{attempt}/jobs", { - owner: "commaai", - repo: "${{ github.event.repository.name }}", - run_id: "${{ github.run_id }}", - attempt: "${{ github.run_attempt }}", - }) - var report = {} - jobs.slice(1, jobs.length-1).forEach(job => { - if (job.conclusion === "skipped") return; - const jobName = job.name.split(" / ")[2]; - const runRegex = /\((.*?)\)/; - const run = job.name.match(runRegex)[1]; - report[jobName] = report[jobName] || { successes: [], failures: [], canceled: [] }; - switch (job.conclusion) { - case "success": - report[jobName].successes.push({ "run_number": run, "link": job.html_url}); break; - case "failure": - report[jobName].failures.push({ "run_number": run, "link": job.html_url }); break; - case "canceled": - report[jobName].canceled.push({ "run_number": run, "link": job.html_url }); break; - } - }); - return JSON.stringify({"jobs": report}); - - - name: Add job results to summary - env: - JOB_RESULTS: ${{ fromJSON(steps.get-job-results.outputs.result) }} - run: | - cat <> template.html - - - - - - - - - - - {% for key in jobs.keys() %} - - - - - - {% endfor %} -
Job✅ Passing❌ Failure Details
{% for i in range(5) %}{% if i+1 <= (5 * jobs[key]["successes"]|length // ${{ env.CI_RUNS }}) %}🟩{% else %}🟥{% endif %}{% endfor%}{{ key }}{{ 100 * jobs[key]["successes"]|length // ${{ env.CI_RUNS }} }}%{% if jobs[key]["failures"]|length > 0 %}
{% for failure in jobs[key]["failures"] %}Log for run #{{ failure['run_number'] }}
{% endfor %}
{% else %}{% endif %}
- EOF - - pip install jinja2-cli - echo $JOB_RESULTS | jinja2 template.html > report.html - echo "# CI Test Report - ${{ env.CI_RUNS }} Runs" >> $GITHUB_STEP_SUMMARY - cat report.html >> $GITHUB_STEP_SUMMARY diff --git a/.github/workflows/ci_weekly_run.yaml b/.github/workflows/ci_weekly_run.yaml deleted file mode 100644 index acd24de163969f..00000000000000 --- a/.github/workflows/ci_weekly_run.yaml +++ /dev/null @@ -1,17 +0,0 @@ -name: weekly CI test run -on: - workflow_call: - inputs: - run_number: - required: true - type: string - -concurrency: - group: ci-run-${{ inputs.run_number }}-${{ github.ref }} - cancel-in-progress: true - -jobs: - tests: - uses: commaai/openpilot/.github/workflows/tests.yaml@master - with: - run_number: ${{ inputs.run_number }} diff --git a/.github/workflows/compile-openpilot/action.yaml b/.github/workflows/compile-openpilot/action.yaml deleted file mode 100644 index 4015746c0e3680..00000000000000 --- a/.github/workflows/compile-openpilot/action.yaml +++ /dev/null @@ -1,21 +0,0 @@ -name: 'compile openpilot' - -runs: - using: "composite" - steps: - - shell: bash - name: Build openpilot with all flags - run: | - ${{ env.RUN }} "scons -j$(nproc)" - ${{ env.RUN }} "release/check-dirty.sh" - - shell: bash - name: Cleanup scons cache and rebuild - run: | - ${{ env.RUN }} "rm -rf /tmp/scons_cache/* && \ - scons -j$(nproc) --cache-populate" - - name: Save scons cache - uses: actions/cache/save@v4 - if: github.ref == 'refs/heads/master' - with: - path: .ci_cache/scons_cache - key: scons-${{ runner.arch }}-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml deleted file mode 100644 index 23a89de1c11085..00000000000000 --- a/.github/workflows/docs.yaml +++ /dev/null @@ -1,65 +0,0 @@ -name: docs - -on: - push: - branches: - - master - pull_request: - workflow_call: - inputs: - run_number: - default: '1' - required: true - type: string -concurrency: - group: docs-tests-ci-run-${{ inputs.run_number }}-${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && github.run_id || github.head_ref || github.ref }}-${{ github.workflow }}-${{ github.event_name }} - cancel-in-progress: true - -jobs: - docs: - name: build docs - runs-on: ubuntu-24.04 - steps: - - uses: commaai/timeout@v1 - - - uses: actions/checkout@v6 - with: - submodules: true - - # Build - - name: Build docs - run: | - # TODO: can we install just the "docs" dependency group without the normal deps? - pip install mkdocs - mkdocs build - - # Push to docs.comma.ai - - uses: actions/checkout@v6 - if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' - with: - path: openpilot-docs - ssh-key: ${{ secrets.OPENPILOT_DOCS_KEY }} - repository: commaai/openpilot-docs - - name: Push - if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' - run: | - set -x - - source release/identity.sh - - cd openpilot-docs - git checkout --orphan tmp - git rm -rf . - - # copy over docs - cp -r ../docs_site/ docs/ - - # GitHub pages config - touch docs/.nojekyll - echo -n docs.comma.ai > docs/CNAME - - git add -f . - git commit -m "build docs" - - # docs live in different repo to not bloat openpilot's full clone size - git push -f origin tmp:gh-pages diff --git a/.github/workflows/jenkins-pr-trigger.yaml b/.github/workflows/jenkins-pr-trigger.yaml deleted file mode 100644 index f8a53c5ae0ccd3..00000000000000 --- a/.github/workflows/jenkins-pr-trigger.yaml +++ /dev/null @@ -1,59 +0,0 @@ -name: jenkins scan - -on: - issue_comment: - types: [created, edited] - -jobs: - # TODO: gc old branches in a separate job in this workflow - scan-comments: - runs-on: ubuntu-latest - if: ${{ github.event.issue.pull_request }} - permissions: - contents: write - issues: write - steps: - - name: Check for trigger phrase - id: check_comment - uses: actions/github-script@v8 - with: - script: | - const triggerPhrase = "trigger-jenkins"; - const comment = context.payload.comment.body; - const commenter = context.payload.comment.user.login; - - const { data: permissions } = await github.rest.repos.getCollaboratorPermissionLevel({ - owner: context.repo.owner, - repo: context.repo.repo, - username: commenter - }); - - const hasWriteAccess = permissions.permission === 'write' || permissions.permission === 'admin'; - - return (hasWriteAccess && comment.includes(triggerPhrase)); - result-encoding: json - - - name: Checkout repository - if: steps.check_comment.outputs.result == 'true' - uses: actions/checkout@v6 - with: - ref: refs/pull/${{ github.event.issue.number }}/head - - - name: Push to tmp-jenkins branch - if: steps.check_comment.outputs.result == 'true' - run: | - git config --global user.name "github-actions[bot]" - git config --global user.email "github-actions[bot]@users.noreply.github.com" - git checkout -b tmp-jenkins-${{ github.event.issue.number }} - GIT_LFS_SKIP_PUSH=1 git push -f origin tmp-jenkins-${{ github.event.issue.number }} - - - name: Delete trigger comment - if: steps.check_comment.outputs.result == 'true' && always() - uses: actions/github-script@v8 - with: - script: | - await github.rest.issues.deleteComment({ - owner: context.repo.owner, - repo: context.repo.repo, - comment_id: context.payload.comment.id, - }); diff --git a/.github/workflows/mici_raylib_ui_preview.yaml b/.github/workflows/mici_raylib_ui_preview.yaml deleted file mode 100644 index 5025d407cd69e6..00000000000000 --- a/.github/workflows/mici_raylib_ui_preview.yaml +++ /dev/null @@ -1,151 +0,0 @@ -name: "mici raylib ui preview" -on: - push: - branches: - - master - pull_request_target: - types: [assigned, opened, synchronize, reopened, edited] - branches: - - 'master' - paths: - - 'selfdrive/assets/**' - - 'selfdrive/ui/**' - - 'system/ui/**' - workflow_dispatch: - -env: - UI_JOB_NAME: "Create mici raylib UI Report" - REPORT_NAME: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && 'master' || github.event.number }} - SHA: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && github.sha || github.event.pull_request.head.sha }} - BRANCH_NAME: "openpilot/pr-${{ github.event.number }}-mici-raylib-ui" - MASTER_BRANCH_NAME: "openpilot_master_ui_mici_raylib" - # All report files are pushed here - REPORT_FILES_BRANCH_NAME: "mici-raylib-ui-reports" - -jobs: - preview: - if: github.repository == 'commaai/openpilot' - name: preview - runs-on: ubuntu-latest - timeout-minutes: 20 - permissions: - contents: read - pull-requests: write - actions: read - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - - name: Waiting for ui generation to end - uses: lewagon/wait-on-check-action@v1.3.4 - with: - ref: ${{ env.SHA }} - check-name: ${{ env.UI_JOB_NAME }} - repo-token: ${{ secrets.GITHUB_TOKEN }} - allowed-conclusions: success - wait-interval: 20 - - - name: Getting workflow run ID - id: get_run_id - run: | - echo "run_id=$(curl https://api.github.com/repos/${{ github.repository }}/commits/${{ env.SHA }}/check-runs | jq -r '.check_runs[] | select(.name == "${{ env.UI_JOB_NAME }}") | .html_url | capture("(?[0-9]+)") | .number')" >> $GITHUB_OUTPUT - - - name: Getting proposed ui # filename: pr_ui/mici_ui_replay.mp4 - id: download-artifact - uses: dawidd6/action-download-artifact@v6 - with: - github_token: ${{ secrets.GITHUB_TOKEN }} - run_id: ${{ steps.get_run_id.outputs.run_id }} - search_artifacts: true - name: mici-raylib-report-1-${{ env.REPORT_NAME }} - path: ${{ github.workspace }}/pr_ui - - - name: Getting master ui # filename: master_ui_raylib/mici_ui_replay.mp4 - uses: actions/checkout@v6 - with: - repository: commaai/ci-artifacts - ssh-key: ${{ secrets.CI_ARTIFACTS_DEPLOY_KEY }} - path: ${{ github.workspace }}/master_ui_raylib - ref: ${{ env.MASTER_BRANCH_NAME }} - - - name: Saving new master ui - if: github.ref == 'refs/heads/master' && github.event_name == 'push' - working-directory: ${{ github.workspace }}/master_ui_raylib - run: | - git checkout --orphan=new_master_ui_mici_raylib - git rm -rf * - git branch -D ${{ env.MASTER_BRANCH_NAME }} - git branch -m ${{ env.MASTER_BRANCH_NAME }} - git config user.name "GitHub Actions Bot" - git config user.email "<>" - mv ${{ github.workspace }}/pr_ui/* . - git add . - git commit -m "mici raylib video for commit ${{ env.SHA }}" - git push origin ${{ env.MASTER_BRANCH_NAME }} --force - - - name: Setup FFmpeg - uses: AnimMouse/setup-ffmpeg@ae28d57dabbb148eff63170b6bf7f2b60062cbae - - - name: Finding diff - if: github.event_name == 'pull_request_target' - id: find_diff - run: | - # Find the video file from PR - pr_video="${{ github.workspace }}/pr_ui/mici_ui_replay_proposed.mp4" - mv "${{ github.workspace }}/pr_ui/mici_ui_replay.mp4" "$pr_video" - - master_video="${{ github.workspace }}/pr_ui/mici_ui_replay_master.mp4" - mv "${{ github.workspace }}/master_ui_raylib/mici_ui_replay.mp4" "$master_video" - - # Run report - export PYTHONPATH=${{ github.workspace }} - baseurl="https://github.com/commaai/ci-artifacts/raw/refs/heads/${{ env.BRANCH_NAME }}" - diff_exit_code=0 - python3 ${{ github.workspace }}/selfdrive/ui/tests/diff/diff.py "${{ github.workspace }}/pr_ui/mici_ui_replay_master.mp4" "${{ github.workspace }}/pr_ui/mici_ui_replay_proposed.mp4" "diff.html" --basedir "$baseurl" --no-open || diff_exit_code=$? - - # Copy diff report files - cp ${{ github.workspace }}/selfdrive/ui/tests/diff/report/diff.html ${{ github.workspace }}/pr_ui/ - cp ${{ github.workspace }}/selfdrive/ui/tests/diff/report/diff.mp4 ${{ github.workspace }}/pr_ui/ - - REPORT_URL="https://commaai.github.io/ci-artifacts/diff_pr_${{ github.event.number }}.html" - if [ $diff_exit_code -eq 0 ]; then - DIFF="✅ Videos are identical! [View Diff Report]($REPORT_URL)" - else - DIFF="❌ Videos differ! [View Diff Report]($REPORT_URL)" - fi - echo "DIFF=$DIFF" >> "$GITHUB_OUTPUT" - - - name: Saving proposed ui - if: github.event_name == 'pull_request_target' - working-directory: ${{ github.workspace }}/master_ui_raylib - run: | - # Overwrite PR branch w/ proposed ui, and master ui at this point in time for future reference - git config user.name "GitHub Actions Bot" - git config user.email "<>" - git checkout --orphan=${{ env.BRANCH_NAME }} - git rm -rf * - mv ${{ github.workspace }}/pr_ui/* . - git add . - git commit -m "mici raylib video for PR #${{ github.event.number }}" - git push origin ${{ env.BRANCH_NAME }} --force - - # Append diff report to report files branch - git fetch origin ${{ env.REPORT_FILES_BRANCH_NAME }} - git checkout ${{ env.REPORT_FILES_BRANCH_NAME }} - cp ${{ github.workspace }}/selfdrive/ui/tests/diff/report/diff.html diff_pr_${{ github.event.number }}.html - git add diff_pr_${{ github.event.number }}.html - git commit -m "mici raylib ui diff report for PR #${{ github.event.number }}" || echo "No changes to commit" - git push origin ${{ env.REPORT_FILES_BRANCH_NAME }} - - - name: Comment Video on PR - if: github.event_name == 'pull_request_target' - uses: thollander/actions-comment-pull-request@v2 - with: - message: | - - ## mici raylib UI Preview - ${{ steps.find_diff.outputs.DIFF }} - comment_tag: run_id_video_mici_raylib - pr_number: ${{ github.event.number }} - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/model_review.yaml b/.github/workflows/model_review.yaml deleted file mode 100644 index 6b8ce143dbf5ea..00000000000000 --- a/.github/workflows/model_review.yaml +++ /dev/null @@ -1,42 +0,0 @@ -name: "model review" - -on: - pull_request: - types: [opened, reopened, synchronize] - paths: - - 'selfdrive/modeld/models/*.onnx' - workflow_dispatch: - -jobs: - comment: - permissions: - contents: read - pull-requests: write - runs-on: ubuntu-latest - if: github.repository == 'commaai/openpilot' - steps: - - name: Checkout - uses: actions/checkout@v6 - - name: Checkout master - uses: actions/checkout@v6 - with: - ref: master - path: base - - run: git lfs pull - - run: cd base && git lfs pull - - - run: pip install onnx - - - name: scripts/reporter.py - id: report - run: | - echo "content<> $GITHUB_OUTPUT - echo "## Model Review" >> $GITHUB_OUTPUT - MASTER_PATH=${{ github.workspace }}/base python scripts/reporter.py >> $GITHUB_OUTPUT - echo "EOF" >> $GITHUB_OUTPUT - - - name: Post model report comment - uses: marocchino/sticky-pull-request-comment@baa7203ed60924babbe5dcd0ac8eae3b66ec5e16 - with: - header: model-review - message: ${{ steps.report.outputs.content }} \ No newline at end of file diff --git a/.github/workflows/prebuilt.yaml b/.github/workflows/prebuilt.yaml index 921c27465b94e7..b659d4ceee5c71 100644 --- a/.github/workflows/prebuilt.yaml +++ b/.github/workflows/prebuilt.yaml @@ -2,38 +2,44 @@ name: prebuilt on: schedule: - cron: '0 * * * *' + workflow_dispatch: env: + BASE_IMAGE: openpilot-base + DOCKER_REGISTRY: ghcr.io/commaai + DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }} - BUILD: selfdrive/test/docker_build.sh prebuilt + BUILD: | + docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true + docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base . jobs: build_prebuilt: name: build prebuilt - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 + timeout-minutes: 60 if: github.repository == 'commaai/openpilot' env: - PUSH_IMAGE: true - permissions: - checks: read - contents: read - packages: write + IMAGE_NAME: openpilot-prebuilt steps: - name: Wait for green check mark - if: ${{ github.event_name != 'workflow_dispatch' }} - uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc + uses: lewagon/wait-on-check-action@e2558238c09778af25867eb5de5a3ce4bbae3dcd with: ref: master wait-interval: 30 running-workflow-name: 'build prebuilt' - repo-token: ${{ secrets.GITHUB_TOKEN }} check-regexp: ^((?!.*(build master-ci).*).)*$ - - uses: actions/checkout@v6 + - uses: actions/checkout@v3 with: submodules: true - - run: git lfs pull - - name: Build and Push docker image + - name: Build Docker image run: | - $DOCKER_LOGIN eval "$BUILD" + docker pull $DOCKER_REGISTRY/$IMAGE_NAME:latest || true + docker build --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f Dockerfile.openpilot . + - name: Push to container registry + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/$IMAGE_NAME:latest diff --git a/.github/workflows/raylib_ui_preview.yaml b/.github/workflows/raylib_ui_preview.yaml deleted file mode 100644 index 9044a97f536b46..00000000000000 --- a/.github/workflows/raylib_ui_preview.yaml +++ /dev/null @@ -1,175 +0,0 @@ -name: "raylib ui preview" -on: - push: - branches: - - master - pull_request_target: - types: [assigned, opened, synchronize, reopened, edited] - branches: - - 'master' - paths: - - 'selfdrive/assets/**' - - 'selfdrive/ui/**' - - 'system/ui/**' - workflow_dispatch: - -env: - UI_JOB_NAME: "Create raylib UI Report" - REPORT_NAME: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && 'master' || github.event.number }} - SHA: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && github.sha || github.event.pull_request.head.sha }} - BRANCH_NAME: "openpilot/pr-${{ github.event.number }}-raylib-ui" - -jobs: - preview: - if: github.repository == 'commaai/openpilot' - name: preview - runs-on: ubuntu-latest - timeout-minutes: 20 - permissions: - contents: read - pull-requests: write - actions: read - steps: - - name: Waiting for ui generation to start - run: sleep 30 - - - name: Waiting for ui generation to end - uses: lewagon/wait-on-check-action@v1.3.4 - with: - ref: ${{ env.SHA }} - check-name: ${{ env.UI_JOB_NAME }} - repo-token: ${{ secrets.GITHUB_TOKEN }} - allowed-conclusions: success - wait-interval: 20 - - - name: Getting workflow run ID - id: get_run_id - run: | - echo "run_id=$(curl https://api.github.com/repos/${{ github.repository }}/commits/${{ env.SHA }}/check-runs | jq -r '.check_runs[] | select(.name == "${{ env.UI_JOB_NAME }}") | .html_url | capture("(?[0-9]+)") | .number')" >> $GITHUB_OUTPUT - - - name: Getting proposed ui - id: download-artifact - uses: dawidd6/action-download-artifact@v6 - with: - github_token: ${{ secrets.GITHUB_TOKEN }} - run_id: ${{ steps.get_run_id.outputs.run_id }} - search_artifacts: true - name: raylib-report-1-${{ env.REPORT_NAME }} - path: ${{ github.workspace }}/pr_ui - - - name: Getting master ui - uses: actions/checkout@v6 - with: - repository: commaai/ci-artifacts - ssh-key: ${{ secrets.CI_ARTIFACTS_DEPLOY_KEY }} - path: ${{ github.workspace }}/master_ui_raylib - ref: openpilot_master_ui_raylib - - - name: Saving new master ui - if: github.ref == 'refs/heads/master' && github.event_name == 'push' - working-directory: ${{ github.workspace }}/master_ui_raylib - run: | - git checkout --orphan=new_master_ui_raylib - git rm -rf * - git branch -D openpilot_master_ui_raylib - git branch -m openpilot_master_ui_raylib - git config user.name "GitHub Actions Bot" - git config user.email "<>" - mv ${{ github.workspace }}/pr_ui/*.png . - git add . - git commit -m "raylib screenshots for commit ${{ env.SHA }}" - git push origin openpilot_master_ui_raylib --force - - - name: Finding diff - if: github.event_name == 'pull_request_target' - id: find_diff - run: >- - sudo apt-get update && sudo apt-get install -y imagemagick - - scenes=$(find ${{ github.workspace }}/pr_ui/*.png -type f -printf "%f\n" | cut -d '.' -f 1 | grep -v 'pair_device') - A=($scenes) - - DIFF="" - TABLE="
All Screenshots" - TABLE="${TABLE}" - - for ((i=0; i<${#A[*]}; i=i+1)); - do - # Check if the master file exists - if [ ! -f "${{ github.workspace }}/master_ui_raylib/${A[$i]}.png" ]; then - # This is a new file in PR UI that doesn't exist in master - DIFF="${DIFF}
" - DIFF="${DIFF}${A[$i]} : \$\${\\color{cyan}\\text{NEW}}\$\$" - DIFF="${DIFF}
" - - DIFF="${DIFF}" - DIFF="${DIFF} " - DIFF="${DIFF}" - - DIFF="${DIFF}
" - DIFF="${DIFF}
" - elif ! compare -fuzz 2% -highlight-color DeepSkyBlue1 -lowlight-color Black -compose Src ${{ github.workspace }}/master_ui_raylib/${A[$i]}.png ${{ github.workspace }}/pr_ui/${A[$i]}.png ${{ github.workspace }}/pr_ui/${A[$i]}_diff.png; then - convert ${{ github.workspace }}/pr_ui/${A[$i]}_diff.png -transparent black mask.png - composite mask.png ${{ github.workspace }}/master_ui_raylib/${A[$i]}.png composite_diff.png - convert -delay 100 ${{ github.workspace }}/master_ui_raylib/${A[$i]}.png composite_diff.png -loop 0 ${{ github.workspace }}/pr_ui/${A[$i]}_diff.gif - - mv ${{ github.workspace }}/master_ui_raylib/${A[$i]}.png ${{ github.workspace }}/pr_ui/${A[$i]}_master_ref.png - - DIFF="${DIFF}
" - DIFF="${DIFF}${A[$i]} : \$\${\\color{red}\\text{DIFFERENT}}\$\$" - DIFF="${DIFF}" - - DIFF="${DIFF}" - DIFF="${DIFF} " - DIFF="${DIFF} " - DIFF="${DIFF}" - - DIFF="${DIFF}" - DIFF="${DIFF} " - DIFF="${DIFF} " - DIFF="${DIFF}" - - DIFF="${DIFF}
master proposed
diff composite diff
" - DIFF="${DIFF}
" - else - rm -f ${{ github.workspace }}/pr_ui/${A[$i]}_diff.png - fi - - INDEX=$(($i % 2)) - if [[ $INDEX -eq 0 ]]; then - TABLE="${TABLE}" - fi - TABLE="${TABLE} " - if [[ $INDEX -eq 1 || $(($i + 1)) -eq ${#A[*]} ]]; then - TABLE="${TABLE}" - fi - done - - TABLE="${TABLE}" - - echo "DIFF=$DIFF$TABLE" >> "$GITHUB_OUTPUT" - - - name: Saving proposed ui - if: github.event_name == 'pull_request_target' - working-directory: ${{ github.workspace }}/master_ui_raylib - run: | - git config user.name "GitHub Actions Bot" - git config user.email "<>" - git checkout --orphan=${{ env.BRANCH_NAME }} - git rm -rf * - mv ${{ github.workspace }}/pr_ui/* . - git add . - git commit -m "raylib screenshots for PR #${{ github.event.number }}" - git push origin ${{ env.BRANCH_NAME }} --force - - - name: Comment Screenshots on PR - if: github.event_name == 'pull_request_target' - uses: thollander/actions-comment-pull-request@v2 - with: - message: | - - ## raylib UI Preview - ${{ steps.find_diff.outputs.DIFF }} - comment_tag: run_id_screenshots_raylib - pr_number: ${{ github.event.number }} - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/release.yaml b/.github/workflows/release.yaml index 0f34dbe435bf93..8df89dcc384a1a 100644 --- a/.github/workflows/release.yaml +++ b/.github/workflows/release.yaml @@ -1,42 +1,29 @@ name: release on: schedule: - - cron: '0 9 * * *' + - cron: '0 * * * *' workflow_dispatch: jobs: build_masterci: name: build master-ci - env: - ImageOS: ubuntu24 - container: - image: ghcr.io/commaai/openpilot-base:latest - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 + timeout-minutes: 60 if: github.repository == 'commaai/openpilot' - permissions: - checks: read - contents: write steps: - - name: Install wait-on-check-action dependencies - run: | - sudo apt-get update - sudo apt-get install -y libyaml-dev - name: Wait for green check mark - if: ${{ github.event_name == 'schedule' }} - uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc + uses: lewagon/wait-on-check-action@e2558238c09778af25867eb5de5a3ce4bbae3dcd with: ref: master wait-interval: 30 running-workflow-name: 'build master-ci' - repo-token: ${{ secrets.GITHUB_TOKEN }} check-regexp: ^((?!.*(build prebuilt).*).)*$ - - uses: actions/checkout@v6 + - uses: actions/checkout@v3 with: submodules: true fetch-depth: 0 - name: Pull LFS + run: git lfs pull + - name: Build master-ci run: | - git config --global --add safe.directory '*' - git lfs pull - - name: Push master-ci - run: BRANCH=__nightly release/build_stripped.sh + BRANCH=master-ci release/build_devel.sh diff --git a/.github/workflows/repo-maintenance.yaml b/.github/workflows/repo-maintenance.yaml deleted file mode 100644 index 810b602d711886..00000000000000 --- a/.github/workflows/repo-maintenance.yaml +++ /dev/null @@ -1,72 +0,0 @@ -name: repo maintenance - -on: - schedule: - - cron: "0 14 * * 1" # every Monday at 2am UTC (6am PST) - workflow_dispatch: - -env: - BASE_IMAGE: openpilot-base - BUILD: selfdrive/test/docker_build.sh base - RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c - -jobs: - update_translations: - runs-on: ubuntu-latest - if: github.repository == 'commaai/openpilot' - steps: - - uses: actions/checkout@v6 - - uses: ./.github/workflows/setup-with-retry - - name: Update translations - run: | - ${{ env.RUN }} "python3 selfdrive/ui/update_translations.py --vanish" - - name: Create Pull Request - uses: peter-evans/create-pull-request@9153d834b60caba6d51c9b9510b087acf9f33f83 - with: - author: Vehicle Researcher - commit-message: "Update translations" - title: "[bot] Update translations" - body: "Automatic PR from repo-maintenance -> update_translations" - branch: "update-translations" - base: "master" - delete-branch: true - labels: bot - - package_updates: - name: package_updates - runs-on: ubuntu-latest - container: - image: ghcr.io/commaai/openpilot-base:latest - if: github.repository == 'commaai/openpilot' - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - name: uv lock - run: | - python3 -m ensurepip --upgrade - pip3 install uv - uv lock --upgrade - - name: bump submodules - run: | - git config --global --add safe.directory '*' - git submodule update --remote - git add . - - name: update car docs - run: | - export PYTHONPATH="$PWD" - scons -j$(nproc) --minimal opendbc_repo - python selfdrive/car/docs.py - git add docs/CARS.md - - name: Create Pull Request - uses: peter-evans/create-pull-request@9153d834b60caba6d51c9b9510b087acf9f33f83 - with: - author: Vehicle Researcher - token: ${{ secrets.ACTIONS_CREATE_PR_PAT }} - commit-message: Update Python packages - title: '[bot] Update Python packages' - branch: auto-package-updates - base: master - delete-branch: true - body: 'Automatic PR from repo-maintenance -> package_updates' - labels: bot diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml new file mode 100644 index 00000000000000..9606c0563151ff --- /dev/null +++ b/.github/workflows/selfdrive_tests.yaml @@ -0,0 +1,414 @@ +name: selfdrive +on: + push: + branches-ignore: + - 'testing-closet*' + pull_request: + +env: + BASE_IMAGE: openpilot-base + CL_BASE_IMAGE: openpilot-base-cl + DOCKER_REGISTRY: ghcr.io/commaai + AZURE_TOKEN: ${{ secrets.AZURE_COMMADATACI_OPENPILOTCI_TOKEN }} + + DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }} + BUILD: | + docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true + docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base . + + RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c + + BUILD_CL: | + docker pull $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest || true + docker build --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl . + RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c + + UNIT_TEST: coverage run --append -m unittest discover + +jobs: + build_release: + name: build release + runs-on: ubuntu-20.04 + timeout-minutes: 30 + env: + STRIPPED_DIR: /tmp/releasepilot + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + submodules: true + - name: Build devel + run: TARGET_DIR=$STRIPPED_DIR release/build_devel.sh + - uses: ./.github/workflows/setup + with: + save-cache: true + - name: Check submodules + if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' + run: release/check-submodules.sh + - name: Build openpilot and run checks + run: | + cd $STRIPPED_DIR + ${{ env.RUN }} "CI=1 python selfdrive/manager/build.py" + - name: Run tests + run: | + cd $STRIPPED_DIR + ${{ env.RUN }} "release/check-dirty.sh && \ + python -m unittest discover selfdrive/car" + - name: pre-commit + run: | + cd $GITHUB_WORKSPACE + cp .pre-commit-config.yaml $STRIPPED_DIR + cp .pylintrc $STRIPPED_DIR + cp mypy.ini $STRIPPED_DIR + cd $STRIPPED_DIR + ${{ env.RUN }} "pre-commit run --all" + + build_all: + name: build all + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Build openpilot with all flags + run: ${{ env.RUN }} "scons -j$(nproc) --extras && release/check-dirty.sh" + - name: Cleanup scons cache + run: | + ${{ env.RUN }} "rm -rf /tmp/scons_cache/* && \ + scons -j$(nproc) --extras --cache-populate" + + #build_mac: + # name: build macos + # runs-on: macos-latest + # timeout-minutes: 60 + # steps: + # - uses: actions/checkout@v3 + # with: + # submodules: true + # - name: Determine pre-existing Homebrew packages + # if: steps.dependency-cache.outputs.cache-hit != 'true' + # run: | + # echo 'EXISTING_CELLAR<> $GITHUB_ENV + # ls -1 /usr/local/Cellar >> $GITHUB_ENV + # echo 'EOF' >> $GITHUB_ENV + # - name: Cache dependencies + # id: dependency-cache + # uses: actions/cache@v2 + # with: + # path: | + # ~/.pyenv + # ~/.local/share/virtualenvs/ + # /usr/local/Cellar + # ~/github_brew_cache_entries.txt + # /tmp/scons_cache + # key: macos-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'Pipfile*') }} + # restore-keys: macos- + # - name: Brew link restored dependencies + # run: | + # if [ -f ~/github_brew_cache_entries.txt ]; then + # while read pkg; do + # brew link --force "$pkg" # `--force` for keg-only packages + # done < ~/github_brew_cache_entries.txt + # else + # echo "Cache entries not found" + # fi + # - name: Install dependencies + # run: ./tools/mac_setup.sh + # - name: Build openpilot + # run: | + # source tools/openpilot_env.sh + # pipenv run selfdrive/manager/build.py + # + # # cleanup scons cache + # rm -rf /tmp/scons_cache/ + # pipenv run scons -j$(nproc) --cache-populate + # - name: Remove pre-existing Homebrew packages for caching + # if: steps.dependency-cache.outputs.cache-hit != 'true' + # run: | + # cd /usr/local/Cellar + # new_cellar=$(ls -1) + # comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do + # if [[ $pkg != "zstd" ]]; then # caching step needs zstd + # rm -rf "$pkg" + # fi + # done + # comm -13 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | tee ~/github_brew_cache_entries.txt + + docker_push: + name: docker push + runs-on: ubuntu-20.04 + timeout-minutes: 50 + if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot' + needs: static_analysis # hack to ensure slow tests run first since this and static_analysis are fast + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Build Docker image + run: eval "$BUILD" + - name: Push to container registry + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest + - name: Build CL Docker image + run: eval "$BUILD_CL" + - name: Push to container registry + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest + + static_analysis: + name: static analysis + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Build Docker image + run: eval "$BUILD" + - name: pre-commit + run: ${{ env.RUN }} "pre-commit run --all" + + valgrind: + name: valgrind + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Run valgrind + run: | + ${{ env.RUN }} "scons -j$(nproc) && \ + python selfdrive/test/test_valgrind_replay.py" + - name: Print logs + if: always() + run: cat selfdrive/test/valgrind_logs.txt + + unit_tests: + name: unit tests + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Run unit tests + run: | + ${{ env.RUN }} "export SKIP_LONG_TESTS=1 && \ + scons -j$(nproc) && \ + $UNIT_TEST common && \ + $UNIT_TEST opendbc/can && \ + $UNIT_TEST selfdrive/boardd && \ + $UNIT_TEST selfdrive/controls && \ + $UNIT_TEST selfdrive/monitoring && \ + $UNIT_TEST selfdrive/loggerd && \ + $UNIT_TEST selfdrive/car && \ + $UNIT_TEST selfdrive/locationd && \ + selfdrive/locationd/test/_test_locationd_lib.py && \ + $UNIT_TEST selfdrive/athena && \ + $UNIT_TEST selfdrive/thermald && \ + $UNIT_TEST system/hardware/tici && \ + $UNIT_TEST selfdrive/modeld && \ + $UNIT_TEST tools/lib/tests && \ + ./selfdrive/ui/tests/create_test_translations.sh && \ + QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \ + ./selfdrive/ui/tests/test_translations.py && \ + ./common/tests/test_util && \ + ./common/tests/test_swaglog && \ + ./selfdrive/boardd/tests/test_boardd_usbprotocol && \ + ./selfdrive/loggerd/tests/test_logger &&\ + ./system/proclogd/tests/test_proclog && \ + ./tools/replay/tests/test_replay && \ + ./system/camerad/test/ae_gray_test && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 + + process_replay: + name: process replay + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Cache test routes + id: dependency-cache + uses: actions/cache@v3 + with: + path: /tmp/comma_download_cache + key: proc-replay-${{ hashFiles('.github/workflows/selfdrive_tests.yaml', 'selfdrive/test/process_replay/ref_commit') }} + - name: Run replay + run: | + ${{ env.RUN }} "scons -j$(nproc) && \ + CI=1 coverage run selfdrive/test/process_replay/test_processes.py -j$(nproc) && \ + coverage xml" + - name: Print diff + if: always() + run: cat selfdrive/test/process_replay/diff.txt + - uses: actions/upload-artifact@v2 + if: always() + continue-on-error: true + with: + name: process_replay_diff.txt + path: selfdrive/test/process_replay/diff.txt + - name: Upload reference logs + if: ${{ failure() && github.event_name == 'pull_request' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }} + run: | + ${{ env.RUN }} "scons -j$(nproc) && \ + CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 + + model_replay_onnx: + name: model replay onnx + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Build Docker image + # Sim docker is needed to get the OpenCL drivers + run: eval "$BUILD_CL" + - name: Run replay + run: | + ${{ env.RUN_CL }} "scons -j$(nproc) && \ + ONNXCPU=1 CI=1 coverage run \ + selfdrive/test/process_replay/model_replay.py -j$(nproc) && \ + coverage xml" + + test_longitudinal: + name: longitudinal + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Test longitudinal + run: | + ${{ env.RUN }} "mkdir -p selfdrive/test/out && \ + scons -j$(nproc) && \ + cd selfdrive/test/longitudinal_maneuvers && \ + coverage run ./test_longitudinal.py && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 + - uses: actions/upload-artifact@v2 + if: always() + continue-on-error: true + with: + name: longitudinal + path: selfdrive/test/longitudinal_maneuvers/out/longitudinal/ + + test_cars: + name: cars + runs-on: ubuntu-20.04 + timeout-minutes: 50 + strategy: + fail-fast: false + matrix: + job: [0, 1, 2, 3, 4] + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - uses: ./.github/workflows/setup + - name: Cache test routes + id: dependency-cache + uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b + with: + path: /tmp/comma_download_cache + key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/car/tests/routes.py') }}-${{ matrix.job }} + - name: Test car models + run: | + ${{ env.RUN }} "scons -j$(nproc) && \ + coverage run -m pytest selfdrive/car/tests/test_models.py && \ + coverage xml && \ + chmod -R 777 /tmp/comma_download_cache" + env: + NUM_JOBS: 5 + JOB_ID: ${{ matrix.job }} + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 + + docs: + name: build docs + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Build docker container + run: | + docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker pull $DOCKER_REGISTRY/openpilot-docs:latest || true + DOCKER_BUILDKIT=1 docker build --cache-from $DOCKER_REGISTRY/openpilot-docs:latest -t $DOCKER_REGISTRY/openpilot-docs:latest -f docs/docker/Dockerfile . + - name: Push docker container + if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot' + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/openpilot-docs:latest + + car_docs_diff: + name: comment on PR with car docs diff + runs-on: ubuntu-20.04 + timeout-minutes: 50 + if: github.event_name == 'pull_request' + steps: + - uses: actions/checkout@v3 + with: + submodules: true + ref: ${{ github.event.pull_request.base.ref }} + - uses: ./.github/workflows/setup + - name: Get base car info + run: | + ${{ env.RUN }} "scons -j$(nproc) && python selfdrive/debug/dump_car_info.py --path /tmp/openpilot_cache/base_car_info" + sudo chown -R $USER:$USER ${{ github.workspace }} + - uses: actions/checkout@v3 + with: + submodules: true + - name: Save car docs diff + id: save_diff + run: | + ${{ env.RUN }} "scons -j$(nproc)" + output=$(${{ env.RUN }} "python selfdrive/debug/print_docs_diff.py --path /tmp/openpilot_cache/base_car_info") || true + output="${output//$'\n'/'%0A'}" + echo "::set-output name=diff::$output" + - name: Find comment + if: ${{ env.AZURE_TOKEN != '' }} + uses: peter-evans/find-comment@1769778a0c5bd330272d749d12c036d65e70d39d + id: fc + with: + issue-number: ${{ github.event.pull_request.number }} + body-includes: This PR makes changes to + - name: Update comment + if: ${{ steps.save_diff.outputs.diff != '' && env.AZURE_TOKEN != '' }} + uses: peter-evans/create-or-update-comment@b95e16d2859ad843a14218d1028da5b2c4cbc4b4 + with: + comment-id: ${{ steps.fc.outputs.comment-id }} + issue-number: ${{ github.event.pull_request.number }} + body: "${{ steps.save_diff.outputs.diff }}" + edit-mode: replace + - name: Delete comment + if: ${{ steps.fc.outputs.comment-id != '' && steps.save_diff.outputs.diff == '' && env.AZURE_TOKEN != '' }} + uses: actions/github-script@v6 + with: + script: | + github.rest.issues.deleteComment({ + owner: context.repo.owner, + repo: context.repo.repo, + comment_id: ${{ steps.fc.outputs.comment-id }} + }) diff --git a/.github/workflows/setup-with-retry/action.yaml b/.github/workflows/setup-with-retry/action.yaml deleted file mode 100644 index 98a3913600b9f8..00000000000000 --- a/.github/workflows/setup-with-retry/action.yaml +++ /dev/null @@ -1,52 +0,0 @@ -name: 'openpilot env setup, with retry on failure' - -inputs: - docker_hub_pat: - description: 'Auth token for Docker Hub, required for BuildJet jobs' - required: false - default: '' - sleep_time: - description: 'Time to sleep between retries' - required: false - default: 30 - -outputs: - duration: - description: 'Duration of the setup process in seconds' - value: ${{ steps.get_duration.outputs.duration }} - -runs: - using: "composite" - steps: - - id: start_time - shell: bash - run: echo "START_TIME=$(date +%s)" >> $GITHUB_ENV - - id: setup1 - uses: ./.github/workflows/setup - continue-on-error: true - with: - is_retried: true - - if: steps.setup1.outcome == 'failure' - shell: bash - run: sleep ${{ inputs.sleep_time }} - - id: setup2 - if: steps.setup1.outcome == 'failure' - uses: ./.github/workflows/setup - continue-on-error: true - with: - is_retried: true - - if: steps.setup2.outcome == 'failure' - shell: bash - run: sleep ${{ inputs.sleep_time }} - - id: setup3 - if: steps.setup2.outcome == 'failure' - uses: ./.github/workflows/setup - with: - is_retried: true - - id: get_duration - shell: bash - run: | - END_TIME=$(date +%s) - DURATION=$((END_TIME - START_TIME)) - echo "Total duration: $DURATION seconds" - echo "duration=$DURATION" >> $GITHUB_OUTPUT diff --git a/.github/workflows/setup/action.yaml b/.github/workflows/setup/action.yaml index 818060c3b010cc..79c4c890abec60 100644 --- a/.github/workflows/setup/action.yaml +++ b/.github/workflows/setup/action.yaml @@ -1,31 +1,13 @@ name: 'openpilot env setup' inputs: - is_retried: - description: 'A mock param that asserts that we use the setup-with-retry instead of this action directly' + save-cache: + default: false required: false - default: 'false' runs: using: "composite" steps: - # assert that this action is retried using the setup-with-retry - - shell: bash - if: ${{ inputs.is_retried == 'false' }} - run: | - echo "You should not run this action directly. Use setup-with-retry instead" - exit 1 - - - shell: bash - name: No retries! - run: | - if [ "${{ github.run_attempt }}" -gt 1 ]; then - echo -e "\033[0;31m##################################################" - echo -e "\033[0;31m Retries not allowed! Fix the flaky test! " - echo -e "\033[0;31m##################################################\033[0m" - exit 1 - fi - # do this after checkout to ensure our custom LFS config is used to pull from GitLab - shell: bash run: git lfs pull @@ -33,24 +15,23 @@ runs: # build cache - id: date shell: bash - run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV + run: echo "::set-output name=date::$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d')" - shell: bash - run: echo "$CACHE_COMMIT_DATE" + run: echo "${{ steps.date.outputs.date }}" + - shell: bash + run: echo "CACHE_SKIP_SAVE=true" >> $GITHUB_ENV + if: github.ref != 'refs/heads/master' || inputs.save-cache == 'false' - id: scons-cache - uses: ./.github/workflows/auto-cache + # TODO: change the version to the released version + # when https://github.com/actions/cache/pull/489 (or 571) is merged. + uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b with: - path: .ci_cache/scons_cache - key: scons-${{ runner.arch }}-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} + path: /tmp/scons_cache + key: scons-${{ steps.date.outputs.date }}-${{ github.sha }} restore-keys: | - scons-${{ runner.arch }}-${{ env.CACHE_COMMIT_DATE }} - scons-${{ runner.arch }} - # as suggested here: https://github.com/moby/moby/issues/32816#issuecomment-910030001 - - id: normalize-file-permissions - shell: bash - name: Normalize file permissions to ensure a consistent docker build cache - run: | - find . -type f -executable -not -perm 755 -exec chmod 755 {} \; - find . -type f -not -executable -not -perm 644 -exec chmod 644 {} \; + scons-${{ steps.date.outputs.date }}- + scons- + # build our docker image - shell: bash run: eval ${{ env.BUILD }} diff --git a/.github/workflows/stale.yaml b/.github/workflows/stale.yaml deleted file mode 100644 index cb7c0ac0764ca0..00000000000000 --- a/.github/workflows/stale.yaml +++ /dev/null @@ -1,52 +0,0 @@ -name: stale -on: - schedule: - - cron: '30 1 * * *' - workflow_dispatch: - -env: - DAYS_BEFORE_PR_CLOSE: 7 - DAYS_BEFORE_PR_STALE: 24 - DAYS_BEFORE_PR_STALE_DRAFT: 30 - -jobs: - stale: - runs-on: ubuntu-latest - steps: - - uses: actions/stale@v10 - with: - exempt-all-milestones: true - - # pull request config - stale-pr-message: 'This PR has had no activity for ${{ env.DAYS_BEFORE_PR_STALE }} days. It will be automatically closed in ${{ env.DAYS_BEFORE_PR_CLOSE }} days if there is no activity.' - close-pr-message: 'This PR has been automatically closed due to inactivity. Feel free to re-open once activity resumes.' - stale-pr-label: stale - delete-branch: ${{ github.event.pull_request.head.repo.full_name == 'commaai/openpilot' }} # only delete branches on the main repo - exempt-pr-labels: "ignore stale,needs testing" # if wip or it needs testing from the community, don't mark as stale - days-before-pr-stale: ${{ env.DAYS_BEFORE_PR_STALE }} - days-before-pr-close: ${{ env.DAYS_BEFORE_PR_CLOSE }} - exempt-draft-pr: false - - # issue config - days-before-issue-stale: -1 # ignore issues for now - - # same as above, but give draft PRs more time - stale_drafts: - runs-on: ubuntu-latest - steps: - - uses: actions/stale@v10 - with: - exempt-all-milestones: true - - # pull request config - stale-pr-message: 'This PR has had no activity for ${{ env.DAYS_BEFORE_PR_STALE_DRAFT }} days. It will be automatically closed in ${{ env.DAYS_BEFORE_PR_CLOSE }} days if there is no activity.' - close-pr-message: 'This PR has been automatically closed due to inactivity. Feel free to re-open once activity resumes.' - stale-pr-label: stale - delete-branch: ${{ github.event.pull_request.head.repo.full_name == 'commaai/openpilot' }} # only delete branches on the main repo - exempt-pr-labels: "ignore stale,needs testing" # if wip or it needs testing from the community, don't mark as stale - days-before-pr-stale: ${{ env.DAYS_BEFORE_PR_STALE_DRAFT }} - days-before-pr-close: ${{ env.DAYS_BEFORE_PR_CLOSE }} - exempt-draft-pr: true - - # issue config - days-before-issue-stale: -1 # ignore issues for now diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml deleted file mode 100644 index 4ade42b665c8f0..00000000000000 --- a/.github/workflows/tests.yaml +++ /dev/null @@ -1,293 +0,0 @@ -name: tests - -on: - push: - branches: - - master - pull_request: - workflow_dispatch: - workflow_call: - inputs: - run_number: - default: '1' - required: true - type: string - -concurrency: - group: tests-ci-run-${{ inputs.run_number }}-${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && github.run_id || github.head_ref || github.ref }}-${{ github.workflow }}-${{ github.event_name }} - cancel-in-progress: true - -env: - PYTHONWARNINGS: error - BASE_IMAGE: openpilot-base - AZURE_TOKEN: ${{ secrets.AZURE_COMMADATACI_OPENPILOTCI_TOKEN }} - - DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }} - BUILD: selfdrive/test/docker_build.sh base - - RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c - - PYTEST: pytest --continue-on-collection-errors --durations=0 -n logical - -jobs: - build_release: - name: build release - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - env: - STRIPPED_DIR: /tmp/releasepilot - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - name: Getting LFS files - uses: nick-fields/retry@7152eba30c6575329ac0576536151aca5a72780e - with: - timeout_minutes: 2 - max_attempts: 3 - command: git lfs pull - - name: Build devel - timeout-minutes: 1 - run: TARGET_DIR=$STRIPPED_DIR release/build_stripped.sh - - uses: ./.github/workflows/setup-with-retry - - name: Build openpilot and run checks - timeout-minutes: ${{ ((steps.restore-scons-cache.outputs.cache-hit == 'true') && 10 || 30) }} # allow more time when we missed the scons cache - run: | - cd $STRIPPED_DIR - ${{ env.RUN }} "python3 system/manager/build.py" - - name: Run tests - timeout-minutes: 1 - run: | - cd $STRIPPED_DIR - ${{ env.RUN }} "release/check-dirty.sh" - - name: Check submodules - if: github.repository == 'commaai/openpilot' - timeout-minutes: 3 - run: release/check-submodules.sh - - build: - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - name: Setup docker push - if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot' - run: | - echo "PUSH_IMAGE=true" >> "$GITHUB_ENV" - $DOCKER_LOGIN - - uses: ./.github/workflows/setup-with-retry - - uses: ./.github/workflows/compile-openpilot - timeout-minutes: 30 - - build_mac: - name: build macOS - 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@v6 - with: - submodules: true - - 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 - with: - path: ~/Library/Caches/Homebrew - key: brew-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} - restore-keys: | - brew-macos-${{ env.CACHE_COMMIT_DATE }} - brew-macos - - name: Install dependencies - run: ./tools/mac_setup.sh - env: - PYTHONWARNINGS: default # package install has DeprecationWarnings - HOMEBREW_DISPLAY_INSTALL_TIMES: 1 - - run: git lfs pull - - name: Getting scons cache - uses: ./.github/workflows/auto-cache - with: - path: /tmp/scons_cache - key: scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} - restore-keys: | - scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }} - scons-${{ runner.arch }}-macos - - name: Building openpilot - run: . .venv/bin/activate && scons -j$(nproc) - - static_analysis: - name: static analysis - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - env: - PYTHONWARNINGS: default - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - - name: Static analysis - timeout-minutes: 1 - run: ${{ env.RUN }} "scripts/lint/lint.sh" - - unit_tests: - name: unit tests - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - id: setup-step - - name: Build openpilot - run: ${{ env.RUN }} "scons -j$(nproc)" - - name: Run unit tests - timeout-minutes: ${{ contains(runner.name, 'nsc') && ((steps.setup-step.outputs.duration < 18) && 1 || 2) || 20 }} - run: | - ${{ env.RUN }} "source selfdrive/test/setup_xvfb.sh && \ - # Pre-compile Python bytecode so each pytest worker doesn't need to - $PYTEST --collect-only -m 'not slow' -qq && \ - MAX_EXAMPLES=1 $PYTEST -m 'not slow' && \ - chmod -R 777 /tmp/comma_download_cache" - - process_replay: - name: process replay - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - id: setup-step - - name: Cache test routes - id: dependency-cache - uses: actions/cache@v5 - with: - path: .ci_cache/comma_download_cache - key: proc-replay-${{ hashFiles('selfdrive/test/process_replay/ref_commit', 'selfdrive/test/process_replay/test_processes.py') }} - - name: Build openpilot - run: | - ${{ env.RUN }} "scons -j$(nproc)" - - name: Run replay - timeout-minutes: ${{ contains(runner.name, 'nsc') && (steps.dependency-cache.outputs.cache-hit == 'true') && ((steps.setup-step.outputs.duration < 18) && 1 || 2) || 20 }} - run: | - ${{ env.RUN }} "selfdrive/test/process_replay/test_processes.py -j$(nproc) && \ - chmod -R 777 /tmp/comma_download_cache" - - name: Print diff - id: print-diff - if: always() - run: cat selfdrive/test/process_replay/diff.txt - - uses: actions/upload-artifact@v6 - if: always() - continue-on-error: true - with: - name: process_replay_diff.txt - path: selfdrive/test/process_replay/diff.txt - - name: Upload reference logs - if: false # TODO: move this to github instead of azure - run: | - ${{ env.RUN }} "unset PYTHONWARNINGS && AZURE_TOKEN='$AZURE_TOKEN' python3 selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only" - - name: Run regen - if: false - timeout-minutes: 4 - run: | - ${{ env.RUN }} "ONNXCPU=1 $PYTEST selfdrive/test/process_replay/test_regen.py && \ - chmod -R 777 /tmp/comma_download_cache" - - simulator_driving: - name: simulator driving - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - if: false # FIXME: Started to timeout recently - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - id: setup-step - - name: Build openpilot - run: | - ${{ env.RUN }} "scons -j$(nproc)" - - name: Driving test - timeout-minutes: ${{ (steps.setup-step.outputs.duration < 18) && 1 || 2 }} - run: | - ${{ env.RUN }} "source selfdrive/test/setup_xvfb.sh && \ - source selfdrive/test/setup_vsound.sh && \ - CI=1 pytest -s tools/sim/tests/test_metadrive_bridge.py" - - create_raylib_ui_report: - name: Create raylib UI Report - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - - name: Build openpilot - run: ${{ env.RUN }} "scons -j$(nproc)" - - name: Create raylib UI Report - run: > - ${{ env.RUN }} "PYTHONWARNINGS=ignore && - source selfdrive/test/setup_xvfb.sh && - python3 selfdrive/ui/tests/test_ui/raylib_screenshots.py" - - name: Upload Raylib UI Report - uses: actions/upload-artifact@v6 - with: - name: raylib-report-${{ inputs.run_number || '1' }}-${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && 'master' || github.event.number }} - path: selfdrive/ui/tests/test_ui/raylib_report/screenshots - - create_mici_raylib_ui_report: - name: Create mici raylib UI Report - runs-on: ${{ - (github.repository == 'commaai/openpilot') && - ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) - && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') - || fromJSON('["ubuntu-24.04"]') }} - steps: - - uses: actions/checkout@v6 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - - name: Build openpilot - run: ${{ env.RUN }} "scons -j$(nproc)" - - name: Create mici raylib UI Report - run: > - ${{ env.RUN }} "PYTHONWARNINGS=ignore && - source selfdrive/test/setup_xvfb.sh && - WINDOWED=1 python3 selfdrive/ui/tests/diff/replay.py" - - name: Upload Raylib UI Report - uses: actions/upload-artifact@v6 - with: - name: mici-raylib-report-${{ inputs.run_number || '1' }}-${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && 'master' || github.event.number }} - path: selfdrive/ui/tests/diff/report diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml new file mode 100644 index 00000000000000..173e2083847264 --- /dev/null +++ b/.github/workflows/tools_tests.yaml @@ -0,0 +1,67 @@ +name: tools +on: + push: + pull_request: + +env: + BASE_IMAGE: openpilot-base + CL_BASE_IMAGE: openpilot-base-cl + DOCKER_REGISTRY: ghcr.io/commaai + DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }} + + BUILD: | + docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base) || true + docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker build --cache-from $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $DOCKER_REGISTRY/$BASE_IMAGE:latest -t $BASE_IMAGE:latest -f Dockerfile.openpilot_base . + BUILD_CL: | + docker pull $(grep -iohP '(?<=^from)\s+\S+' Dockerfile.openpilot_base_cl) || true + docker pull $DOCKER_REGISTRY/$BASE_IMAGE:latest || true + docker build --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl . + RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e \ + GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/comma_download_cache:/tmp/comma_download_cache $BASE_IMAGE /bin/sh -c + +jobs: + plotjuggler: + name: plotjuggler + runs-on: ubuntu-20.04 + timeout-minutes: 30 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Build Docker image + run: eval "$BUILD" + - name: Unit test + run: | + ${{ env.RUN }} "scons -j$(nproc) --directory=/tmp/openpilot/cereal && \ + apt-get update && \ + apt-get install -y libdw-dev libqt5svg5-dev libqt5x11extras5-dev && \ + cd /tmp/openpilot/tools/plotjuggler && \ + ./test_plotjuggler.py" + + simulator: + name: simulator + runs-on: ubuntu-20.04 + timeout-minutes: 50 + env: + IMAGE_NAME: openpilot-sim + if: github.repository == 'commaai/openpilot' + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Pull LFS + run: git lfs pull + - name: Build base image + run: eval "$BUILD" + - name: Build base cl image + run: eval "$BUILD_CL" + - name: Pull latest simulator image + run: docker pull $DOCKER_REGISTRY/$IMAGE_NAME:latest || true + - name: Build simulator image + run: docker build --cache-from $DOCKER_REGISTRY/$IMAGE_NAME:latest -t $DOCKER_REGISTRY/$IMAGE_NAME:latest -f tools/sim/Dockerfile.sim . + - name: Push to container registry + if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' + run: | + $DOCKER_LOGIN + docker push $DOCKER_REGISTRY/$IMAGE_NAME:latest diff --git a/.gitignore b/.gitignore index e2a30fb70aba44..062358ef2478b5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,4 @@ venv/ -.venv/ -.ci_cache .env .clang-format .DS_Store @@ -10,13 +8,11 @@ venv/ .overlay_init .overlay_consistent .sconsign.dblite +.vscode* +model2.png a.out .hypothesis -.cache/ -/docs_site/ - -*.mp4 *.dylib *.DSYM *.d @@ -36,22 +32,37 @@ a.out *.class *.pyxbldc *.vcd -*.mo -*_pyx.cpp -*.stats +*.qm config.json clcache compile_commands.json compare_runtime*.html -selfdrive/pandad/pandad -cereal/services.h -cereal/gen -cereal/messaging/bridge -selfdrive/ui/translations/tmp -selfdrive/car/tests/cars_dump +persist +board/obj/ +selfdrive/boardd/boardd +selfdrive/logcatd/logcatd +selfdrive/mapd/default_speeds_by_region.json +system/proclogd/proclogd +selfdrive/ui/_ui +selfdrive/test/longitudinal_maneuvers/out +selfdrive/visiond/visiond +selfdrive/sensord/_gpsd +selfdrive/sensord/_sensord system/camerad/camerad system/camerad/test/ae_gray_test +selfdrive/modeld/_modeld +selfdrive/modeld/_dmonitoringmodeld +/src/ + +one +openpilot +notebooks +xx +yy +hyperthneed +panda_jungle +provisioning .coverage* coverage.xml @@ -64,38 +75,11 @@ flycheck_* cppcheck_report.txt comma*.sh -selfdrive/modeld/models/*.pkl +selfdrive/modeld/thneed/compile +selfdrive/modeld/models/*.thneed -# openpilot log files *.bz2 -*.zst build/ !**/.gitkeep - -poetry.toml -Pipfile - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json -!.vscode/*.code-snippets - -# Local History for Visual Studio Code -.history/ - -# Built Visual Studio Code Extensions -*.vsix - -### VisualStudioCode Patch ### -# Ignore all local history of files -.history -.ionide - -.claude/ -PLAN.md -TASK.md diff --git a/.gitmodules b/.gitmodules index ad6530de9ac910..26f93ef164e782 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,17 +2,20 @@ path = panda url = ../../commaai/panda.git [submodule "opendbc"] - path = opendbc_repo + path = opendbc url = ../../commaai/opendbc.git -[submodule "msgq"] - path = msgq_repo - url = ../../commaai/msgq.git +[submodule "laika_repo"] + path = laika_repo + url = ../../commaai/laika.git +[submodule "cereal"] + path = cereal + url = ../../commaai/cereal.git [submodule "rednose_repo"] path = rednose_repo url = ../../commaai/rednose.git -[submodule "teleoprtc_repo"] - path = teleoprtc_repo - url = ../../commaai/teleoprtc +[submodule "body"] + path = body + url = ../../commaai/body.git [submodule "tinygrad"] path = tinygrad_repo - url = https://github.com/tinygrad/tinygrad.git + url = https://github.com/geohot/tinygrad.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000000000..347216f2fba3c2 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,76 @@ +repos: +- repo: meta + hooks: + - id: check-hooks-apply + - id: check-useless-excludes +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: check-ast + exclude: '^(pyextra)/' + - id: check-json + - id: check-xml + - id: check-yaml + - id: check-merge-conflict + - id: check-symlinks + - id: check-added-large-files + args: ['--maxkb=100'] +- repo: https://github.com/codespell-project/codespell + rev: v2.2.1 + hooks: + - id: codespell + exclude: '^(pyextra/)|(third_party/)|(body/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(include/)|(selfdrive/ui/translations/.*.ts)|(selfdrive/controls/lib/cluster)' + args: + # if you've got a short variable name that's getting flagged, add it here + - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup + - --builtins clear,rare,informal,usage,code,names,en-GB_to_en-US +- repo: https://github.com/pre-commit/mirrors-mypy + rev: v0.931 + hooks: + - id: mypy + exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/|(tinygrad/)|(tinygrad_repo/)' + additional_dependencies: ['types-PyYAML', 'lxml', 'numpy', 'types-atomicwrites', 'types-pycurl', 'types-requests', 'types-certifi'] + args: + - --warn-redundant-casts + - --warn-return-any + - --warn-unreachable + - --warn-unused-ignores + #- --html-report=/home/batman/openpilot +- repo: https://github.com/PyCQA/flake8 + rev: 4.0.1 + hooks: + - id: flake8 + exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(selfdrive/debug/)/' + additional_dependencies: ['flake8-no-implicit-concat'] + args: + - --indent-size=2 + - --enable-extensions=NIC + - --select=F,E112,E113,E304,E502,E701,E702,E703,E71,E72,E731,W191,W6 + - --statistics + - -j4 +- repo: local + hooks: + - id: pylint + name: pylint + entry: pylint + language: system + types: [python] + exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(laika_repo/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)' + args: + - -rn + - -sn + - --rcfile=.pylintrc +- repo: local + hooks: + - id: cppcheck + name: cppcheck + entry: cppcheck + language: system + types: [c++] + exclude: '^(third_party/)|(pyextra/)|(cereal/)|(body/)|(rednose/)|(rednose_repo/)|(opendbc/)|(panda/)|(tools/)|(selfdrive/modeld/thneed/debug/)|(selfdrive/modeld/test/)|(selfdrive/camerad/test/)|(installer/)' + args: + - --error-exitcode=1 + - --language=c++ + - --quiet + - --force + - -j8 diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 00000000000000..58988c5d74f8a1 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,469 @@ +[MASTER] + +# A comma-separated list of package or module names from where C extensions may +# be loaded. Extensions are loading into the active Python interpreter and may +# run arbitrary code +extension-pkg-whitelist=scipy,cereal.messaging.messaging_pyx,PyQt5,av + +# Add files or directories to the blacklist. They should be base names, not +# paths. +ignore=CVS + +# Add files or directories matching the regex patterns to the blacklist. The +# regex matches against base names, not paths. +ignore-patterns= + +# Python code to execute, usually for sys.path manipulation such as +# pygtk.require(). +#init-hook= + +# Use multiple processes to speed up Pylint. +jobs=4 + +# List of plugins (as comma separated values of python modules names) to load, +# usually to register additional checkers. +load-plugins= + +# Pickle collected data for later comparisons. +persistent=yes + +# Specify a configuration file. +#rcfile= + +# When enabled, pylint would attempt to guess common misconfiguration and emit +# user-friendly hints instead of false-positive error messages +suggestion-mode=yes + +# Allow loading of arbitrary C extensions. Extensions are imported into the +# active Python interpreter and may run arbitrary code. +unsafe-load-any-extension=no + + +[MESSAGES CONTROL] + +# Only show warnings with the listed confidence levels. Leave empty to show +# all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED +confidence= + +# Disable the message, report, category or checker with the given id(s). You +# can either give multiple identifiers separated by comma (,) or put this +# option multiple times (only on the command line, not in the configuration +# file where it should appear only once).You can also use "--disable=all" to +# disable everything first and then reenable specific checks. For example, if +# you want to run only the similarities checker, you can use "--disable=all +# --enable=similarities". If you want to run only the classes checker, but have +# no Warning level messages displayed, use"--disable=all --enable=classes +# --disable=W" +disable=C,R,W0613,W0511,W0212,W0201,W0106,W0603,W0621,W0703,W1201,W1203,E1136,W1514 + + +# Enable the message, report, category or checker with the given id(s). You can +# either give multiple identifier separated by comma (,) or put this option +# multiple time (only on the command line, not in the configuration file where +# it should appear only once). See also the "--disable" option for examples. +enable=c-extension-no-member + + +[REPORTS] + +# Python expression which should return a note less than 10 (10 is the highest +# note). You have access to the variables errors warning, statement which +# respectively contain the number of errors / warnings messages and the total +# number of statements analyzed. This is used by the global evaluation report +# (RP0004). +evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) + +# Template used to display messages. This is a python new-style format string +# used to format the message information. See doc for all details +#msg-template= + +# Set the output format. Available formats are text, parseable, colorized, json +# and msvs (visual studio).You can also give a reporter class, eg +# mypackage.mymodule.MyReporterClass. +output-format=text + +# Tells whether to display a full report or only the messages +reports=no + +# Activate the evaluation score. +score=yes + + +[REFACTORING] + +# Maximum number of nested blocks for function / method body +max-nested-blocks=5 + +# Complete name of functions that never returns. When checking for +# inconsistent-return-statements if a never returning function is called then +# it will be considered as an explicit return statement and no message will be +# printed. +never-returning-functions=optparse.Values,sys.exit + + +[LOGGING] + +# Logging modules to check that the string format arguments are in logging +# function parameter format +logging-modules=logging + + +[SPELLING] + +# Limits count of emitted suggestions for spelling mistakes +max-spelling-suggestions=4 + +# Spelling dictionary name. Available dictionaries: none. To make it working +# install python-enchant package. +spelling-dict= + +# List of comma separated words that should not be checked. +spelling-ignore-words= + +# A path to a file that contains private dictionary; one word per line. +spelling-private-dict-file= + +# Tells whether to store unknown words to indicated private dictionary in +# --spelling-private-dict-file option instead of raising a message. +spelling-store-unknown-words=no + + +[MISCELLANEOUS] + +# List of note tags to take in consideration, separated by a comma. +notes=FIXME, + XXX, + TODO + + +[SIMILARITIES] + +# Ignore comments when computing similarities. +ignore-comments=yes + +# Ignore docstrings when computing similarities. +ignore-docstrings=yes + +# Ignore imports when computing similarities. +ignore-imports=no + +# Minimum lines number of a similarity. +min-similarity-lines=4 + + +[TYPECHECK] + +# List of decorators that produce context managers, such as +# contextlib.contextmanager. Add to this list to register other decorators that +# produce valid context managers. +contextmanager-decorators=contextlib.contextmanager + +# List of members which are set dynamically and missed by pylint inference +# system, and so shouldn't trigger E1101 when accessed. Python regular +# expressions are accepted. +generated-members=capnp.* cereal.* pygame.* zmq.* setproctitle.* smbus2.* usb1.* serial.* cv2.* ft4222.* carla.* + +# Tells whether missing members accessed in mixin class should be ignored. A +# mixin class is detected if its name ends with "mixin" (case insensitive). +ignore-mixin-members=yes + +# This flag controls whether pylint should warn about no-member and similar +# checks whenever an opaque object is returned when inferring. The inference +# can return multiple potential results while evaluating a Python object, but +# some branches might not be evaluated, which results in partial inference. In +# that case, it might be useful to still emit no-member and other checks for +# the rest of the inferred objects. +ignore-on-opaque-inference=yes + +# List of class names for which member attributes should not be checked (useful +# for classes with dynamically set attributes). This supports the use of +# qualified names. +ignored-classes=optparse.Values,thread._local,_thread._local + +# List of module names for which member attributes should not be checked +# (useful for modules/projects where namespaces are manipulated during runtime +# and thus existing member attributes cannot be deduced by static analysis. It +# supports qualified module names, as well as Unix pattern matching. +ignored-modules=flask setproctitle usb1 flask.ext.socketio smbus2 usb1.* + +# Show a hint with possible names when a member name was not found. The aspect +# of finding the hint is based on edit distance. +missing-member-hint=yes + +# The minimum edit distance a name should have in order to be considered a +# similar match for a missing member name. +missing-member-hint-distance=1 + +# The total number of similar names that should be taken in consideration when +# showing a hint for a missing member. +missing-member-max-choices=1 + + +[VARIABLES] + +# List of additional names supposed to be defined in builtins. Remember that +# you should avoid to define new builtins when possible. +additional-builtins= + +# Tells whether unused global variables should be treated as a violation. +allow-global-unused-variables=yes + +# List of strings which can identify a callback function by name. A callback +# name must start or end with one of those strings. +callbacks=cb_, + _cb + +# A regular expression matching the name of dummy variables (i.e. expectedly +# not used). +dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ + +# Argument names that match this expression will be ignored. Default to name +# with leading underscore +ignored-argument-names=_.*|^ignored_|^unused_ + +# Tells whether we should check for unused import in __init__ files. +init-import=no + +# List of qualified module names which can have objects that can redefine +# builtins. +redefining-builtins-modules=six.moves,past.builtins,future.builtins + + +[FORMAT] + +# Expected format of line ending, e.g. empty (any line ending), LF or CRLF. +expected-line-ending-format= + +# Regexp for a line that is allowed to be longer than the limit. +ignore-long-lines=^\s*(# )??$ + +# Number of spaces of indent required inside a hanging or continued line. +indent-after-paren=4 + +# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 +# tab). +indent-string=' ' + +# Maximum number of characters on a single line. +max-line-length=100 + +# Maximum number of lines in a module +max-module-lines=1000 + +# Allow the body of a class to be on the same line as the declaration if body +# contains single statement. +single-line-class-stmt=no + +# Allow the body of an if to be on the same line as the test if there is no +# else. +single-line-if-stmt=no + + +[BASIC] + +# Naming style matching correct argument names +argument-naming-style=snake_case + +# Regular expression matching correct argument names. Overrides argument- +# naming-style +#argument-rgx= + +# Naming style matching correct attribute names +attr-naming-style=snake_case + +# Regular expression matching correct attribute names. Overrides attr-naming- +# style +#attr-rgx= + +# Bad variable names which should always be refused, separated by a comma +bad-names=foo, + bar, + baz, + toto, + tutu, + tata + +# Naming style matching correct class attribute names +class-attribute-naming-style=any + +# Regular expression matching correct class attribute names. Overrides class- +# attribute-naming-style +#class-attribute-rgx= + +# Naming style matching correct class names +class-naming-style=PascalCase + +# Regular expression matching correct class names. Overrides class-naming-style +#class-rgx= + +# Naming style matching correct constant names +const-naming-style=UPPER_CASE + +# Regular expression matching correct constant names. Overrides const-naming- +# style +#const-rgx= + +# Minimum line length for functions/classes that require docstrings, shorter +# ones are exempt. +docstring-min-length=-1 + +# Naming style matching correct function names +function-naming-style=snake_case + +# Regular expression matching correct function names. Overrides function- +# naming-style +#function-rgx= + +# Good variable names which should always be accepted, separated by a comma +good-names=i, + j, + k, + ex, + Run, + _ + +# Include a hint for the correct naming format with invalid-name +include-naming-hint=no + +# Naming style matching correct inline iteration names +inlinevar-naming-style=any + +# Regular expression matching correct inline iteration names. Overrides +# inlinevar-naming-style +#inlinevar-rgx= + +# Naming style matching correct method names +method-naming-style=snake_case + +# Regular expression matching correct method names. Overrides method-naming- +# style +#method-rgx= + +# Naming style matching correct module names +module-naming-style=snake_case + +# Regular expression matching correct module names. Overrides module-naming- +# style +#module-rgx= + +# Colon-delimited sets of names that determine each other's naming style when +# the name regexes allow several styles. +name-group= + +# Regular expression which should only match function or class names that do +# not require a docstring. +no-docstring-rgx=^_ + +# List of decorators that produce properties, such as abc.abstractproperty. Add +# to this list to register other decorators that produce valid properties. +property-classes=abc.abstractproperty + +# Naming style matching correct variable names +variable-naming-style=snake_case + +# Regular expression matching correct variable names. Overrides variable- +# naming-style +#variable-rgx= + + +[DESIGN] + +# Maximum number of arguments for function / method +max-args=5 + +# Maximum number of attributes for a class (see R0902). +max-attributes=7 + +# Maximum number of boolean expressions in a if statement +max-bool-expr=5 + +# Maximum number of branch for function / method body +max-branches=12 + +# Maximum number of locals for function / method body +max-locals=15 + +# Maximum number of parents for a class (see R0901). +max-parents=7 + +# Maximum number of public methods for a class (see R0904). +max-public-methods=20 + +# Maximum number of return / yield for function / method body +max-returns=6 + +# Maximum number of statements in function / method body +max-statements=50 + +# Minimum number of public methods for a class (see R0903). +min-public-methods=2 + + +[CLASSES] + +# List of method names used to declare (i.e. assign) instance attributes. +defining-attr-methods=__init__, + __new__, + setUp + +# List of member names, which should be excluded from the protected access +# warning. +exclude-protected=_asdict, + _fields, + _replace, + _source, + _make + +# List of valid names for the first argument in a class method. +valid-classmethod-first-arg=cls + +# List of valid names for the first argument in a metaclass class method. +valid-metaclass-classmethod-first-arg=mcs + + +[IMPORTS] + +# Allow wildcard imports from modules that define __all__. +allow-wildcard-with-all=no + +# Analyse import fallback blocks. This can be used to support both Python 2 and +# 3 compatible code, which means that the block might have code that exists +# only in one or another interpreter, leading to false positives when analysed. +analyse-fallback-blocks=no + +# Deprecated modules which should not be used, separated by a comma +deprecated-modules=regsub, + TERMIOS, + Bastion, + rexec + +# Create a graph of external dependencies in the given file (report RP0402 must +# not be disabled) +ext-import-graph= + +# Create a graph of every (i.e. internal and external) dependencies in the +# given file (report RP0402 must not be disabled) +import-graph= + +# Create a graph of internal dependencies in the given file (report RP0402 must +# not be disabled) +int-import-graph= + +# Force import order to recognize a module as part of the standard +# compatibility libraries. +known-standard-library= + +# Force import order to recognize a module as part of a third party library. +known-third-party=enchant + +[STRING] + +# This flag controls whether the implicit-str-concat should generate a warning +# on implicit string concatenation in sequences defined over several lines. +check-str-concat-over-line-jumps=yes + +[EXCEPTIONS] + +# Exceptions that will emit a warning when being caught. Defaults to +# "Exception" +overgeneral-exceptions=Exception diff --git a/.python-version b/.python-version new file mode 100644 index 00000000000000..d20cc2bf020ea4 --- /dev/null +++ b/.python-version @@ -0,0 +1 @@ +3.8.10 diff --git a/.vscode/extensions.json b/.vscode/extensions.json deleted file mode 100644 index 458312fc88af81..00000000000000 --- a/.vscode/extensions.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "recommendations": [ - "ms-python.python", - "ms-vscode.cpptools", - "elagil.pre-commit-helper", - "charliermarsh.ruff", - ] -} diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index f090061c42b0a2..00000000000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "version": "0.2.0", - "inputs": [ - { - "id": "python_process", - "type": "pickString", - "description": "Select the process to debug", - "options": [ - "selfdrive/controls/controlsd.py", - "system/timed/timed.py", - "tools/sim/run_bridge.py" - ] - }, - { - "id": "cpp_process", - "type": "pickString", - "description": "Select the process to debug", - "options": [ - "selfdrive/ui/ui" - ] - }, - { - "id": "args", - "description": "Arguments to pass to the process", - "type": "promptString" - }, - { - "id": "replayArg", - "type": "promptString", - "description": "Enter route or segment to replay." - } - ], - "configurations": [ - { - "name": "Python: openpilot Process", - "type": "debugpy", - "request": "launch", - "program": "${input:python_process}", - "console": "integratedTerminal", - "justMyCode": true, - "args": "${input:args}" - }, - { - "name": "C++: openpilot Process", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/${input:cpp_process}", - "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/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index f0731c362d9a5c..00000000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "editor.tabSize": 2, - "editor.insertSpaces": true, - "editor.renderWhitespace": "trailing", - "files.trimTrailingWhitespace": true, - "search.exclude": { - "**/.git": true, - "**/.venv": true, - "**/__pycache__": true - }, - "files.exclude": { - "**/.git": true, - "**/.venv": true, - "**/__pycache__": true - }, - "python.analysis.exclude": [ - "**/.git", - "**/.venv", - "**/__pycache__", - // exclude directories that should be using the symlinked version - "common/**", - "selfdrive/**", - "system/**", - "third_party/**", - "tools/**", - ] -} diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 106a06e3a2045e..102da78d7d01ee 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -1,14 +1,28 @@ FROM ghcr.io/commaai/openpilot-base:latest -ENV PYTHONUNBUFFERED=1 +ENV PYTHONUNBUFFERED 1 -ENV OPENPILOT_PATH=/home/batman/openpilot +ENV OPENPILOT_PATH /home/batman/openpilot/ +ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} RUN mkdir -p ${OPENPILOT_PATH} WORKDIR ${OPENPILOT_PATH} -COPY . ${OPENPILOT_PATH}/ +COPY SConstruct ${OPENPILOT_PATH} -ENV UV_BIN="/home/batman/.local/bin/" -ENV PATH="$UV_BIN:$PATH" -RUN UV_PROJECT_ENVIRONMENT=$VIRTUAL_ENV uv run scons --cache-readonly -j$(nproc) +COPY ./pyextra ${OPENPILOT_PATH}/pyextra +COPY ./third_party ${OPENPILOT_PATH}/third_party +COPY ./site_scons ${OPENPILOT_PATH}/site_scons +COPY ./laika ${OPENPILOT_PATH}/laika +COPY ./laika_repo ${OPENPILOT_PATH}/laika_repo +COPY ./rednose ${OPENPILOT_PATH}/rednose +COPY ./tools ${OPENPILOT_PATH}/tools +COPY ./release ${OPENPILOT_PATH}/release +COPY ./common ${OPENPILOT_PATH}/common +COPY ./opendbc ${OPENPILOT_PATH}/opendbc +COPY ./cereal ${OPENPILOT_PATH}/cereal +COPY ./panda ${OPENPILOT_PATH}/panda +COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive +COPY ./system ${OPENPILOT_PATH}/system + +RUN scons --cache-readonly -j$(nproc) diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index 44d8d95e95d926..0de3008baf9faa 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -1,81 +1,33 @@ -FROM ubuntu:24.04 +FROM ubuntu:20.04 -ENV PYTHONUNBUFFERED=1 +ENV PYTHONUNBUFFERED 1 ENV DEBIAN_FRONTEND=noninteractive RUN apt-get update && \ - apt-get install -y --no-install-recommends sudo tzdata locales ssh pulseaudio xvfb x11-xserver-utils gnome-screenshot python3-tk python3-dev && \ + apt-get install -y --no-install-recommends sudo tzdata locales ssh && \ rm -rf /var/lib/apt/lists/* RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen -ENV LANG=en_US.UTF-8 -ENV LANGUAGE=en_US:en -ENV LC_ALL=en_US.UTF-8 +ENV LANG en_US.UTF-8 +ENV LANGUAGE en_US:en +ENV LC_ALL en_US.UTF-8 + +ENV PIPENV_SYSTEM=1 +ENV PYENV_VERSION=3.8.10 +ENV PYENV_ROOT="/root/.pyenv" +ENV PATH="$PYENV_ROOT/bin:$PYENV_ROOT/shims:$PATH" + +COPY Pipfile Pipfile.lock .python-version update_requirements.sh /tmp/ +COPY tools/ubuntu_setup.sh /tmp/tools/ +RUN cd /tmp && \ + tools/ubuntu_setup.sh && \ + rm -rf /var/lib/apt/lists/* && \ + rm -rf /tmp/* && \ + rm -rf /root/.cache && \ + pip uninstall -y pipenv && \ + # remove unused architectures from gcc for panda + cd /usr/lib/gcc/arm-none-eabi/9.2.1 && \ + rm -rf arm/ && \ + rm -rf thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp -COPY tools/install_ubuntu_dependencies.sh /tmp/tools/ -RUN /tmp/tools/install_ubuntu_dependencies.sh && \ - rm -rf /var/lib/apt/lists/* /tmp/* && \ - cd /usr/lib/gcc/arm-none-eabi/* && \ - rm -rf arm/ thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp - -# Add OpenCL -RUN apt-get update && apt-get install -y --no-install-recommends \ - apt-utils \ - alien \ - unzip \ - tar \ - curl \ - xz-utils \ - dbus \ - gcc-arm-none-eabi \ - tmux \ - vim \ - libx11-6 \ - wget \ - && rm -rf /var/lib/apt/lists/* - -RUN mkdir -p /tmp/opencl-driver-intel && \ - cd /tmp/opencl-driver-intel && \ - wget https://github.com/intel/llvm/releases/download/2024-WW14/oclcpuexp-2024.17.3.0.09_rel.tar.gz && \ - wget https://github.com/oneapi-src/oneTBB/releases/download/v2021.12.0/oneapi-tbb-2021.12.0-lin.tgz && \ - mkdir -p /opt/intel/oclcpuexp_2024.17.3.0.09_rel && \ - cd /opt/intel/oclcpuexp_2024.17.3.0.09_rel && \ - tar -zxvf /tmp/opencl-driver-intel/oclcpuexp-2024.17.3.0.09_rel.tar.gz && \ - mkdir -p /etc/OpenCL/vendors && \ - echo /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64/libintelocl.so > /etc/OpenCL/vendors/intel_expcpu.icd && \ - cd /opt/intel && \ - tar -zxvf /tmp/opencl-driver-intel/oneapi-tbb-2021.12.0-lin.tgz && \ - ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbb.so /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \ - ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbbmalloc.so /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \ - ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbb.so.12 /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \ - ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbbmalloc.so.2 /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \ - mkdir -p /etc/ld.so.conf.d && \ - echo /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 > /etc/ld.so.conf.d/libintelopenclexp.conf && \ - ldconfig -f /etc/ld.so.conf.d/libintelopenclexp.conf && \ - cd / && \ - rm -rf /tmp/opencl-driver-intel - -ENV NVIDIA_VISIBLE_DEVICES=all -ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute -ENV QTWEBENGINE_DISABLE_SANDBOX=1 - -RUN dbus-uuidgen > /etc/machine-id - -ARG USER=batman -ARG USER_UID=1001 -RUN useradd -m -s /bin/bash -u $USER_UID $USER -RUN usermod -aG sudo $USER -RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers -USER $USER - -COPY --chown=$USER pyproject.toml uv.lock /home/$USER -COPY --chown=$USER tools/install_python_dependencies.sh /home/$USER/tools/ - -ENV VIRTUAL_ENV=/home/$USER/.venv -ENV PATH="$VIRTUAL_ENV/bin:$PATH" -RUN cd /home/$USER && \ - tools/install_python_dependencies.sh && \ - rm -rf tools/ pyproject.toml uv.lock .cache - -USER root RUN sudo git config --global --add safe.directory /tmp/openpilot diff --git a/Dockerfile.openpilot_base_cl b/Dockerfile.openpilot_base_cl new file mode 100644 index 00000000000000..7652b7e4e6b124 --- /dev/null +++ b/Dockerfile.openpilot_base_cl @@ -0,0 +1,37 @@ +FROM ghcr.io/commaai/openpilot-base:latest + +RUN apt-get update && apt-get install -y --no-install-recommends\ + apt-utils \ + alien \ + unzip \ + tar \ + curl \ + xz-utils \ + dbus \ + gcc-arm-none-eabi \ + tmux \ + vim \ + lsb-core \ + libx11-6 \ + && rm -rf /var/lib/apt/lists/* + +# Intel OpenCL driver +ARG INTEL_DRIVER=l_opencl_p_18.1.0.015.tgz +ARG INTEL_DRIVER_URL=http://registrationcenter-download.intel.com/akdlm/irc_nas/vcp/15532 +RUN mkdir -p /tmp/opencl-driver-intel +WORKDIR /tmp/opencl-driver-intel +RUN echo INTEL_DRIVER is $INTEL_DRIVER && \ + curl -O $INTEL_DRIVER_URL/$INTEL_DRIVER && \ + tar -xzf $INTEL_DRIVER && \ + for i in $(basename $INTEL_DRIVER .tgz)/rpm/*.rpm; do alien --to-deb $i; done && \ + dpkg -i *.deb && \ + rm -rf $INTEL_DRIVER $(basename $INTEL_DRIVER .tgz) *.deb && \ + mkdir -p /etc/OpenCL/vendors && \ + echo /opt/intel/opencl_compilers_and_libraries_18.1.0.015/linux/compiler/lib/intel64_lin/libintelocl.so > /etc/OpenCL/vendors/intel.icd && \ + rm -rf /tmp/opencl-driver-intel +ENV NVIDIA_VISIBLE_DEVICES all +ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute +ENV QTWEBENGINE_DISABLE_SANDBOX 1 + +RUN dbus-uuidgen > /etc/machine-id + diff --git a/Jenkinsfile b/Jenkinsfile index c095eda8a91ccf..c4038090e14be9 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,269 +1,164 @@ -def retryWithDelay(int maxRetries, int delay, Closure body) { - for (int i = 0; i < maxRetries; i++) { - try { - return body() - } catch (Exception e) { - sleep(delay) - } - } - throw Exception("Failed after ${maxRetries} retries") -} - -def device(String ip, String step_label, String cmd) { +def phone(String ip, String step_label, String cmd) { withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) { def ssh_cmd = """ -ssh -o ConnectTimeout=5 -o ServerAliveInterval=5 -o ServerAliveCountMax=2 -o BatchMode=yes -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' exec /usr/bin/bash <<'END' +ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <<'END' set -e -export TERM=xterm-256color - -shopt -s huponexit # kill all child processes when the shell exits - export CI=1 -export PYTHONWARNINGS=error -#export LOGPRINT=debug # this has gotten too spammy... export TEST_DIR=${env.TEST_DIR} export SOURCE_DIR=${env.SOURCE_DIR} export GIT_BRANCH=${env.GIT_BRANCH} export GIT_COMMIT=${env.GIT_COMMIT} -export CI_ARTIFACTS_TOKEN=${env.CI_ARTIFACTS_TOKEN} -export GITHUB_COMMENTS_TOKEN=${env.GITHUB_COMMENTS_TOKEN} export AZURE_TOKEN='${env.AZURE_TOKEN}' -# only use 1 thread for tici tests since most require HIL -export PYTEST_ADDOPTS="-n0 -s" - - -export GIT_SSH_COMMAND="ssh -i /data/gitkey" source ~/.bash_profile if [ -f /TICI ]; then source /etc/profile - - rm -rf /tmp/tmp* - rm -rf ~/.commacache - rm -rf /dev/shm/* - rm -rf /dev/tmp/tmp* - - if ! systemctl is-active --quiet systemd-resolved; then - echo "restarting resolved" - sudo systemctl start systemd-resolved - sleep 3 - fi - - # restart aux USB - if [ -e /sys/bus/usb/drivers/hub/3-0:1.0 ]; then - echo "restarting aux usb" - echo "3-0:1.0" | sudo tee /sys/bus/usb/drivers/hub/unbind - sleep 0.5 - echo "3-0:1.0" | sudo tee /sys/bus/usb/drivers/hub/bind - fi -fi -if [ -f /data/openpilot/launch_env.sh ]; then - source /data/openpilot/launch_env.sh fi ln -snf ${env.TEST_DIR} /data/pythonpath cd ${env.TEST_DIR} || true -time ${cmd} +${cmd} +exit 0 + END""" sh script: ssh_cmd, label: step_label } } -def deviceStage(String stageName, String deviceType, List extra_env, def steps) { - stage(stageName) { - if (currentBuild.result != null) { - return - } - - if (isReplay()) { - error("REPLAYING TESTS IS NOT ALLOWED. FIX THEM INSTEAD.") - } - - def extra = extra_env.collect { "export ${it}" }.join('\n'); - def branch = env.BRANCH_NAME ?: 'master'; - def gitDiff = sh returnStdout: true, script: 'curl -s -H "Authorization: Bearer ${GITHUB_COMMENTS_TOKEN}" https://api.github.com/repos/commaai/openpilot/compare/master...${GIT_BRANCH} | jq .files[].filename || echo "/"', label: 'Getting changes' - - lock(resource: "", label: deviceType, inversePrecedence: true, variable: 'device_ip', quantity: 1, resourceSelectStrategy: 'random') { - docker.image('ghcr.io/commaai/alpine-ssh').inside('--user=root') { - timeout(time: 35, unit: 'MINUTES') { - retry (3) { - def date = sh(script: 'date', returnStdout: true).trim(); - device(device_ip, "set time", "date -s '" + date + "'") - device(device_ip, "git checkout", extra + "\n" + readFile("selfdrive/test/setup_device_ci.sh")) - } - steps.each { item -> - def name = item[0] - def cmd = item[1] - - def args = item[2] - def diffPaths = args.diffPaths ?: [] - def cmdTimeout = args.timeout ?: 9999 - - if (branch != "master" && !branch.contains("__jenkins_loop_") && diffPaths && !hasPathChanged(gitDiff, diffPaths)) { - println "Skipping ${name}: no changes in ${diffPaths}." - return - } else { - timeout(time: cmdTimeout, unit: 'SECONDS') { - device(device_ip, name, cmd) - } - } - } - } +def phone_steps(String device_type, steps) { + lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) { + timeout(time: 20, unit: 'MINUTES') { + phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),) + steps.each { item -> + phone(device_ip, item[0], item[1]) } } } } -def hasPathChanged(String gitDiff, List paths) { - for (path in paths) { - if (gitDiff.contains(path)) { - return true - } +pipeline { + agent none + environment { + CI = "1" + TEST_DIR = "/data/openpilot" + SOURCE_DIR = "/data/openpilot_source/" + AZURE_TOKEN = credentials('azure_token') } - return false -} - -def isReplay() { - def replayClass = "org.jenkinsci.plugins.workflow.cps.replay.ReplayCause" - return currentBuild.rawBuild.getCauses().any{ cause -> cause.toString().contains(replayClass) } -} - -def setupCredentials() { - withCredentials([ - string(credentialsId: 'azure_token', variable: 'AZURE_TOKEN'), - ]) { - env.AZURE_TOKEN = "${AZURE_TOKEN}" - } - - withCredentials([ - string(credentialsId: 'ci_artifacts_pat', variable: 'CI_ARTIFACTS_TOKEN'), - ]) { - env.CI_ARTIFACTS_TOKEN = "${CI_ARTIFACTS_TOKEN}" + options { + timeout(time: 4, unit: 'HOURS') } - withCredentials([ - string(credentialsId: 'post_comments_github_pat', variable: 'GITHUB_COMMENTS_TOKEN'), - ]) { - env.GITHUB_COMMENTS_TOKEN = "${GITHUB_COMMENTS_TOKEN}" - } -} + stages { + stage('build release3') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + when { + branch 'devel-staging' + } + steps { + phone_steps("tici", [ + ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"], + ]) + } + } -def step(String name, String cmd, Map args = [:]) { - return [name, cmd, args] -} + stage('openpilot tests') { + when { + not { + anyOf { + branch 'master-ci'; branch 'devel'; branch 'devel-staging'; + branch 'release3'; branch 'release3-staging'; branch 'dashcam3'; branch 'dashcam3-staging'; + branch 'testing-closet*'; branch 'hotfix-*' + } + } + } -node { - env.CI = "1" - env.PYTHONWARNINGS = "error" - env.TEST_DIR = "/data/openpilot" - env.SOURCE_DIR = "/data/openpilot_source/" - setupCredentials() + parallel { - env.GIT_BRANCH = checkout(scm).GIT_BRANCH - env.GIT_COMMIT = checkout(scm).GIT_COMMIT + stage('simulator') { + agent { + dockerfile { + filename 'Dockerfile.sim_nvidia' + dir 'tools/sim' + args '--user=root' + } + } + steps { + sh "git config --global --add safe.directory ${WORKSPACE}" + sh "git lfs pull" + lock(resource: "", label: "simulator", inversePrecedence: true, quantity: 1) { + sh "${WORKSPACE}/tools/sim/build_container.sh" + sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" + sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + } + } - def excludeBranches = ['__nightly', 'devel', 'devel-staging', 'release3', 'release3-staging', - 'release-tici', 'release-tizi', 'release-tizi-staging', 'testing-closet*', 'hotfix-*'] - def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*') + post { + always { + sh "docker kill carla_sim || true" + sh "rm -rf ${WORKSPACE}/* || true" + sh "rm -rf .* || true" + } + } + } - if (env.BRANCH_NAME != 'master' && !env.BRANCH_NAME.contains('__jenkins_loop_')) { - properties([ - disableConcurrentBuilds(abortPrevious: true) - ]) - } + stage('build') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + environment { + R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" + } + steps { + phone_steps("tici", [ + ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"], + ["build openpilot", "cd selfdrive/manager && ./build.py"], + ["check dirty", "release/check-dirty.sh"], + ["test manager", "python selfdrive/manager/test/test_manager.py"], + ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], + ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], + ]) + } + } - try { - if (env.BRANCH_NAME == 'devel-staging') { - deviceStage("build release-tizi-staging", "tizi-needs-can", [], [ - step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging $SOURCE_DIR/release/build_release.sh"), - ]) - } + stage('HW + Unit Tests') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici2", [ + ["build", "cd selfdrive/manager && ./build.py"], + //["test power draw", "python system/hardware/tici/test_power_draw.py"], + ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], + ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], + ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], + ["test sensord", "python selfdrive/sensord/tests/test_sensord.py"], + ["test pigeond", "python selfdrive/sensord/tests/test_pigeond.py"], + ]) + } + } - if (env.BRANCH_NAME == '__nightly') { - parallel ( - 'nightly': { - deviceStage("build nightly", "tizi-needs-can", [], [ - step("build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"), - ]) - }, - 'nightly-dev': { - 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"), - ]) - }, - ) - } + stage('camerad') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici-party", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["test camerad", "python system/camerad/test/test_camerad.py"], + ["test exposure", "python system/camerad/test/test_exposure.py"], + ]) + } + } - if (!env.BRANCH_NAME.matches(excludeRegex)) { - parallel ( - 'onroad tests': { - 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("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 manager", "pytest system/manager/test/test_manager.py"), - ]) - }, - 'loopback': { - 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 OX03C10': { - 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"), - ]) - }, - 'camerad OS04C10': { - deviceStage("OS04C10", "tici-os04c10", ["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"), - ]) - }, - 'sensord': { - 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", "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"]]), - ]) - }, - 'tizi': { - deviceStage("tizi", "tizi", ["UNSAFE=1"], [ - 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 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"), - step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]), - ]) - }, + stage('replay') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("tici3", [ + ["build", "cd selfdrive/manager && ./build.py"], + ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"], + ]) + } + } + } - ) } - } catch (Exception e) { - currentBuild.result = 'FAILED' - throw e } } diff --git a/Pipfile b/Pipfile new file mode 100644 index 00000000000000..644fd2a90bb2e0 --- /dev/null +++ b/Pipfile @@ -0,0 +1,94 @@ +[[source]] +name = "pypi" +url = "https://pypi.org/simple" +verify_ssl = true + +[dev-packages] +av = "*" +azure-storage-blob = "~=2.1" +control = "*" +coverage = "*" +dictdiffer = "*" +fastcluster = "*" +hexdump = "*" +hypothesis = "==6.46.7" +inputs = "*" +lru-dict = "*" +markdown-it-py = "*" +matplotlib = "*" +mypy = "*" +myst-parser = "*" +natsort = "*" +numpy = "*" +opencv-python-headless = "*" +parameterized = "*" +paramiko = "*" +pprofile = "*" +pre-commit = "*" +pycurl = "*" +pygame = "*" +pyprof2calltree = "*" +pytest = "*" +pytest-xdist = "*" +reverse_geocoder = "*" +scipy = "*" +sphinx = "*" +sphinx-sitemap = "*" +sphinx-rtd-theme = "*" +breathe = "*" +subprocess32 = "*" +tenacity = "*" +mpld3 = "*" +carla = {version = "==0.9.13", markers="platform_system != 'Darwin'"} +ft4222 = "*" +pandas = "*" +tabulate = "*" + +[packages] +atomicwrites = "*" +casadi = {version = "*", markers="platform_system != 'Darwin'"} +cffi = "*" +crcmod = "*" +cryptography = "*" +Cython = "*" +flake8 = "*" +Flask = "*" +future-fstrings = "*" # for acados +gunicorn = "*" +hexdump = "*" +Jinja2 = "*" +json-rpc = "*" +libusb1 = "*" +nose = "*" +numpy = "*" +protobuf = "==3.20.1" +onnx = "*" +onnxruntime-gpu = {version = "*", markers="platform_system != 'Darwin'"} +pillow = "*" +psutil = "*" +pycapnp = "==1.1.0" +pycryptodome = "*" +PyJWT = "*" +pylint = "*" +pyopencl = "*" +pyserial = "*" +python-dateutil = "*" +PyYAML = "*" +pyzmq = "*" +requests = "*" +scons = "*" +sentry-sdk = "*" +setproctitle = "*" +six = "*" +smbus2 = "*" +sympy = "!=1.6.1" +timezonefinder = "*" +tqdm = "*" +urllib3 = "*" +utm = "*" +websocket_client = "*" +hatanaka = "==2.4" +PySide2 = "*" + +[requires] +python_version = "3.8" diff --git a/Pipfile.lock b/Pipfile.lock new file mode 100644 index 00000000000000..e6f05fbcd4f294 --- /dev/null +++ b/Pipfile.lock @@ -0,0 +1,2855 @@ +{ + "_meta": { + "hash": { + "sha256": "adf64558652d394d9de8e45777f1a2f50ed1ac37b75664206e7957792832b5a4" + }, + "pipfile-spec": 6, + "requires": { + "python_version": "3.8" + }, + "sources": [ + { + "name": "pypi", + "url": "https://pypi.org/simple", + "verify_ssl": true + } + ] + }, + "default": { + "astroid": { + "hashes": [ + "sha256:396c88d0a58d7f8daadf730b2ce90838bf338c6752558db719ec6f99c18ec20e", + "sha256:d612609242996c4365aeb0345e61edba34363eaaba55f1c0addf6a98f073bef6" + ], + "markers": "python_full_version >= '3.7.2'", + "version": "==2.12.5" + }, + "atomicwrites": { + "hashes": [ + "sha256:81b2c9071a49367a7f770170e5eec8cb66567cfbbc8c73d20ce5ca4a8d71cf11" + ], + "index": "pypi", + "version": "==1.4.1" + }, + "casadi": { + "hashes": [ + "sha256:09e103bb597d46aa338fc57bc49270068a1f07be35f9494c9f796dea4b801aeb", + "sha256:13277151efc76b221de8ca6b5ab7b8bbdd2b0e139f282866840adf88dfe53bc9", + "sha256:1c451a07b2440c00d552e040b6285b6e79b677d2978212368b28b86f5d267669", + "sha256:2322748a8d5e88750fd2fc0abcdc56cfbad1a8cd538fe0e7d7b6d8ce0cb3fa62", + "sha256:24fbac649ee26572884029dcd0e108b4a2412cad003a84ed915c4e44a94ecae7", + "sha256:253569c85f881a6a8fe5e1c0758858edb1ecb4c3d8bce4aee4b52e5dc59fc091", + "sha256:292e2768280393bad406256e0ef9c30ddcd4867dbd42148b36f9d92a32d9e199", + "sha256:353a79e50aa84ac5e0d9f04bc3b2d78a2cc8edae3b842d757756449682778944", + "sha256:36db4c84d8f3aad328faaeaeaa454a633c95a854d78ea188791b147888379342", + "sha256:3aec6737c282e7fb5be41f6c7d0649e52ce49efb3508f30bada707e809bbbb5f", + "sha256:4086669280b2335d235c664373db46dcd7f6485dba4663ce1944ea01753c5e8b", + "sha256:4143803af909f284400c02f59de4d97e5ba9319de28366215ef55ef261914f9a", + "sha256:473bb86fa64ac9703d74a474514703b4665fa9a384221ced620b5025e64532a7", + "sha256:4932b2b5361013420189dbc8d30e970672d036b37cb382f1c09c3b6cfe651a37", + "sha256:49a8b713f0ff0bbc2f2af2e71c515cdced238786e25ef504f5982618c84c67a7", + "sha256:54d89442058271007ae8573dfa33360bea10e26603545481090b45e8b90c9d10", + "sha256:55df534d003efdd120c4ebfeb6b252c443d273cdc4b97a394eb0268367477795", + "sha256:5de5c3c1381ac303e71fdef75dace34af6e1d50b46ac081051cd209b8b933837", + "sha256:5f6eb8de31735c14ecc777e3ad77b57767b5f2dbea29265909ef696f51e8be92", + "sha256:6192e2ed81c15a7dab2554f5f69b134df8d1a982f8d9f13e57bdef93364d2120", + "sha256:643e48f92eaf65eb82964816bb7e7064ddb8239959210fa6168e8bce6fe6ef94", + "sha256:6ce7ac8a301a145f98d46db0bfd13bc8b3831a5bb92e8054d531a1f233bb4b93", + "sha256:7309a75b27c57f09b00a61815fb38c40da8e62e3004598e55ea1b8f713d96221", + "sha256:77f33cb95be6a49b93d8d6b81f05193676ae09857699cedf8f1a14a4285d077e", + "sha256:7a624d40c7b5ded7916f6cc65998af4585b4557c9ea65dc1e3a6273ebb2313ec", + "sha256:a06c0b96eb9d3bc88c627eec6e465726934ca0394347dc33efc742b8c91db83d", + "sha256:a4ce51e988570160af9ccfbbb1b9679546cbb1865d3a74ef0276f37fd94d91d9", + "sha256:ab6a600a9b2ea27453d56fd4464ad0db0ae69f5cea42595fcbdaabcd40396440", + "sha256:ab85c7cf772ba54f2718ebe366b836fffff868443f7c0c02389ed0a288cbde1f", + "sha256:ac45b91616e9b8afbe266ca08e80770b28e9e6d7a5852e3677fb37e42bde2047", + "sha256:adf20c34ba2cec1840a026023d93cc6d9b3581dfda6a044f434fc75b50c9a2ce", + "sha256:bd94048388b602fc30fdac2fecb986c034110ed8d2d17af7fd13b0de45c58bd7", + "sha256:c3440c90c31b61ae1df82f6c784643393f723354dc08013f9d5cedf25507c67c", + "sha256:cd630a2e6ec6df6a4977af63080fa8d63a0053ff8c06ea0200959b47ae75201c", + "sha256:d4e49cb46404cef61f83b30bb20ec9597c50ae7f55cfd6b89c17facc74675437", + "sha256:ec26244f9d9047f1bb401f1b86ff4775e1ddf638f4b4992bbc362a27a6f56673", + "sha256:f08a99e98b0a15083f06b1e221f064a29b3ed9e20617dc55aa8e823f2f732ace", + "sha256:fbf39dcd63f1d3b63c300fce59b7ea678bd5ea1d014e1e090a5226600a4132cb" + ], + "index": "pypi", + "markers": "platform_system != 'Darwin'", + "version": "==3.5.5" + }, + "certifi": { + "hashes": [ + "sha256:84c85a9078b11105f04f3036a9482ae10e4621616db313fe045dd24743a0820d", + "sha256:fe86415d55e84719d75f8b69414f6438ac3547d2078ab91b67e779ef69378412" + ], + "markers": "python_version >= '3.6'", + "version": "==2022.6.15" + }, + "cffi": { + "hashes": [ + "sha256:00a9ed42e88df81ffae7a8ab6d9356b371399b91dbdf0c3cb1e84c03a13aceb5", + "sha256:03425bdae262c76aad70202debd780501fabeaca237cdfddc008987c0e0f59ef", + "sha256:04ed324bda3cda42b9b695d51bb7d54b680b9719cfab04227cdd1e04e5de3104", + "sha256:0e2642fe3142e4cc4af0799748233ad6da94c62a8bec3a6648bf8ee68b1c7426", + "sha256:173379135477dc8cac4bc58f45db08ab45d228b3363adb7af79436135d028405", + "sha256:198caafb44239b60e252492445da556afafc7d1e3ab7a1fb3f0584ef6d742375", + "sha256:1e74c6b51a9ed6589199c787bf5f9875612ca4a8a0785fb2d4a84429badaf22a", + "sha256:2012c72d854c2d03e45d06ae57f40d78e5770d252f195b93f581acf3ba44496e", + "sha256:21157295583fe8943475029ed5abdcf71eb3911894724e360acff1d61c1d54bc", + "sha256:2470043b93ff09bf8fb1d46d1cb756ce6132c54826661a32d4e4d132e1977adf", + "sha256:285d29981935eb726a4399badae8f0ffdff4f5050eaa6d0cfc3f64b857b77185", + "sha256:30d78fbc8ebf9c92c9b7823ee18eb92f2e6ef79b45ac84db507f52fbe3ec4497", + "sha256:320dab6e7cb2eacdf0e658569d2575c4dad258c0fcc794f46215e1e39f90f2c3", + "sha256:33ab79603146aace82c2427da5ca6e58f2b3f2fb5da893ceac0c42218a40be35", + "sha256:3548db281cd7d2561c9ad9984681c95f7b0e38881201e157833a2342c30d5e8c", + "sha256:3799aecf2e17cf585d977b780ce79ff0dc9b78d799fc694221ce814c2c19db83", + "sha256:39d39875251ca8f612b6f33e6b1195af86d1b3e60086068be9cc053aa4376e21", + "sha256:3b926aa83d1edb5aa5b427b4053dc420ec295a08e40911296b9eb1b6170f6cca", + "sha256:3bcde07039e586f91b45c88f8583ea7cf7a0770df3a1649627bf598332cb6984", + "sha256:3d08afd128ddaa624a48cf2b859afef385b720bb4b43df214f85616922e6a5ac", + "sha256:3eb6971dcff08619f8d91607cfc726518b6fa2a9eba42856be181c6d0d9515fd", + "sha256:40f4774f5a9d4f5e344f31a32b5096977b5d48560c5592e2f3d2c4374bd543ee", + "sha256:4289fc34b2f5316fbb762d75362931e351941fa95fa18789191b33fc4cf9504a", + "sha256:470c103ae716238bbe698d67ad020e1db9d9dba34fa5a899b5e21577e6d52ed2", + "sha256:4f2c9f67e9821cad2e5f480bc8d83b8742896f1242dba247911072d4fa94c192", + "sha256:50a74364d85fd319352182ef59c5c790484a336f6db772c1a9231f1c3ed0cbd7", + "sha256:54a2db7b78338edd780e7ef7f9f6c442500fb0d41a5a4ea24fff1c929d5af585", + "sha256:5635bd9cb9731e6d4a1132a498dd34f764034a8ce60cef4f5319c0541159392f", + "sha256:59c0b02d0a6c384d453fece7566d1c7e6b7bae4fc5874ef2ef46d56776d61c9e", + "sha256:5d598b938678ebf3c67377cdd45e09d431369c3b1a5b331058c338e201f12b27", + "sha256:5df2768244d19ab7f60546d0c7c63ce1581f7af8b5de3eb3004b9b6fc8a9f84b", + "sha256:5ef34d190326c3b1f822a5b7a45f6c4535e2f47ed06fec77d3d799c450b2651e", + "sha256:6975a3fac6bc83c4a65c9f9fcab9e47019a11d3d2cf7f3c0d03431bf145a941e", + "sha256:6c9a799e985904922a4d207a94eae35c78ebae90e128f0c4e521ce339396be9d", + "sha256:70df4e3b545a17496c9b3f41f5115e69a4f2e77e94e1d2a8e1070bc0c38c8a3c", + "sha256:7473e861101c9e72452f9bf8acb984947aa1661a7704553a9f6e4baa5ba64415", + "sha256:8102eaf27e1e448db915d08afa8b41d6c7ca7a04b7d73af6514df10a3e74bd82", + "sha256:87c450779d0914f2861b8526e035c5e6da0a3199d8f1add1a665e1cbc6fc6d02", + "sha256:8b7ee99e510d7b66cdb6c593f21c043c248537a32e0bedf02e01e9553a172314", + "sha256:91fc98adde3d7881af9b59ed0294046f3806221863722ba7d8d120c575314325", + "sha256:94411f22c3985acaec6f83c6df553f2dbe17b698cc7f8ae751ff2237d96b9e3c", + "sha256:98d85c6a2bef81588d9227dde12db8a7f47f639f4a17c9ae08e773aa9c697bf3", + "sha256:9ad5db27f9cabae298d151c85cf2bad1d359a1b9c686a275df03385758e2f914", + "sha256:a0b71b1b8fbf2b96e41c4d990244165e2c9be83d54962a9a1d118fd8657d2045", + "sha256:a0f100c8912c114ff53e1202d0078b425bee3649ae34d7b070e9697f93c5d52d", + "sha256:a591fe9e525846e4d154205572a029f653ada1a78b93697f3b5a8f1f2bc055b9", + "sha256:a5c84c68147988265e60416b57fc83425a78058853509c1b0629c180094904a5", + "sha256:a66d3508133af6e8548451b25058d5812812ec3798c886bf38ed24a98216fab2", + "sha256:a8c4917bd7ad33e8eb21e9a5bbba979b49d9a97acb3a803092cbc1133e20343c", + "sha256:b3bbeb01c2b273cca1e1e0c5df57f12dce9a4dd331b4fa1635b8bec26350bde3", + "sha256:cba9d6b9a7d64d4bd46167096fc9d2f835e25d7e4c121fb2ddfc6528fb0413b2", + "sha256:cc4d65aeeaa04136a12677d3dd0b1c0c94dc43abac5860ab33cceb42b801c1e8", + "sha256:ce4bcc037df4fc5e3d184794f27bdaab018943698f4ca31630bc7f84a7b69c6d", + "sha256:cec7d9412a9102bdc577382c3929b337320c4c4c4849f2c5cdd14d7368c5562d", + "sha256:d400bfb9a37b1351253cb402671cea7e89bdecc294e8016a707f6d1d8ac934f9", + "sha256:d61f4695e6c866a23a21acab0509af1cdfd2c013cf256bbf5b6b5e2695827162", + "sha256:db0fbb9c62743ce59a9ff687eb5f4afbe77e5e8403d6697f7446e5f609976f76", + "sha256:dd86c085fae2efd48ac91dd7ccffcfc0571387fe1193d33b6394db7ef31fe2a4", + "sha256:e00b098126fd45523dd056d2efba6c5a63b71ffe9f2bbe1a4fe1716e1d0c331e", + "sha256:e229a521186c75c8ad9490854fd8bbdd9a0c9aa3a524326b55be83b54d4e0ad9", + "sha256:e263d77ee3dd201c3a142934a086a4450861778baaeeb45db4591ef65550b0a6", + "sha256:ed9cb427ba5504c1dc15ede7d516b84757c3e3d7868ccc85121d9310d27eed0b", + "sha256:fa6693661a4c91757f4412306191b6dc88c1703f780c8234035eac011922bc01", + "sha256:fcd131dd944808b5bdb38e6f5b53013c5aa4f334c5cad0c72742f6eba4b73db0" + ], + "index": "pypi", + "version": "==1.15.1" + }, + "charset-normalizer": { + "hashes": [ + "sha256:5a3d016c7c547f69d6f81fb0db9449ce888b418b5b9952cc5e6e66843e9dd845", + "sha256:83e9a75d1911279afd89352c68b45348559d1fc0506b054b346651b5e7fee29f" + ], + "markers": "python_version >= '3.6'", + "version": "==2.1.1" + }, + "click": { + "hashes": [ + "sha256:7682dc8afb30297001674575ea00d1814d808d6a36af415a82bd481d37ba7b8e", + "sha256:bb4d8133cb15a609f44e8213d9b391b0809795062913b383c62be0ee95b1db48" + ], + "markers": "python_version >= '3.7'", + "version": "==8.1.3" + }, + "coloredlogs": { + "hashes": [ + "sha256:612ee75c546f53e92e70049c9dbfcc18c935a2b9a53b66085ce9ef6a6e5c0934", + "sha256:7c991aa71a4577af2f82600d8f8f3a89f936baeaf9b50a9c197da014e5bf16b0" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", + "version": "==15.0.1" + }, + "crcmod": { + "hashes": [ + "sha256:50586ab48981f11e5b117523d97bb70864a2a1af246cf6e4f5c4a21ef4611cd1", + "sha256:69a2e5c6c36d0f096a7beb4cd34e5f882ec5fd232efb710cdb85d4ff196bd52e", + "sha256:737fb308fa2ce9aed2e29075f0d5980d4a89bfbec48a368c607c5c63b3efb90e", + "sha256:dc7051a0db5f2bd48665a990d3ec1cc305a466a77358ca4492826f41f283601e" + ], + "index": "pypi", + "version": "==1.7" + }, + "cryptography": { + "hashes": [ + "sha256:190f82f3e87033821828f60787cfa42bff98404483577b591429ed99bed39d59", + "sha256:2be53f9f5505673eeda5f2736bea736c40f051a739bfae2f92d18aed1eb54596", + "sha256:30788e070800fec9bbcf9faa71ea6d8068f5136f60029759fd8c3efec3c9dcb3", + "sha256:3d41b965b3380f10e4611dbae366f6dc3cefc7c9ac4e8842a806b9672ae9add5", + "sha256:4c590ec31550a724ef893c50f9a97a0c14e9c851c85621c5650d699a7b88f7ab", + "sha256:549153378611c0cca1042f20fd9c5030d37a72f634c9326e225c9f666d472884", + "sha256:63f9c17c0e2474ccbebc9302ce2f07b55b3b3fcb211ded18a42d5764f5c10a82", + "sha256:6bc95ed67b6741b2607298f9ea4932ff157e570ef456ef7ff0ef4884a134cc4b", + "sha256:7099a8d55cd49b737ffc99c17de504f2257e3787e02abe6d1a6d136574873441", + "sha256:75976c217f10d48a8b5a8de3d70c454c249e4b91851f6838a4e48b8f41eb71aa", + "sha256:7bc997818309f56c0038a33b8da5c0bfbb3f1f067f315f9abd6fc07ad359398d", + "sha256:80f49023dd13ba35f7c34072fa17f604d2f19bf0989f292cedf7ab5770b87a0b", + "sha256:91ce48d35f4e3d3f1d83e29ef4a9267246e6a3be51864a5b7d2247d5086fa99a", + "sha256:a958c52505c8adf0d3822703078580d2c0456dd1d27fabfb6f76fe63d2971cd6", + "sha256:b62439d7cd1222f3da897e9a9fe53bbf5c104fff4d60893ad1355d4c14a24157", + "sha256:b7f8dd0d4c1f21759695c05a5ec8536c12f31611541f8904083f3dc582604280", + "sha256:d204833f3c8a33bbe11eda63a54b1aad7aa7456ed769a982f21ec599ba5fa282", + "sha256:e007f052ed10cc316df59bc90fbb7ff7950d7e2919c9757fd42a2b8ecf8a5f67", + "sha256:f2dcb0b3b63afb6df7fd94ec6fbddac81b5492513f7b0436210d390c14d46ee8", + "sha256:f721d1885ecae9078c3f6bbe8a88bc0786b6e749bf32ccec1ef2b18929a05046", + "sha256:f7a6de3e98771e183645181b3627e2563dcde3ce94a9e42a3f427d2255190327", + "sha256:f8c0a6e9e1dd3eb0414ba320f85da6b0dcbd543126e30fcc546e7372a7fbf3b9" + ], + "index": "pypi", + "version": "==37.0.4" + }, + "cython": { + "hashes": [ + "sha256:061e25151c38f2361bc790d3bcf7f9d9828a0b6a4d5afa56fbed3bd33fb2373a", + "sha256:06be83490c906b6429b4389e13487a26254ccaad2eef6f3d4ee21d8d3a4aaa2b", + "sha256:07d173d3289415bb496e72cb0ddd609961be08fe2968c39094d5712ffb78672b", + "sha256:0bbc27abdf6aebfa1bce34cd92bd403070356f28b0ecb3198ff8a182791d58b9", + "sha256:0ea8267fc373a2c5064ad77d8ff7bf0ea8b88f7407098ff51829381f8ec1d5d9", + "sha256:3875c2b2ea752816a4d7ae59d45bb546e7c4c79093c83e3ba7f4d9051dd02928", + "sha256:39afb4679b8c6bf7ccb15b24025568f4f9b4d7f9bf3cbd981021f542acecd75b", + "sha256:3f85eb2343d20d91a4ea9cf14e5748092b376a64b7e07fc224e85b2753e9070b", + "sha256:40eff7aa26e91cf108fd740ffd4daf49f39b2fdffadabc7292b4b7dc5df879f0", + "sha256:479690d2892ca56d34812fe6ab8f58e4b2e0129140f3d94518f15993c40553da", + "sha256:4a4b03ab483271f69221c3210f7cde0dcc456749ecf8243b95bc7a701e5677e0", + "sha256:513e9707407608ac0d306c8b09d55a28be23ea4152cbd356ceaec0f32ef08d65", + "sha256:5514f3b4122cb22317122a48e175a7194e18e1803ca555c4c959d7dfe68eaf98", + "sha256:5ba622326f2862f9c1f99ca8d47ade49871241920a352c917e16861e25b0e5c3", + "sha256:63b79d9e1f7c4d1f498ab1322156a0d7dc1b6004bf981a8abda3f66800e140cd", + "sha256:656dc5ff1d269de4d11ee8542f2ffd15ab466c447c1f10e5b8aba6f561967276", + "sha256:67fdd2f652f8d4840042e2d2d91e15636ba2bcdcd92e7e5ffbc68e6ef633a754", + "sha256:79e3bab19cf1b021b613567c22eb18b76c0c547b9bc3903881a07bfd9e7e64cf", + "sha256:856d2fec682b3f31583719cb6925c6cdbb9aa30f03122bcc45c65c8b6f515754", + "sha256:8669cadeb26d9a58a5e6b8ce34d2c8986cc3b5c0bfa77eda6ceb471596cb2ec3", + "sha256:8733cf4758b79304f2a4e39ebfac5e92341bce47bcceb26c1254398b2f8c1af7", + "sha256:97335b2cd4acebf30d14e2855d882de83ad838491a09be2011745579ac975833", + "sha256:afbce249133a830f121b917f8c9404a44f2950e0e4f5d1e68f043da4c2e9f457", + "sha256:b0595aee62809ba353cebc5c7978e0e443760c3e882e2c7672c73ffe46383673", + "sha256:b6da3063c5c476f5311fd76854abae6c315f1513ef7d7904deed2e774623bbb9", + "sha256:c8e8025f496b5acb6ba95da2fb3e9dacffc97d9a92711aacfdd42f9c5927e094", + "sha256:cddc47ec746a08603037731f5d10aebf770ced08666100bd2cdcaf06a85d4d1b", + "sha256:cdf10af3e2e3279dc09fdc5f95deaa624850a53913f30350ceee824dc14fc1a6", + "sha256:d968ffc403d92addf20b68924d95428d523436adfd25cf505d427ed7ba3bee8b", + "sha256:dbee03b8d42dca924e6aa057b836a064c769ddfd2a4c2919e65da2c8a362d528", + "sha256:e1958e0227a4a6a2c06fd6e35b7469de50adf174102454db397cec6e1403cce3", + "sha256:e6ffa08aa1c111a1ebcbd1cf4afaaec120bc0bbdec3f2545f8bb7d3e8e77a1cd", + "sha256:e83228e0994497900af954adcac27f64c9a57cd70a9ec768ab0cb2c01fd15cf1", + "sha256:ea1dcc07bfb37367b639415333cfbfe4a93c3be340edf1db10964bc27d42ed64", + "sha256:eca3065a1279456e81c615211d025ea11bfe4e19f0c5650b859868ca04b3fcbd", + "sha256:ed087eeb88a8cf96c60fb76c5c3b5fb87188adee5e179f89ec9ad9a43c0c54b3", + "sha256:eeb475eb6f0ccf6c039035eb4f0f928eb53ead88777e0a760eccb140ad90930b", + "sha256:eefd2b9a5f38ded8d859fe96cc28d7d06e098dc3f677e7adbafda4dcdd4a461c", + "sha256:f3fd44cc362eee8ae569025f070d56208908916794b6ab21e139cea56470a2b3", + "sha256:f9944013588a3543fca795fffb0a070a31a243aa4f2d212f118aa95e69485831" + ], + "index": "pypi", + "version": "==0.29.32" + }, + "dill": { + "hashes": [ + "sha256:33501d03270bbe410c72639b350e941882a8b0fd55357580fbc873fba0c59302", + "sha256:d75e41f3eff1eee599d738e76ba8f4ad98ea229db8b085318aa2b3333a208c86" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4, 3.5, 3.6'", + "version": "==0.3.5.1" + }, + "flake8": { + "hashes": [ + "sha256:6fbe320aad8d6b95cec8b8e47bc933004678dc63095be98528b7bdd2a9f510db", + "sha256:7a1cf6b73744f5806ab95e526f6f0d8c01c66d7bbe349562d22dfca20610b248" + ], + "index": "pypi", + "version": "==5.0.4" + }, + "flask": { + "hashes": [ + "sha256:642c450d19c4ad482f96729bd2a8f6d32554aa1e231f4f6b4e7e5264b16cca2b", + "sha256:b9c46cc36662a7949f34b52d8ec7bb59c0d74ba08ba6cb9ce9adc1d8676d9526" + ], + "index": "pypi", + "version": "==2.2.2" + }, + "flatbuffers": { + "hashes": [ + "sha256:0ae7d69c5b82bf41962ca5fde9cc43033bc9501311d975fd5a25e8a7d29c1245", + "sha256:71e135d533be527192819aaab757c5e3d109cb10fbb01e687f6bdb7a61ad39d1" + ], + "version": "==2.0.7" + }, + "future-fstrings": { + "hashes": [ + "sha256:6cf41cbe97c398ab5a81168ce0dbb8ad95862d3caf23c21e4430627b90844089", + "sha256:90e49598b553d8746c4dc7d9442e0359d038c3039d802c91c0a55505da318c63" + ], + "index": "pypi", + "version": "==1.2.0" + }, + "gunicorn": { + "hashes": [ + "sha256:9dcc4547dbb1cb284accfb15ab5667a0e5d1881cc443e0677b4882a4067a807e", + "sha256:e0a968b5ba15f8a328fdfd7ab1fcb5af4470c28aaf7e55df02a99bc13138e6e8" + ], + "index": "pypi", + "version": "==20.1.0" + }, + "h3": { + "hashes": [ + "sha256:08a09f7a43ed142573c602ef487a058da54ab4d86c173082b29a5057805fe2d3", + "sha256:0a96ea1844182bd0511cdcdc89e38e3026d9a3d4139fd0c5e899709edd798ffa", + "sha256:1387166f453816f91d624c6ce70876a3c20356cd28a3a759920dee23c78684cf", + "sha256:25f0c22f4802ab71c45b86d206bd30fa0a6c7fbc3b630398b60c22907e9742e6", + "sha256:2bf4d75fe42a260ac23bf4cb9f9de6e6f2aa37279b2719387711f3e0727c4653", + "sha256:2faf304020493c5ffede34264bd28ed529b8b7238103e0904c0f3e9ca880bcfd", + "sha256:33b147ecc0e19ab1f27303d0e3ae28e5a457f3347ce18ca9a58b694a8b0cdd0a", + "sha256:38a084d74b234d48aafc01e4329cd9a92966e3f45b8cf21224118643b6eaa1c0", + "sha256:3b1642085939c597a9c723ae3b187f80527ffc79cad0ded0e55be9c9bac69c6c", + "sha256:4469fdf90034b1a67e155cac4f46b077fdc404b6182ab33abcb7081c9bfbf411", + "sha256:46a1284521d86cab414981056390be944dca780fe74c6c9e463a16d1c8d24871", + "sha256:585f375ba2a95ceb16b115a378e9118159c912c26703cf1627f57a004818c3b3", + "sha256:62057c1c3d1c7fe492841e42fa360825d66fafd55ac37dc4e90b2292af21cb47", + "sha256:68227df989274b0da54de9101a50741c70c48197ba3beacfb97c88170445c18e", + "sha256:7b0ddfdd02920996d7c6672c91e83efb5432c67ff83f89a03f774e84bcfe19f8", + "sha256:7c5366d24c2c01ef3bae68547c15f1965fac6053b2596c0073766bf7544ecaf0", + "sha256:8155b2de1938eb56128fe4fd96e4f6d2022d4c34d8137bc95d73cbf329f8f89e", + "sha256:83c2b0cd8259541f95b0493a620fb781b6a18c7c1e8fac1bda4fb234ae23ab43", + "sha256:8d03622433da1a2761574311af378ff1ff841f5956db25927837c6aee9d1c13c", + "sha256:960cd005b8817314d95fbaff3e848a72385df4e3c6c9703ff99b08581c8def69", + "sha256:9b0277d82578b3ed3220167ef5c5acd8b4e0ef2fcd6c2fd69dbf29e0c4e03765", + "sha256:a33fae02a54c63acb3c30fe49388715d658d76d42858a6ad4563e7e6859a9e57", + "sha256:a59d7d10597a2da9e9729637a625ae8dff2ed4e7c6c0b4952f0a5b2db6ef7152", + "sha256:ad21cfa8d97a62984ce30692a7ddf71a32a0c744cc247c43cbdbac1536aec4de", + "sha256:be63482de86bbb91db7f3f3b7dd452b9e08a11dacbda2088386831fb0e7de59c", + "sha256:c1108a9acb755310dce50a6e3c58ae1a2460ef60901d40e1155d633c7392f858", + "sha256:c644ab3f221c7faaab2d1ccd11bc3b1106f172e9bb1c85a863b0a097f6b71cce", + "sha256:c95c0818c163b69989c9e876dd82005e60edfbaabfd45429abebfc26f9a357e8", + "sha256:cdd68e684f0c6e18604d46ee04dbcfe5c79de62238b2c29f1db0f3a5d8dfa47b", + "sha256:ce86c6dce2c923bfb16e26586bc5f0443a8be61d4f43227be8587ccb95588a46", + "sha256:e61d3c6b1b66072f5b74d46dbee7df29daac6ce9738b9a6223a67dc577114515", + "sha256:f8edf5a546b31afdcd801b60448ea890ce1ff418fb784335e1329519f13aa85e" + ], + "version": "==3.7.4" + }, + "hatanaka": { + "hashes": [ + "sha256:5a0624f6812b13abb4c996398a60338566885c1786841c4c04de9b1b91da28d2", + "sha256:8fda4aa56f27313de75a806a2f5aa83ed5bb2dc7561bebab856a774d06cf1ee7", + "sha256:c22970b99169bddaf22e5239672e856a6bc9602c435f8793d26ad49619a70a99", + "sha256:ef594d63473782fac46df5b0c92a59211a3efea1d47c1a964244a0abffc9f3f6" + ], + "index": "pypi", + "version": "==2.4" + }, + "hexdump": { + "hashes": [ + "sha256:d781a43b0c16ace3f9366aade73e8ad3a7bd5137d58f0b45ab2d3f54876f20db" + ], + "index": "pypi", + "version": "==3.3" + }, + "humanfriendly": { + "hashes": [ + "sha256:1697e1a8a8f550fd43c2865cd84542fc175a61dcb779b6fee18cf6b6ccba1477", + "sha256:6b0b831ce8f15f7300721aa49829fc4e83921a9a301cc7f606be6686a2288ddc" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", + "version": "==10.0" + }, + "idna": { + "hashes": [ + "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", + "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" + ], + "markers": "python_version >= '3.5'", + "version": "==3.3" + }, + "importlib-metadata": { + "hashes": [ + "sha256:637245b8bab2b6502fcbc752cc4b7a6f6243bb02b31c5c26156ad103d3d45670", + "sha256:7401a975809ea1fdc658c3aa4f78cc2195a0e019c5cbc4c06122884e9ae80c23" + ], + "markers": "python_version < '3.10'", + "version": "==4.12.0" + }, + "importlib-resources": { + "hashes": [ + "sha256:5481e97fb45af8dcf2f798952625591c58fe599d0735d86b10f54de086a61681", + "sha256:f78a8df21a79bcc30cfd400bdc38f314333de7c0fb619763f6b9dabab8268bb7" + ], + "markers": "python_version >= '3.7'", + "version": "==5.9.0" + }, + "isort": { + "hashes": [ + "sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7", + "sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951" + ], + "markers": "python_version < '4.0' and python_full_version >= '3.6.1'", + "version": "==5.10.1" + }, + "itsdangerous": { + "hashes": [ + "sha256:2c2349112351b88699d8d4b6b075022c0808887cb7ad10069318a8b0bc88db44", + "sha256:5dbbc68b317e5e42f327f9021763545dc3fc3bfe22e6deb96aaf1fc38874156a" + ], + "markers": "python_version >= '3.7'", + "version": "==2.1.2" + }, + "jinja2": { + "hashes": [ + "sha256:31351a702a408a9e7595a8fc6150fc3f43bb6bf7e319770cbc0db9df9437e852", + "sha256:6088930bfe239f0e6710546ab9c19c9ef35e29792895fed6e6e31a023a182a61" + ], + "index": "pypi", + "version": "==3.1.2" + }, + "json-rpc": { + "hashes": [ + "sha256:84b45058e5ba95f49c7b6afcf7e03ab86bee89bf2c01f3ad8dd41fe114fc1f84", + "sha256:def0dbcf5b7084fc31d677f2f5990d988d06497f2f47f13024274cfb2d5d7589" + ], + "index": "pypi", + "version": "==1.13.0" + }, + "lazy-object-proxy": { + "hashes": [ + "sha256:043651b6cb706eee4f91854da4a089816a6606c1428fd391573ef8cb642ae4f7", + "sha256:07fa44286cda977bd4803b656ffc1c9b7e3bc7dff7d34263446aec8f8c96f88a", + "sha256:12f3bb77efe1367b2515f8cb4790a11cffae889148ad33adad07b9b55e0ab22c", + "sha256:2052837718516a94940867e16b1bb10edb069ab475c3ad84fd1e1a6dd2c0fcfc", + "sha256:2130db8ed69a48a3440103d4a520b89d8a9405f1b06e2cc81640509e8bf6548f", + "sha256:39b0e26725c5023757fc1ab2a89ef9d7ab23b84f9251e28f9cc114d5b59c1b09", + "sha256:46ff647e76f106bb444b4533bb4153c7370cdf52efc62ccfc1a28bdb3cc95442", + "sha256:4dca6244e4121c74cc20542c2ca39e5c4a5027c81d112bfb893cf0790f96f57e", + "sha256:553b0f0d8dbf21890dd66edd771f9b1b5f51bd912fa5f26de4449bfc5af5e029", + "sha256:677ea950bef409b47e51e733283544ac3d660b709cfce7b187f5ace137960d61", + "sha256:6a24357267aa976abab660b1d47a34aaf07259a0c3859a34e536f1ee6e76b5bb", + "sha256:6a6e94c7b02641d1311228a102607ecd576f70734dc3d5e22610111aeacba8a0", + "sha256:6aff3fe5de0831867092e017cf67e2750c6a1c7d88d84d2481bd84a2e019ec35", + "sha256:6ecbb350991d6434e1388bee761ece3260e5228952b1f0c46ffc800eb313ff42", + "sha256:7096a5e0c1115ec82641afbdd70451a144558ea5cf564a896294e346eb611be1", + "sha256:70ed0c2b380eb6248abdef3cd425fc52f0abd92d2b07ce26359fcbc399f636ad", + "sha256:8561da8b3dd22d696244d6d0d5330618c993a215070f473b699e00cf1f3f6443", + "sha256:85b232e791f2229a4f55840ed54706110c80c0a210d076eee093f2b2e33e1bfd", + "sha256:898322f8d078f2654d275124a8dd19b079080ae977033b713f677afcfc88e2b9", + "sha256:8f3953eb575b45480db6568306893f0bd9d8dfeeebd46812aa09ca9579595148", + "sha256:91ba172fc5b03978764d1df5144b4ba4ab13290d7bab7a50f12d8117f8630c38", + "sha256:9d166602b525bf54ac994cf833c385bfcc341b364e3ee71e3bf5a1336e677b55", + "sha256:a57d51ed2997e97f3b8e3500c984db50a554bb5db56c50b5dab1b41339b37e36", + "sha256:b9e89b87c707dd769c4ea91f7a31538888aad05c116a59820f28d59b3ebfe25a", + "sha256:bb8c5fd1684d60a9902c60ebe276da1f2281a318ca16c1d0a96db28f62e9166b", + "sha256:c19814163728941bb871240d45c4c30d33b8a2e85972c44d4e63dd7107faba44", + "sha256:c4ce15276a1a14549d7e81c243b887293904ad2d94ad767f42df91e75fd7b5b6", + "sha256:c7a683c37a8a24f6428c28c561c80d5f4fd316ddcf0c7cab999b15ab3f5c5c69", + "sha256:d609c75b986def706743cdebe5e47553f4a5a1da9c5ff66d76013ef396b5a8a4", + "sha256:d66906d5785da8e0be7360912e99c9188b70f52c422f9fc18223347235691a84", + "sha256:dd7ed7429dbb6c494aa9bc4e09d94b778a3579be699f9d67da7e6804c422d3de", + "sha256:df2631f9d67259dc9620d831384ed7732a198eb434eadf69aea95ad18c587a28", + "sha256:e368b7f7eac182a59ff1f81d5f3802161932a41dc1b1cc45c1f757dc876b5d2c", + "sha256:e40f2013d96d30217a51eeb1db28c9ac41e9d0ee915ef9d00da639c5b63f01a1", + "sha256:f769457a639403073968d118bc70110e7dce294688009f5c24ab78800ae56dc8", + "sha256:fccdf7c2c5821a8cbd0a9440a456f5050492f2270bd54e94360cac663398739b", + "sha256:fd45683c3caddf83abbb1249b653a266e7069a09f486daa8863fb0e7496a9fdb" + ], + "markers": "python_version >= '3.6'", + "version": "==1.7.1" + }, + "libusb1": { + "hashes": [ + "sha256:083f75e5d15cb5e2159e64c308c5317284eae926a820d6dce53a9505d18be3b2", + "sha256:0e652b04cbe85ec8e74f9ee82b49f861fb14b5320ae51399387ad2601ccc0500", + "sha256:5792a9defee40f15d330a40d9b1800545c32e47ba7fc66b6f28f133c9fcc8538", + "sha256:6f6bb010632ada35c661d17a65e135077beef0fbb2434d5ffdb3a4a911fd9490" + ], + "index": "pypi", + "version": "==3.0.0" + }, + "markupsafe": { + "hashes": [ + "sha256:0212a68688482dc52b2d45013df70d169f542b7394fc744c02a57374a4207003", + "sha256:089cf3dbf0cd6c100f02945abeb18484bd1ee57a079aefd52cffd17fba910b88", + "sha256:10c1bfff05d95783da83491be968e8fe789263689c02724e0c691933c52994f5", + "sha256:33b74d289bd2f5e527beadcaa3f401e0df0a89927c1559c8566c066fa4248ab7", + "sha256:3799351e2336dc91ea70b034983ee71cf2f9533cdff7c14c90ea126bfd95d65a", + "sha256:3ce11ee3f23f79dbd06fb3d63e2f6af7b12db1d46932fe7bd8afa259a5996603", + "sha256:421be9fbf0ffe9ffd7a378aafebbf6f4602d564d34be190fc19a193232fd12b1", + "sha256:43093fb83d8343aac0b1baa75516da6092f58f41200907ef92448ecab8825135", + "sha256:46d00d6cfecdde84d40e572d63735ef81423ad31184100411e6e3388d405e247", + "sha256:4a33dea2b688b3190ee12bd7cfa29d39c9ed176bda40bfa11099a3ce5d3a7ac6", + "sha256:4b9fe39a2ccc108a4accc2676e77da025ce383c108593d65cc909add5c3bd601", + "sha256:56442863ed2b06d19c37f94d999035e15ee982988920e12a5b4ba29b62ad1f77", + "sha256:671cd1187ed5e62818414afe79ed29da836dde67166a9fac6d435873c44fdd02", + "sha256:694deca8d702d5db21ec83983ce0bb4b26a578e71fbdbd4fdcd387daa90e4d5e", + "sha256:6a074d34ee7a5ce3effbc526b7083ec9731bb3cbf921bbe1d3005d4d2bdb3a63", + "sha256:6d0072fea50feec76a4c418096652f2c3238eaa014b2f94aeb1d56a66b41403f", + "sha256:6fbf47b5d3728c6aea2abb0589b5d30459e369baa772e0f37a0320185e87c980", + "sha256:7f91197cc9e48f989d12e4e6fbc46495c446636dfc81b9ccf50bb0ec74b91d4b", + "sha256:86b1f75c4e7c2ac2ccdaec2b9022845dbb81880ca318bb7a0a01fbf7813e3812", + "sha256:8dc1c72a69aa7e082593c4a203dcf94ddb74bb5c8a731e4e1eb68d031e8498ff", + "sha256:8e3dcf21f367459434c18e71b2a9532d96547aef8a871872a5bd69a715c15f96", + "sha256:8e576a51ad59e4bfaac456023a78f6b5e6e7651dcd383bcc3e18d06f9b55d6d1", + "sha256:96e37a3dc86e80bf81758c152fe66dbf60ed5eca3d26305edf01892257049925", + "sha256:97a68e6ada378df82bc9f16b800ab77cbf4b2fada0081794318520138c088e4a", + "sha256:99a2a507ed3ac881b975a2976d59f38c19386d128e7a9a18b7df6fff1fd4c1d6", + "sha256:a49907dd8420c5685cfa064a1335b6754b74541bbb3706c259c02ed65b644b3e", + "sha256:b09bf97215625a311f669476f44b8b318b075847b49316d3e28c08e41a7a573f", + "sha256:b7bd98b796e2b6553da7225aeb61f447f80a1ca64f41d83612e6139ca5213aa4", + "sha256:b87db4360013327109564f0e591bd2a3b318547bcef31b468a92ee504d07ae4f", + "sha256:bcb3ed405ed3222f9904899563d6fc492ff75cce56cba05e32eff40e6acbeaa3", + "sha256:d4306c36ca495956b6d568d276ac11fdd9c30a36f1b6eb928070dc5360b22e1c", + "sha256:d5ee4f386140395a2c818d149221149c54849dfcfcb9f1debfe07a8b8bd63f9a", + "sha256:dda30ba7e87fbbb7eab1ec9f58678558fd9a6b8b853530e176eabd064da81417", + "sha256:e04e26803c9c3851c931eac40c695602c6295b8d432cbe78609649ad9bd2da8a", + "sha256:e1c0b87e09fa55a220f058d1d49d3fb8df88fbfab58558f1198e08c1e1de842a", + "sha256:e72591e9ecd94d7feb70c1cbd7be7b3ebea3f548870aa91e2732960fa4d57a37", + "sha256:e8c843bbcda3a2f1e3c2ab25913c80a3c5376cd00c6e8c4a86a89a28c8dc5452", + "sha256:efc1913fd2ca4f334418481c7e595c00aad186563bbc1ec76067848c7ca0a933", + "sha256:f121a1420d4e173a5d96e47e9a0c0dcff965afdf1626d28de1460815f7c4ee7a", + "sha256:fc7b548b17d238737688817ab67deebb30e8073c95749d55538ed473130ec0c7" + ], + "markers": "python_version >= '3.7'", + "version": "==2.1.1" + }, + "mccabe": { + "hashes": [ + "sha256:348e0240c33b60bbdf4e523192ef919f28cb2c3d7d5c7794f74009290f236325", + "sha256:6c2d30ab6be0e4a46919781807b4f0d834ebdd6c6e3dca0bda5a15f863427b6e" + ], + "markers": "python_version >= '3.6'", + "version": "==0.7.0" + }, + "mpmath": { + "hashes": [ + "sha256:604bc21bd22d2322a177c73bdb573994ef76e62edd595d17e00aff24b0667e5c", + "sha256:79ffb45cf9f4b101a807595bcb3e72e0396202e0b1d25d689134b48c4216a81a" + ], + "version": "==1.2.1" + }, + "ncompress": { + "hashes": [ + "sha256:0349d7de11edd70a7efea9ce9dc67f0e47b5774832dd063f7ae68a9e3e36ea31", + "sha256:070044eab19586a45d1855c3e50e000ce86d6075b122a5ec8cffd480713dffac", + "sha256:13fa26ec8000d786a8079bb265508b5df4b445a4f460481a13549b4bd3c83824", + "sha256:15f10fbfa11345ff0af090e3e6ae13a1fe2b52a2bb39d4f2373c2d6aeac75e5d", + "sha256:2a104803fbe3ab0a96edb14927fa22c8142be838aabe7e938b4a52a4e82db56e", + "sha256:34754041d9bac2d6908ae0d07ba541e4d6d606cca222ddd53f3a57e15f386b0a", + "sha256:34c6496168fd4dbc13f1fc0c0fcbadded1957639956f8cbc6894c39999817e36", + "sha256:3590e66313041721ae81e72ece06b7048c9293321bb30900358638673608e264", + "sha256:393cc3c126b9451fb32fe2bc07773264c90e73afbd37da0df472ac23bfd1a2d5", + "sha256:5336a8831a7e587829ce54e9e27d1fb2e04ddbc7d2d983693e35a3a03ac3ce79", + "sha256:5a2ae8a9170fa1f45df7efa292eb8c437b18c225b63d4adca4f50f9da0e8e0c7", + "sha256:6540556d47670a8fb93878a44d0206bbdc87f32e4c5b57d6fe63691efafbb982", + "sha256:66d991155a1655ccd98e8433c4a7e811d63eb649adb55f47d8f9528a30cc4b7a", + "sha256:736dbae078107742cf6ac7ccc11ae9c5eab77ef2c02aab3ef64802877bb01cab", + "sha256:7608fbda43d04d9f476be2dbf4ef3c96e72d83b9557a48b07fbc9ff3ad29cdd2", + "sha256:78674f246878938387b6f82b10d1aa2192e02544d214541943d12ef1a45e66c6", + "sha256:8322482e72ac2802d1dca1007ec06aa281a4d5cf1cf9f8c75bb51e917382b756", + "sha256:8b9acc46cf36bb998ed215d6e76a94e2bd1e827b9a4cb5362982b7004b5a7620", + "sha256:8eb4a55cbeaeb238a3b412952077be6b3f37b3416cd0211cc22776391ff2fef7", + "sha256:916671d62167191af58d6b4a17b1c09c647e349dcff1fc0b7d764aa64c3773ee", + "sha256:94b3f4e851f5b37e1d4cf2d8da911fa10783a59cba3d7f1f2ae5bd2842558077", + "sha256:9cd040ad73a3b0e917e01cdfba507e10e0bb56849daaac3ac3d86382d4d7ad82", + "sha256:9d89acf209858e7940223cf35324e1b2effec119bb009a41f039e2ea4db22177", + "sha256:9da7c81313aed4b6c6e8020442ed8d03d04bff72947f9380ea1ce2c63ffb8ad1", + "sha256:aaa18a509d9fc173b4b47d53c834e43ced1eda63d2aa7d4613dc59b2f802a31a", + "sha256:ab9fc62baaa55faf8ed8ac67f2c64a7295fec91d7c1f306ac46aa894ca4edf91", + "sha256:af0011bae90e44121f4e4edbff3dccdce7e4c5fc5e354db7eb48410d71f496df", + "sha256:b031e06b42037b181e3514261e1e85a9eae4af990c12b9348a9f22b8042201ff", + "sha256:d11df815d280985dfa660974df11dbe051a1a18dca2f91f9d30fbd6548237b8f", + "sha256:d45ec59a8a3ce00613df0c81e5567854574dbbbf01ecd1a5a0929cd8fb04844d", + "sha256:da216a53db7cd4c0247376f87367dd71df457443567e55310f6d3d23a9aff2f2", + "sha256:e0ebd71990ef7909b6627b5341a2fe1977dcce61dd3760a29e19e3f9e4c6a275", + "sha256:e6f5bf381412e9d3847b76e8b6bd1f84dfadcd3d9c25903c8592facb437909a0", + "sha256:e7bbf10cca1376f4f17ae2c447e33a9d4067525abb0c71d488c9a5ced50552f1", + "sha256:f9ba6ab2aadd6fd90365fdad5219e4dc7bc2459b94f1e900a733dddaf4e9b2e6", + "sha256:fe0a671a2f7dc1ee0438d278ef30ab425a969536100c4352b5cb6bc0b6210818" + ], + "version": "==1.0.0" + }, + "nose": { + "hashes": [ + "sha256:9ff7c6cc443f8c51994b34a667bbcf45afd6d945be7477b52e97516fd17c53ac", + "sha256:dadcddc0aefbf99eea214e0f1232b94f2fa9bd98fa8353711dacb112bfcbbb2a", + "sha256:f1bffef9cbc82628f6e7d7b40d7e255aefaa1adb6a1b1d26c69a8b79e6208a98" + ], + "index": "pypi", + "version": "==1.3.7" + }, + "numpy": { + "hashes": [ + "sha256:17e5226674f6ea79e14e3b91bfbc153fdf3ac13f5cc54ee7bc8fdbe820a32da0", + "sha256:2bd879d3ca4b6f39b7770829f73278b7c5e248c91d538aab1e506c628353e47f", + "sha256:4f41f5bf20d9a521f8cab3a34557cd77b6f205ab2116651f12959714494268b0", + "sha256:5593f67e66dea4e237f5af998d31a43e447786b2154ba1ad833676c788f37cde", + "sha256:5e28cd64624dc2354a349152599e55308eb6ca95a13ce6a7d5679ebff2962913", + "sha256:633679a472934b1c20a12ed0c9a6c9eb167fbb4cb89031939bfd03dd9dbc62b8", + "sha256:806970e69106556d1dd200e26647e9bee5e2b3f1814f9da104a943e8d548ca38", + "sha256:806cc25d5c43e240db709875e947076b2826f47c2c340a5a2f36da5bb10c58d6", + "sha256:8247f01c4721479e482cc2f9f7d973f3f47810cbc8c65e38fd1bbd3141cc9842", + "sha256:8ebf7e194b89bc66b78475bd3624d92980fca4e5bb86dda08d677d786fefc414", + "sha256:8ecb818231afe5f0f568c81f12ce50f2b828ff2b27487520d85eb44c71313b9e", + "sha256:8f9d84a24889ebb4c641a9b99e54adb8cab50972f0166a3abc14c3b93163f074", + "sha256:909c56c4d4341ec8315291a105169d8aae732cfb4c250fbc375a1efb7a844f8f", + "sha256:9b83d48e464f393d46e8dd8171687394d39bc5abfe2978896b77dc2604e8635d", + "sha256:ac987b35df8c2a2eab495ee206658117e9ce867acf3ccb376a19e83070e69418", + "sha256:b78d00e48261fbbd04aa0d7427cf78d18401ee0abd89c7559bbf422e5b1c7d01", + "sha256:b8b97a8a87cadcd3f94659b4ef6ec056261fa1e1c3317f4193ac231d4df70215", + "sha256:bd5b7ccae24e3d8501ee5563e82febc1771e73bd268eef82a1e8d2b4d556ae66", + "sha256:bdc02c0235b261925102b1bd586579b7158e9d0d07ecb61148a1799214a4afd5", + "sha256:be6b350dfbc7f708d9d853663772a9310783ea58f6035eec649fb9c4371b5389", + "sha256:c403c81bb8ffb1c993d0165a11493fd4bf1353d258f6997b3ee288b0a48fce77", + "sha256:cf8c6aed12a935abf2e290860af8e77b26a042eb7f2582ff83dc7ed5f963340c", + "sha256:d98addfd3c8728ee8b2c49126f3c44c703e2b005d4a95998e2167af176a9e722", + "sha256:dc76bca1ca98f4b122114435f83f1fcf3c0fe48e4e6f660e07996abf2f53903c", + "sha256:dec198619b7dbd6db58603cd256e092bcadef22a796f778bf87f8592b468441d", + "sha256:df28dda02c9328e122661f399f7655cdcbcf22ea42daa3650a26bce08a187450", + "sha256:e603ca1fb47b913942f3e660a15e55a9ebca906857edfea476ae5f0fe9b457d5", + "sha256:ecfdd68d334a6b97472ed032b5b37a30d8217c097acfff15e8452c710e775524" + ], + "index": "pypi", + "version": "==1.23.2" + }, + "onnx": { + "hashes": [ + "sha256:13b3e77d27523b9dbf4f30dfc9c959455859d5e34e921c44f712d69b8369eff9", + "sha256:213e73610173f6b2e99f99a4b0636f80b379c417312079d603806e48ada4ca8b", + "sha256:23781594bb8b7ee985de1005b3c601648d5b0568a81e01365c48f91d1f5648e4", + "sha256:2d9a7db54e75529160337232282a4816cc50667dc7dc34be178fd6f6b79d4705", + "sha256:341c7016e23273e9ffa9b6e301eee95b8c37d0f04df7cedbdb169d2c39524c96", + "sha256:3c6e6bcffc3f5c1e148df3837dc667fa4c51999788c1b76b0b8fbba607e02da8", + "sha256:5578b93dc6c918cec4dee7fb7d9dd3b09d338301ee64ca8b4f28bc217ed42dca", + "sha256:56ceb7e094c43882b723cfaa107d85ad673cfdf91faeb28d7dcadacca4f43a07", + "sha256:81a3555fd67be2518bf86096299b48fb9154652596219890abfe90bd43a9ec13", + "sha256:8a7aa61aea339bd28f310f4af4f52ce6c4b876386228760b16308efd58f95059", + "sha256:9fd2f4e23078df197bb76a59b9cd8f5a43a6ad2edc035edb3ecfb9042093e05a", + "sha256:af90427ca04c6b7b8107c2021e1273227a3ef1a7a01f3073039cae7855a59833", + "sha256:b3629e8258db15d4e2c9b7f1be91a3186719dd94661c218c6f5fde3cc7de3d4d", + "sha256:bdbd2578424c70836f4d0f9dda16c21868ddb07cc8192f9e8a176908b43d694b", + "sha256:c11162ffc487167da140f1112f49c4f82d815824f06e58bc3095407699f05863", + "sha256:c39a7a0352c856f1df30dccf527eb6cb4909052e5eaf6fa2772a637324c526aa", + "sha256:c7a9b3ea02c30efc1d2662337e280266aca491a8e86be0d8a657f874b7cccd1e", + "sha256:f66d2996e65f490a57b3ae952e4e9189b53cc9fe3f75e601d50d4db2dc1b1cd9", + "sha256:f8800f28c746ab06e51ef8449fd1215621f4ddba91be3ffc264658937d38a2af", + "sha256:fab13feb4d94342aae6d357d480f2e47d41b9f4e584367542b21ca6defda9e0a", + "sha256:fea5156a03398fe0e23248042d8651c1eaac5f6637d4dd683b4c1f1320b9f7b4" + ], + "index": "pypi", + "version": "==1.12.0" + }, + "onnxruntime-gpu": { + "hashes": [ + "sha256:296bd9733986cb7517d15bef5535c555d3f863963a71e6575e92d2a854aee61d", + "sha256:42b0393c5122ed90fa2eb76192a486261d86e9526ccb78b2a98923c22791d2d1", + "sha256:8e46d0724ce54c5908c5760037b78de741fbd48962b370c29ebc20e608b30eda", + "sha256:b37872527d03d3df10756408ca44014bd6ac354a044ab1c4286cd42dc138e518", + "sha256:d73204323aefebe4eecab9fcf76e22b1a00394e3d838c2962a28a27301186b73", + "sha256:e2be6f7f5a1ce0bc8471ce42e10eab92cfb19d0748b857edcb5320b5e98311b7", + "sha256:ecfe97335027e569d4f46725ba89316041e562b8c499690e25e11cfee4601cd1", + "sha256:fd919373be35b9ba54210688265df38ad5e19a530449385c40dab51da407345d" + ], + "index": "pypi", + "markers": "platform_system != 'Darwin'", + "version": "==1.12.1" + }, + "packaging": { + "hashes": [ + "sha256:dd47c42927d89ab911e606518907cc2d3a1f38bbd026385970643f9c5b8ecfeb", + "sha256:ef103e05f519cdc783ae24ea4e2e0f508a9c99b2d4969652eed6a2e1ea5bd522" + ], + "markers": "python_version >= '3.6'", + "version": "==21.3" + }, + "pillow": { + "hashes": [ + "sha256:0030fdbd926fb85844b8b92e2f9449ba89607231d3dd597a21ae72dc7fe26927", + "sha256:030e3460861488e249731c3e7ab59b07c7853838ff3b8e16aac9561bb345da14", + "sha256:0ed2c4ef2451de908c90436d6e8092e13a43992f1860275b4d8082667fbb2ffc", + "sha256:136659638f61a251e8ed3b331fc6ccd124590eeff539de57c5f80ef3a9594e58", + "sha256:13b725463f32df1bfeacbf3dd197fb358ae8ebcd8c5548faa75126ea425ccb60", + "sha256:1536ad017a9f789430fb6b8be8bf99d2f214c76502becc196c6f2d9a75b01b76", + "sha256:15928f824870535c85dbf949c09d6ae7d3d6ac2d6efec80f3227f73eefba741c", + "sha256:17d4cafe22f050b46d983b71c707162d63d796a1235cdf8b9d7a112e97b15bac", + "sha256:1802f34298f5ba11d55e5bb09c31997dc0c6aed919658dfdf0198a2fe75d5490", + "sha256:1cc1d2451e8a3b4bfdb9caf745b58e6c7a77d2e469159b0d527a4554d73694d1", + "sha256:1fd6f5e3c0e4697fa7eb45b6e93996299f3feee73a3175fa451f49a74d092b9f", + "sha256:254164c57bab4b459f14c64e93df11eff5ded575192c294a0c49270f22c5d93d", + "sha256:2ad0d4df0f5ef2247e27fc790d5c9b5a0af8ade9ba340db4a73bb1a4a3e5fb4f", + "sha256:2c58b24e3a63efd22554c676d81b0e57f80e0a7d3a5874a7e14ce90ec40d3069", + "sha256:2d33a11f601213dcd5718109c09a52c2a1c893e7461f0be2d6febc2879ec2402", + "sha256:337a74fd2f291c607d220c793a8135273c4c2ab001b03e601c36766005f36885", + "sha256:37ff6b522a26d0538b753f0b4e8e164fdada12db6c6f00f62145d732d8a3152e", + "sha256:3d1f14f5f691f55e1b47f824ca4fdcb4b19b4323fe43cc7bb105988cad7496be", + "sha256:408673ed75594933714482501fe97e055a42996087eeca7e5d06e33218d05aa8", + "sha256:4134d3f1ba5f15027ff5c04296f13328fecd46921424084516bdb1b2548e66ff", + "sha256:4ad2f835e0ad81d1689f1b7e3fbac7b01bb8777d5a985c8962bedee0cc6d43da", + "sha256:50dff9cc21826d2977ef2d2a205504034e3a4563ca6f5db739b0d1026658e004", + "sha256:510cef4a3f401c246cfd8227b300828715dd055463cdca6176c2e4036df8bd4f", + "sha256:5aed7dde98403cd91d86a1115c78d8145c83078e864c1de1064f52e6feb61b20", + "sha256:69bd1a15d7ba3694631e00df8de65a8cb031911ca11f44929c97fe05eb9b6c1d", + "sha256:6bf088c1ce160f50ea40764f825ec9b72ed9da25346216b91361eef8ad1b8f8c", + "sha256:6e8c66f70fb539301e064f6478d7453e820d8a2c631da948a23384865cd95544", + "sha256:727dd1389bc5cb9827cbd1f9d40d2c2a1a0c9b32dd2261db522d22a604a6eec9", + "sha256:74a04183e6e64930b667d321524e3c5361094bb4af9083db5c301db64cd341f3", + "sha256:75e636fd3e0fb872693f23ccb8a5ff2cd578801251f3a4f6854c6a5d437d3c04", + "sha256:7761afe0126d046974a01e030ae7529ed0ca6a196de3ec6937c11df0df1bc91c", + "sha256:7888310f6214f19ab2b6df90f3f06afa3df7ef7355fc025e78a3044737fab1f5", + "sha256:7b0554af24df2bf96618dac71ddada02420f946be943b181108cac55a7a2dcd4", + "sha256:7c7b502bc34f6e32ba022b4a209638f9e097d7a9098104ae420eb8186217ebbb", + "sha256:808add66ea764ed97d44dda1ac4f2cfec4c1867d9efb16a33d158be79f32b8a4", + "sha256:831e648102c82f152e14c1a0938689dbb22480c548c8d4b8b248b3e50967b88c", + "sha256:93689632949aff41199090eff5474f3990b6823404e45d66a5d44304e9cdc467", + "sha256:96b5e6874431df16aee0c1ba237574cb6dff1dcb173798faa6a9d8b399a05d0e", + "sha256:9a54614049a18a2d6fe156e68e188da02a046a4a93cf24f373bffd977e943421", + "sha256:a138441e95562b3c078746a22f8fca8ff1c22c014f856278bdbdd89ca36cff1b", + "sha256:a647c0d4478b995c5e54615a2e5360ccedd2f85e70ab57fbe817ca613d5e63b8", + "sha256:a9c9bc489f8ab30906d7a85afac4b4944a572a7432e00698a7239f44a44e6efb", + "sha256:ad2277b185ebce47a63f4dc6302e30f05762b688f8dc3de55dbae4651872cdf3", + "sha256:b6d5e92df2b77665e07ddb2e4dbd6d644b78e4c0d2e9272a852627cdba0d75cf", + "sha256:bc431b065722a5ad1dfb4df354fb9333b7a582a5ee39a90e6ffff688d72f27a1", + "sha256:bdd0de2d64688ecae88dd8935012c4a72681e5df632af903a1dca8c5e7aa871a", + "sha256:c79698d4cd9318d9481d89a77e2d3fcaeff5486be641e60a4b49f3d2ecca4e28", + "sha256:cb6259196a589123d755380b65127ddc60f4c64b21fc3bb46ce3a6ea663659b0", + "sha256:d5b87da55a08acb586bad5c3aa3b86505f559b84f39035b233d5bf844b0834b1", + "sha256:dcd7b9c7139dc8258d164b55696ecd16c04607f1cc33ba7af86613881ffe4ac8", + "sha256:dfe4c1fedfde4e2fbc009d5ad420647f7730d719786388b7de0999bf32c0d9fd", + "sha256:ea98f633d45f7e815db648fd7ff0f19e328302ac36427343e4432c84432e7ff4", + "sha256:ec52c351b35ca269cb1f8069d610fc45c5bd38c3e91f9ab4cbbf0aebc136d9c8", + "sha256:eef7592281f7c174d3d6cbfbb7ee5984a671fcd77e3fc78e973d492e9bf0eb3f", + "sha256:f07f1f00e22b231dd3d9b9208692042e29792d6bd4f6639415d2f23158a80013", + "sha256:f3fac744f9b540148fa7715a435d2283b71f68bfb6d4aae24482a890aed18b59", + "sha256:fa768eff5f9f958270b081bb33581b4b569faabf8774726b283edb06617101dc", + "sha256:fac2d65901fb0fdf20363fbd345c01958a742f2dc62a8dd4495af66e3ff502a4" + ], + "index": "pypi", + "version": "==9.2.0" + }, + "platformdirs": { + "hashes": [ + "sha256:027d8e83a2d7de06bbac4e5ef7e023c02b863d7ea5d079477e722bb41ab25788", + "sha256:58c8abb07dcb441e6ee4b11d8df0ac856038f944ab98b7be6b27b2a3c7feef19" + ], + "markers": "python_version >= '3.7'", + "version": "==2.5.2" + }, + "protobuf": { + "hashes": [ + "sha256:06059eb6953ff01e56a25cd02cca1a9649a75a7e65397b5b9b4e929ed71d10cf", + "sha256:097c5d8a9808302fb0da7e20edf0b8d4703274d140fd25c5edabddcde43e081f", + "sha256:0d4719e724472e296062ba8e82a36d64693fcfdb550d9dff98af70ca79eafe3d", + "sha256:284f86a6207c897542d7e956eb243a36bb8f9564c1742b253462386e96c6b78f", + "sha256:2b35602cb65d53c168c104469e714bf68670335044c38eee3c899d6a8af03ffc", + "sha256:32ca378605b41fd180dfe4e14d3226386d8d1b002ab31c969c366549e66a2bb7", + "sha256:32fff501b6df3050936d1839b80ea5899bf34db24792d223d7640611f67de15a", + "sha256:34400fd76f85bdae9a2e9c1444ea4699c0280962423eff4418765deceebd81b5", + "sha256:3767c64593a49c7ac0accd08ed39ce42744405f0989d468f0097a17496fdbe84", + "sha256:3cc797c9d15d7689ed507b165cd05913acb992d78b379f6014e013f9ecb20996", + "sha256:3f2ed842e8ca43b790cb4a101bcf577226e0ded98a6a6ba2d5e12095a08dc4da", + "sha256:52c1e44e25f2949be7ffa7c66acbfea940b0945dd416920231f7cb30ea5ac6db", + "sha256:5d9b5c8270461706973c3871c6fbdd236b51dfe9dab652f1fb6a36aa88287e47", + "sha256:62f1b5c4cd6c5402b4e2d63804ba49a327e0c386c99b1675c8a0fefda23b2067", + "sha256:69ccfdf3657ba59569c64295b7d51325f91af586f8d5793b734260dfe2e94e2c", + "sha256:6f50601512a3d23625d8a85b1638d914a0970f17920ff39cec63aaef80a93fb7", + "sha256:72d357cc4d834cc85bd957e8b8e1f4b64c2eac9ca1a942efeb8eb2e723fca852", + "sha256:7403941f6d0992d40161aa8bb23e12575637008a5a02283a930addc0508982f9", + "sha256:755f3aee41354ae395e104d62119cb223339a8f3276a0cd009ffabfcdd46bb0c", + "sha256:77053d28427a29987ca9caf7b72ccafee011257561259faba8dd308fda9a8739", + "sha256:79cd8d0a269b714f6b32641f86928c718e8d234466919b3f552bfb069dbb159b", + "sha256:7e371f10abe57cee5021797126c93479f59fccc9693dafd6bd5633ab67808a91", + "sha256:9016d01c91e8e625141d24ec1b20fed584703e527d28512aa8c8707f105a683c", + "sha256:9be73ad47579abc26c12024239d3540e6b765182a91dbc88e23658ab71767153", + "sha256:a4c0c6f2f95a559e59a0258d3e4b186f340cbdc5adec5ce1bc06d01972527c88", + "sha256:adc31566d027f45efe3f44eeb5b1f329da43891634d61c75a5944e9be6dd42c9", + "sha256:adfc6cf69c7f8c50fd24c793964eef18f0ac321315439d94945820612849c388", + "sha256:af0ebadc74e281a517141daad9d0f2c5d93ab78e9d455113719a45a49da9db4e", + "sha256:b309fda192850ac4184ca1777aab9655564bc8d10a9cc98f10e1c8bf11295c22", + "sha256:b3d7d4b4945fe3c001403b6c24442901a5e58c0a3059290f5a63523ed4435f82", + "sha256:c8829092c5aeb61619161269b2f8a2e36fd7cb26abbd9282d3bc453f02769146", + "sha256:cb29edb9eab15742d791e1025dd7b6a8f6fcb53802ad2f6e3adcb102051063ab", + "sha256:cd68be2559e2a3b84f517fb029ee611546f7812b1fdd0aa2ecc9bc6ec0e4fdde", + "sha256:cdee09140e1cd184ba9324ec1df410e7147242b94b5f8b0c64fc89e38a8ba531", + "sha256:db977c4ca738dd9ce508557d4fce0f5aebd105e158c725beec86feb1f6bc20d8", + "sha256:dd5789b2948ca702c17027c84c2accb552fc30f4622a98ab5c51fcfe8c50d3e7", + "sha256:e250a42f15bf9d5b09fe1b293bdba2801cd520a9f5ea2d7fb7536d4441811d20", + "sha256:ff8d8fa42675249bb456f5db06c00de6c2f4c27a065955917b28c4f15978b9c3" + ], + "index": "pypi", + "version": "==3.20.1" + }, + "psutil": { + "hashes": [ + "sha256:068935df39055bf27a29824b95c801c7a5130f118b806eee663cad28dca97685", + "sha256:0904727e0b0a038830b019551cf3204dd48ef5c6868adc776e06e93d615fc5fc", + "sha256:0f15a19a05f39a09327345bc279c1ba4a8cfb0172cc0d3c7f7d16c813b2e7d36", + "sha256:19f36c16012ba9cfc742604df189f2f28d2720e23ff7d1e81602dbe066be9fd1", + "sha256:20b27771b077dcaa0de1de3ad52d22538fe101f9946d6dc7869e6f694f079329", + "sha256:28976df6c64ddd6320d281128817f32c29b539a52bdae5e192537bc338a9ec81", + "sha256:29a442e25fab1f4d05e2655bb1b8ab6887981838d22effa2396d584b740194de", + "sha256:3054e923204b8e9c23a55b23b6df73a8089ae1d075cb0bf711d3e9da1724ded4", + "sha256:32c52611756096ae91f5d1499fe6c53b86f4a9ada147ee42db4991ba1520e574", + "sha256:3a76ad658641172d9c6e593de6fe248ddde825b5866464c3b2ee26c35da9d237", + "sha256:44d1826150d49ffd62035785a9e2c56afcea66e55b43b8b630d7706276e87f22", + "sha256:4b6750a73a9c4a4e689490ccb862d53c7b976a2a35c4e1846d049dcc3f17d83b", + "sha256:56960b9e8edcca1456f8c86a196f0c3d8e3e361320071c93378d41445ffd28b0", + "sha256:57f1819b5d9e95cdfb0c881a8a5b7d542ed0b7c522d575706a80bedc848c8954", + "sha256:58678bbadae12e0db55186dc58f2888839228ac9f41cc7848853539b70490021", + "sha256:645bd4f7bb5b8633803e0b6746ff1628724668681a434482546887d22c7a9537", + "sha256:799759d809c31aab5fe4579e50addf84565e71c1dc9f1c31258f159ff70d3f87", + "sha256:79c9108d9aa7fa6fba6e668b61b82facc067a6b81517cab34d07a84aa89f3df0", + "sha256:91c7ff2a40c373d0cc9121d54bc5f31c4fa09c346528e6a08d1845bce5771ffc", + "sha256:9272167b5f5fbfe16945be3db475b3ce8d792386907e673a209da686176552af", + "sha256:944c4b4b82dc4a1b805329c980f270f170fdc9945464223f2ec8e57563139cf4", + "sha256:a6a11e48cb93a5fa606306493f439b4aa7c56cb03fc9ace7f6bfa21aaf07c453", + "sha256:a8746bfe4e8f659528c5c7e9af5090c5a7d252f32b2e859c584ef7d8efb1e689", + "sha256:abd9246e4cdd5b554a2ddd97c157e292ac11ef3e7af25ac56b08b455c829dca8", + "sha256:b14ee12da9338f5e5b3a3ef7ca58b3cba30f5b66f7662159762932e6d0b8f680", + "sha256:b88f75005586131276634027f4219d06e0561292be8bd6bc7f2f00bdabd63c4e", + "sha256:c7be9d7f5b0d206f0bbc3794b8e16fb7dbc53ec9e40bbe8787c6f2d38efcf6c9", + "sha256:d2d006286fbcb60f0b391741f520862e9b69f4019b4d738a2a45728c7e952f1b", + "sha256:db417f0865f90bdc07fa30e1aadc69b6f4cad7f86324b02aa842034efe8d8c4d", + "sha256:e7e10454cb1ab62cc6ce776e1c135a64045a11ec4c6d254d3f7689c16eb3efd2", + "sha256:f65f9a46d984b8cd9b3750c2bdb419b2996895b005aefa6cbaba9a143b1ce2c5", + "sha256:fea896b54f3a4ae6f790ac1d017101252c93f6fe075d0e7571543510f11d2676" + ], + "index": "pypi", + "version": "==5.9.1" + }, + "pycapnp": { + "hashes": [ + "sha256:0c770145a4eccfe97f53ab500283aa9bf969d4a37bffc75737a964db4f2af833", + "sha256:13badfb644e2eb7f1219aab259d18b262d1512021e4112fa1ad5e74d17bc30cf", + "sha256:1774a4fe9db5f094ba40cf00898fa4b437773e7f9c538b779275b9f422a92ebc", + "sha256:2a1746017079107232faf26af8ef4284ab0b20ce5cbe688d44e7553a67e5e5cb", + "sha256:2a7aa9af0185e5977a59228db5042dffb048b2d4bf4f665d2105b4781cf2fcbc", + "sha256:2b28d5d951602c0b832bbe63f85ebdd7685b33118b1c11c2c65a243ec9f35a66", + "sha256:47c8bc28521312660c95cfc8a552654949407f8b17bc7ed6955ad7dae34d21a4", + "sha256:60adf2674f89f629551171116b8f400b17e9a41a2ef15736767acec405d4ca50", + "sha256:61b009faba34855c9d29db107e188898c83099347e22ebcbc1d955774403247b", + "sha256:786a2e39b79e592a41e8a1eaeea6e41e2015ecb9f5b7f7c20dfc5768ba1ae077", + "sha256:a788a374ccb93354943c89f5b1caf785faf7bb90191cd6265e042aa004f8b206", + "sha256:b9a1d6306a0e3e0090574aeb08d432bd67f9eb04ab564e89ef34cd1fe320b20f", + "sha256:bdd013eae51d190a2426d00cc72d0aaed148a5be778ca86ee1adae3ab7a0613f" + ], + "index": "pypi", + "version": "==1.1.0" + }, + "pycodestyle": { + "hashes": [ + "sha256:2c9607871d58c76354b697b42f5d57e1ada7d261c261efac224b664affdc5785", + "sha256:d1735fc58b418fd7c5f658d28d943854f8a849b01a5d0a1e6f3f3fdd0166804b" + ], + "markers": "python_version >= '3.6'", + "version": "==2.9.1" + }, + "pycparser": { + "hashes": [ + "sha256:8ee45429555515e1f6b185e78100aea234072576aa43ab53aefcae078162fca9", + "sha256:e644fdec12f7872f86c58ff790da456218b10f863970249516d60a5eaca77206" + ], + "version": "==2.21" + }, + "pycryptodome": { + "hashes": [ + "sha256:045d75527241d17e6ef13636d845a12e54660aa82e823b3b3341bcf5af03fa79", + "sha256:0926f7cc3735033061ef3cf27ed16faad6544b14666410727b31fea85a5b16eb", + "sha256:092a26e78b73f2530b8bd6b3898e7453ab2f36e42fd85097d705d6aba2ec3e5e", + "sha256:1b22bcd9ec55e9c74927f6b1f69843cb256fb5a465088ce62837f793d9ffea88", + "sha256:2aa55aae81f935a08d5a3c2042eb81741a43e044bd8a81ea7239448ad751f763", + "sha256:2ea63d46157386c5053cfebcdd9bd8e0c8b7b0ac4a0507a027f5174929403884", + "sha256:2ec709b0a58b539a4f9d33fb8508264c3678d7edb33a68b8906ba914f71e8c13", + "sha256:2ffd8b31561455453ca9f62cb4c24e6b8d119d6d531087af5f14b64bee2c23e6", + "sha256:4b52cb18b0ad46087caeb37a15e08040f3b4c2d444d58371b6f5d786d95534c2", + "sha256:4c3ccad74eeb7b001f3538643c4225eac398c77d617ebb3e57571a897943c667", + "sha256:5099c9ca345b2f252f0c28e96904643153bae9258647585e5e6f649bb7a1844a", + "sha256:57f565acd2f0cf6fb3e1ba553d0cb1f33405ec1f9c5ded9b9a0a5320f2c0bd3d", + "sha256:60b4faae330c3624cc5a546ba9cfd7b8273995a15de94ee4538130d74953ec2e", + "sha256:7c9ed8aa31c146bef65d89a1b655f5f4eab5e1120f55fc297713c89c9e56ff0b", + "sha256:7e3a8f6ee405b3bd1c4da371b93c31f7027944b2bcce0697022801db93120d83", + "sha256:9135dddad504592bcc18b0d2d95ce86c3a5ea87ec6447ef25cfedea12d6018b8", + "sha256:9c772c485b27967514d0df1458b56875f4b6d025566bf27399d0c239ff1b369f", + "sha256:9eaadc058106344a566dc51d3d3a758ab07f8edde013712bc8d22032a86b264f", + "sha256:9ee40e2168f1348ae476676a2e938ca80a2f57b14a249d8fe0d3cdf803e5a676", + "sha256:a8f06611e691c2ce45ca09bbf983e2ff2f8f4f87313609d80c125aff9fad6e7f", + "sha256:b9c5b1a1977491533dfd31e01550ee36ae0249d78aae7f632590db833a5012b8", + "sha256:b9cc96e274b253e47ad33ae1fccc36ea386f5251a823ccb50593a935db47fdd2", + "sha256:c3640deff4197fa064295aaac10ab49a0d55ef3d6a54ae1499c40d646655c89f", + "sha256:c77126899c4b9c9827ddf50565e93955cb3996813c18900c16b2ea0474e130e9", + "sha256:d2a39a66057ab191e5c27211a7daf8f0737f23acbf6b3562b25a62df65ffcb7b", + "sha256:e244ab85c422260de91cda6379e8e986405b4f13dc97d2876497178707f87fc1", + "sha256:ecaaef2d21b365d9c5ca8427ffc10cebed9d9102749fd502218c23cb9a05feb5", + "sha256:fd2184aae6ee2a944aaa49113e6f5787cdc5e4db1eb8edb1aea914bd75f33a0c", + "sha256:ff287bcba9fbeb4f1cccc1f2e90a08d691480735a611ee83c80a7d74ad72b9d9", + "sha256:ff7ae90e36c1715a54446e7872b76102baa5c63aa980917f4aa45e8c78d1a3ec" + ], + "index": "pypi", + "version": "==3.15.0" + }, + "pyflakes": { + "hashes": [ + "sha256:4579f67d887f804e67edb544428f264b7b24f435b263c4614f384135cea553d2", + "sha256:491feb020dca48ccc562a8c0cbe8df07ee13078df59813b83959cbdada312ea3" + ], + "markers": "python_version >= '3.6'", + "version": "==2.5.0" + }, + "pyjwt": { + "hashes": [ + "sha256:72d1d253f32dbd4f5c88eaf1fdc62f3a19f676ccbadb9dbc5d07e951b2b26daf", + "sha256:d42908208c699b3b973cbeb01a969ba6a96c821eefb1c5bfe4c390c01d67abba" + ], + "index": "pypi", + "version": "==2.4.0" + }, + "pylint": { + "hashes": [ + "sha256:4b124affc198b7f7c9b5f9ab690d85db48282a025ef9333f51d2d7281b92a6c3", + "sha256:4f3f7e869646b0bd63b3dfb79f3c0f28fc3d2d923ea220d52620fd625aed92b0" + ], + "index": "pypi", + "version": "==2.15.0" + }, + "pyopencl": { + "hashes": [ + "sha256:069e7eb1a223d88c13eafa54d6ae896fa892e75ba3d56ff2135a26107ef1142b", + "sha256:1490e6cdeaecba42854013c273685d65fd9102ee6dc6bc3bcb814e9e2b8179e5", + "sha256:15f7b3d29c9359e1e440e4f52f70de031f8d0d8d0f8de53a3bc01501b89360c0", + "sha256:1a2029b7fda6709eca077f618f997372c3d6f2780ad45512632b0d056e6305f9", + "sha256:25e87b4ccc0cc53487d445bea07ce9bdb478a335725df16986aead2ff65b68a4", + "sha256:2c9ad1cbc3f540afc52038851be8e06640aacfece051c89408bc3aece605a7ee", + "sha256:2df01c95ea9ae3dd66b277f0df47144cf7535a27b48a8d49fdd98e0583e368ce", + "sha256:316f59d0c40bfce4f6c160dbaf6501883b33880370bb1819f360dad747e52dfe", + "sha256:4836bc4619be967d6c28627adac151223037fdca056c4ab54da16b591f719347", + "sha256:4b53f7f3ed85ab671c8bfc61a0bbc5476725a7a5f51a94bba5512c3962b2d609", + "sha256:5304cb336af7316ae0650abb7467c076032635bfe4710b8df191612d245dca28", + "sha256:55e9302b8f0b1964c87b0fdab7b853aa2b2f10b4188f5b4618782d4380448c11", + "sha256:6032bef8a35f6df727a0b66e3c9faedb3f560318052848b28d2f72622cfbeace", + "sha256:6ec55934057e99461f684ccd293d87db59a452f5834c13ae36b19d31dfe38599", + "sha256:7176f96728be9b43024bd71704f60849cbfcf0fafd20270181b68ea4730ceb2d", + "sha256:730901d409d8251cd6e9dc59e6c518dff5cdb20a3a0b728344bfd2c707f28b64", + "sha256:75be43c7f33fb86f9d18b7b6f8e9081d8bd5b6331a90aec0d2cad3e81e72bc8f", + "sha256:7bef8e8bcfff574b481565390113ea0a37cf33fd2587ade7f2980f15e73f7b08", + "sha256:7ca9597877e1f8bdb4a49810988230f538b2d7aac389c33418a21cf4358f2fd4", + "sha256:814389b3eb9e6930cf43b984283c94a955edf20ec286402da5acfa503d3ae790", + "sha256:8efc3467454ce8c644f09029a3308496f9cb6e93ca5e8c08f6b79e7825da72c5", + "sha256:98bad7035f27b6de5c9268f52c1e10bffe3a2874994e862468a1792b699a4884", + "sha256:9bbfe94bb6e9d0458693183334e73c973e2fcba01568f42db15b453b926fb816", + "sha256:9d112a4426f5b356641c1312bf1004247dc4019e649502589b86333557203c01", + "sha256:a845779f505ed57b83f279307ae6307d886f3e41fb24dcf7889da27daa726118", + "sha256:aca3581f1a7f6b809b8cdc78b0e66587848b38b143bf2983e91ff8fb9a41bc8f", + "sha256:af5664b98140a29966c5fb12e9d29b85b6c6310efa97d82aee58310774917e8f", + "sha256:b85fa5ba1678dd40713587fd437787b6aa940000c2ddffa360884431be21723a", + "sha256:bcabfb5217ca8f8770f9c69298f79576080bb994b1883a99494b4c2668b04836", + "sha256:c00989bed1e7e5b32ad498fec3deb1c93403ab802cd99b7c78b9c692bd0910ef", + "sha256:d0ddc3b74ad1804eb3fe238dfa3b844b997e88b1ca5164a717c16b362b4f34c3", + "sha256:d8bb2eea4e960917e0a6132dedd34c8ec0b7a384f22713f775d50dbce154263a", + "sha256:db833ebb1e756969a8f851f15486598eb9e3fb27b0535c2a8193cc1c71455016", + "sha256:dc2d78cb5da0081ada1c263aaa773fd5479b3da5e2c421547bf7f3258d3239a5", + "sha256:dd2728e59ae088c900ed68f68d953476d0ff07189f182f917b74de2ac7b3972e", + "sha256:ea4eff6b922fa4ad2077ef90b3254d78597d050ada09bfbe74c22dd22d10c6ac", + "sha256:f8887d54e654598f3854472540b2eb228ac56b56a2491b95bdfac8f15be1c943" + ], + "index": "pypi", + "version": "==2022.1.6" + }, + "pyparsing": { + "hashes": [ + "sha256:2b020ecf7d21b687f219b71ecad3631f644a47f01403fa1d1036b0c6416d70fb", + "sha256:5026bae9a10eeaefb61dab2f09052b9f4307d44aee4eda64b309723d8d206bbc" + ], + "markers": "python_full_version >= '3.6.8'", + "version": "==3.0.9" + }, + "pyserial": { + "hashes": [ + "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", + "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0" + ], + "index": "pypi", + "version": "==3.5" + }, + "pyside2": { + "hashes": [ + "sha256:235240b6ec8206d9fdf0232472c6ef3241783d480425e5b54796f06e39ed23da", + "sha256:23886c6391ebd916e835fa1b5ae66938048504fd3a2934ae3189a96cd5ac0b46", + "sha256:439509e53cfe05abbf9a99422a2cbad086408b0f9bf5e6f642ff1b13b1f8b055", + "sha256:a9e2e6bbcb5d2ebb421e46e72244a0f4fe0943b2288115f80a863aacc1de1f06", + "sha256:af6b263fe63ba6dea7eaebae80aa7b291491fe66f4f0057c0aafe780cc83da9d", + "sha256:b5e1d92f26b0bbaefff67727ccbb2e1b577f2c0164b349b3d6e80febb4c5bde2" + ], + "index": "pypi", + "version": "==5.15.2.1" + }, + "python-dateutil": { + "hashes": [ + "sha256:0123cacc1627ae19ddf3c27a5de5bd67ee4586fbdd6440d9748f8abb483d3e86", + "sha256:961d03dc3453ebbc59dbdea9e4e11c5651520a876d0f4db161e8674aae935da9" + ], + "index": "pypi", + "version": "==2.8.2" + }, + "pytools": { + "hashes": [ + "sha256:4d62875e9a2ab2a24e393a9a8b799492f1a721bffa840af3807bfd42871dd1f4" + ], + "markers": "python_version ~= '3.6'", + "version": "==2022.1.12" + }, + "pyyaml": { + "hashes": [ + "sha256:0283c35a6a9fbf047493e3a0ce8d79ef5030852c51e9d911a27badfde0605293", + "sha256:055d937d65826939cb044fc8c9b08889e8c743fdc6a32b33e2390f66013e449b", + "sha256:07751360502caac1c067a8132d150cf3d61339af5691fe9e87803040dbc5db57", + "sha256:0b4624f379dab24d3725ffde76559cff63d9ec94e1736b556dacdfebe5ab6d4b", + "sha256:0ce82d761c532fe4ec3f87fc45688bdd3a4c1dc5e0b4a19814b9009a29baefd4", + "sha256:1e4747bc279b4f613a09eb64bba2ba602d8a6664c6ce6396a4d0cd413a50ce07", + "sha256:213c60cd50106436cc818accf5baa1aba61c0189ff610f64f4a3e8c6726218ba", + "sha256:231710d57adfd809ef5d34183b8ed1eeae3f76459c18fb4a0b373ad56bedcdd9", + "sha256:277a0ef2981ca40581a47093e9e2d13b3f1fbbeffae064c1d21bfceba2030287", + "sha256:2cd5df3de48857ed0544b34e2d40e9fac445930039f3cfe4bcc592a1f836d513", + "sha256:40527857252b61eacd1d9af500c3337ba8deb8fc298940291486c465c8b46ec0", + "sha256:473f9edb243cb1935ab5a084eb238d842fb8f404ed2193a915d1784b5a6b5fc0", + "sha256:48c346915c114f5fdb3ead70312bd042a953a8ce5c7106d5bfb1a5254e47da92", + "sha256:50602afada6d6cbfad699b0c7bb50d5ccffa7e46a3d738092afddc1f9758427f", + "sha256:68fb519c14306fec9720a2a5b45bc9f0c8d1b9c72adf45c37baedfcd949c35a2", + "sha256:77f396e6ef4c73fdc33a9157446466f1cff553d979bd00ecb64385760c6babdc", + "sha256:819b3830a1543db06c4d4b865e70ded25be52a2e0631ccd2f6a47a2822f2fd7c", + "sha256:897b80890765f037df3403d22bab41627ca8811ae55e9a722fd0392850ec4d86", + "sha256:98c4d36e99714e55cfbaaee6dd5badbc9a1ec339ebfc3b1f52e293aee6bb71a4", + "sha256:9df7ed3b3d2e0ecfe09e14741b857df43adb5a3ddadc919a2d94fbdf78fea53c", + "sha256:9fa600030013c4de8165339db93d182b9431076eb98eb40ee068700c9c813e34", + "sha256:a80a78046a72361de73f8f395f1f1e49f956c6be882eed58505a15f3e430962b", + "sha256:b3d267842bf12586ba6c734f89d1f5b871df0273157918b0ccefa29deb05c21c", + "sha256:b5b9eccad747aabaaffbc6064800670f0c297e52c12754eb1d976c57e4f74dcb", + "sha256:c5687b8d43cf58545ade1fe3e055f70eac7a5a1a0bf42824308d868289a95737", + "sha256:cba8c411ef271aa037d7357a2bc8f9ee8b58b9965831d9e51baf703280dc73d3", + "sha256:d15a181d1ecd0d4270dc32edb46f7cb7733c7c508857278d3d378d14d606db2d", + "sha256:d4db7c7aef085872ef65a8fd7d6d09a14ae91f691dec3e87ee5ee0539d516f53", + "sha256:d4eccecf9adf6fbcc6861a38015c2a64f38b9d94838ac1810a9023a0609e1b78", + "sha256:d67d839ede4ed1b28a4e8909735fc992a923cdb84e618544973d7dfc71540803", + "sha256:daf496c58a8c52083df09b80c860005194014c3698698d1a57cbcfa182142a3a", + "sha256:e61ceaab6f49fb8bdfaa0f92c4b57bcfbea54c09277b1b4f7ac376bfb7a7c174", + "sha256:f84fbc98b019fef2ee9a1cb3ce93e3187a6df0b2538a651bfb890254ba9f90b5" + ], + "index": "pypi", + "version": "==6.0" + }, + "pyzmq": { + "hashes": [ + "sha256:022cf5ea7bcaa8a06a03c2706e0ae66904b6138b2155577cd34c64bc7cc637ab", + "sha256:044447ae4b2016a6b8697571fd633f799f860b19b76c4a2fd9b1140d52ee6745", + "sha256:07ed8aaf7ffe150af873269690cc654ffeca7491f62aae0f3821baa181f8d5fe", + "sha256:10d1910ec381b851aeb024a042a13db178cb1edf125e76a4e9d2548ad103aadb", + "sha256:12e62ff0d5223ec09b597ab6d73858b9f64a51221399f3cb08aa495e1dff7935", + "sha256:1f368a82b29f80071781b20663c0fc0c8f6b13273f9f5abe1526af939534f90f", + "sha256:20bafc4095eab00f41a510579363a3f5e1f5c69d7ee10f1d88895c4df0259183", + "sha256:2141e6798d5981be04c08996d27962086a1aa3ea536fe9cf7e89817fd4523f86", + "sha256:23e708fbfdf4ee3107422b69ca65da1b9f056b431fc0888096a8c1d6cd908e8f", + "sha256:28dbdb90b2f6b131f8f10e6081012e4e25234213433420e67e0c1162de537113", + "sha256:29b74774a0bfd3c4d98ac853f0bdca55bd9ec89d5b0def5486407cca54472ef8", + "sha256:2b381aa867ece7d0a82f30a0c7f3d4387b7cf2e0697e33efaa5bed6c5784abcd", + "sha256:2f67b63f53c6994d601404fd1a329e6d940ac3dd1d92946a93b2b9c70df67b9f", + "sha256:342ca3077f47ec2ee41b9825142b614e03e026347167cbc72a59b618c4f6106c", + "sha256:35e635343ff367f697d00fa1484262bb68e36bc74c9b80737eac5a1e04c4e1b1", + "sha256:385609812eafd9970c3752c51f2f6c4f224807e3e441bcfd8c8273877d00c8a8", + "sha256:38e106b64bad744fe469dc3dd864f2764d66399178c1bf39d45294cc7980f14f", + "sha256:39dd252b683816935702825e5bf775df16090619ced9bb4ba68c2d0b6f0c9b18", + "sha256:407f909c4e8fde62fbdad9ebd448319792258cc0550c2815567a4d9d8d9e6d18", + "sha256:415ff62ac525d9add1e3550430a09b9928d2d24a20cc4ce809e67caac41219ab", + "sha256:4805af9614b0b41b7e57d17673459facf85604dac502a5a9244f6e8c9a4de658", + "sha256:48400b96788cdaca647021bf19a9cd668384f46e4d9c55cf045bdd17f65299c8", + "sha256:49d30ba7074f469e8167917abf9eb854c6503ae10153034a6d4df33618f1db5f", + "sha256:4bb798bef181648827019001f6be43e1c48b34b477763b37a8d27d8c06d197b8", + "sha256:4d6f110c56f7d5b4d64dde3a382ae61b6d48174e30742859d8e971b18b6c9e5c", + "sha256:55568a020ad2cae9ae36da6058e7ca332a56df968f601cbdb7cf6efb2a77579a", + "sha256:565bd5ab81f6964fc4067ccf2e00877ad0fa917308975694bbb54378389215f8", + "sha256:5c558b50402fca1acc94329c5d8f12aa429738904a5cfb32b9ed3c61235221bb", + "sha256:5e05492be125dce279721d6b54fd1b956546ecc4bcdfcf8e7b4c413bc0874c10", + "sha256:624fd38071a817644acdae075b92a23ea0bdd126a58148288e8284d23ec361ce", + "sha256:650389bbfca73955b262b2230423d89992f38ec48033307ae80e700eaa2fbb63", + "sha256:67975a9e1237b9ccc78f457bef17691bbdd2055a9d26e81ee914ba376846d0ce", + "sha256:6b1e79bba24f6df1712e3188d5c32c480d8eda03e8ecff44dc8ecb0805fa62f3", + "sha256:6fd5d0d50cbcf4bc376861529a907bed026a4cbe8c22a500ff8243231ef02433", + "sha256:71b32a1e827bdcbf73750e60370d3b07685816ff3d8695f450f0f8c3226503f8", + "sha256:794871988c34727c7f79bdfe2546e6854ae1fa2e1feb382784f23a9c6c63ecb3", + "sha256:79a87831b47a9f6161ad23fa5e89d5469dc585abc49f90b9b07fea8905ae1234", + "sha256:7e0113d70b095339e99bb522fe7294f5ae6a7f3b2b8f52f659469a74b5cc7661", + "sha256:84678153432241bcdca2210cf4ff83560b200556867aea913ffbb960f5d5f340", + "sha256:8a68f57b7a3f7b6b52ada79876be1efb97c8c0952423436e84d70cc139f16f0d", + "sha256:8c02a0cd39dc01659b3d6cb70bb3a41aebd9885fd78239acdd8d9c91351c4568", + "sha256:8c842109d31a9281d678f668629241c405928afbebd913c48a5a8e7aee61f63d", + "sha256:8dc66f109a245653b19df0f44a5af7a3f14cb8ad6c780ead506158a057bd36ce", + "sha256:90d88f9d9a2ae6cfb1dc4ea2d1710cdf6456bc1b9a06dd1bb485c5d298f2517e", + "sha256:9269fbfe3a4eb2009199120861c4571ef1655fdf6951c3e7f233567c94e8c602", + "sha256:929d548b74c0f82f7f95b54e4a43f9e4ce2523cfb8a54d3f7141e45652304b2a", + "sha256:99a5a77a10863493a1ee8dece02578c6b32025fb3afff91b40476bc489e81648", + "sha256:9a39ddb0431a68954bd318b923230fa5b649c9c62b0e8340388820c5f1b15bd2", + "sha256:9d0ab2936085c85a1fc6f9fd8f89d5235ae99b051e90ec5baa5e73ad44346e1f", + "sha256:9e5bf6e7239fc9687239de7a283aa8b801ab85371116045b33ae20132a1325d6", + "sha256:a0f09d85c45f58aa8e715b42f8b26beba68b3b63a8f7049113478aca26efbc30", + "sha256:a114992a193577cb62233abf8cb2832970f9975805a64740e325d2f895e7f85a", + "sha256:a3fd44b5046d247e7f0f1660bcafe7b5fb0db55d0934c05dd57dda9e1f823ce7", + "sha256:ad28ddb40db8e450d7d4bf8a1d765d3f87b63b10e7e9a825a3c130c6371a8c03", + "sha256:aecd6ceaccc4b594e0092d6513ef3f1c0fa678dd89f86bb8ff1a47014b8fca35", + "sha256:b815991c7d024bf461f358ad871f2be1135576274caed5749c4828859e40354e", + "sha256:b861db65f6b8906c8d6db51dde2448f266f0c66bf28db2c37aea50f58a849859", + "sha256:c3ebf1668664d20c8f7d468955f18379b7d1f7bc8946b13243d050fa3888c7ff", + "sha256:c56b1a62a1fb87565343c57b6743fd5da6e138b8c6562361d7d9b5ce4acf399a", + "sha256:c780acddd2934c6831ff832ecbf78a45a7b62d4eb216480f863854a8b7d54fa7", + "sha256:c890309296f53f9aa32ffcfc51d805705e1982bffd27c9692a8f1e1b8de279f4", + "sha256:c9cfaf530e6a7ff65f0afe275e99f983f68b54dfb23ea401f0bc297a632766b6", + "sha256:d904f6595acfaaf99a1a61881fea068500c40374d263e5e073aa4005e5f9c28a", + "sha256:e06747014a5ad1b28cebf5bc1ddcdaccfb44e9b441d35e6feb1286c8a72e54be", + "sha256:e1fe30bcd5aea5948c42685fad910cd285eacb2518ea4dc6c170d6b535bee95d", + "sha256:e753eee6d3b93c5354e8ba0a1d62956ee49355f0a36e00570823ef64e66183f5", + "sha256:ec9803aca9491fd6f0d853d2a6147f19f8deaaa23b1b713d05c5d09e56ea7142", + "sha256:efb9e38b2a590282704269585de7eb33bf43dc294cad092e1b172e23d4c217e5", + "sha256:f07016e3cf088dbfc6e7c5a7b3f540db5c23b0190d539e4fd3e2b5e6beffa4b5", + "sha256:f392cbea531b7142d1958c0d4a0c9c8d760dc451e5848d8dd3387804d3e3e62c", + "sha256:f619fd38fc2641abfb53cca719c165182500600b82c695cc548a0f05f764be05", + "sha256:fefdf9b685fda4141b95ebec975946076a5e0723ff70b037032b2085c5317684", + "sha256:ffc6b1623d0f9affb351db4ca61f432dca3628a5ee015f9bf2bfbe9c6836881c" + ], + "index": "pypi", + "version": "==23.2.1" + }, + "requests": { + "hashes": [ + "sha256:7c5599b102feddaa661c826c56ab4fee28bfd17f5abca1ebbe3e7f19d7c97983", + "sha256:8fefa2a1a1365bf5520aac41836fbee479da67864514bdb821f31ce07ce65349" + ], + "index": "pypi", + "version": "==2.28.1" + }, + "scons": { + "hashes": [ + "sha256:7703c4e9d2200b4854a31800c1dbd4587e1fa86e75f58795c740bcfa7eca7eaa", + "sha256:cbbd73b83cf85f1aaaf986370359de1bbfd3af97a765cb3554734f6dcec734e1" + ], + "index": "pypi", + "version": "==4.4.0" + }, + "sentry-sdk": { + "hashes": [ + "sha256:2d7ec7bc88ebbdf2c4b6b2650b3257893d386325a96c9b723adcd31033469b63", + "sha256:b4b41f90951ed83e7b4c176eef021b19ecba39da5b73aca106c97a9b7e279a90" + ], + "index": "pypi", + "version": "==1.9.5" + }, + "setproctitle": { + "hashes": [ + "sha256:1c5d5dad7c28bdd1ec4187d818e43796f58a845aa892bb4481587010dc4d362b", + "sha256:1c8d9650154afaa86a44ff195b7b10d683c73509d085339d174e394a22cccbb9", + "sha256:1f0cde41857a644b7353a0060b5f94f7ba7cf593ebde5a1094da1be581ac9a31", + "sha256:1f29b75e86260b0ab59adb12661ef9f113d2f93a59951373eb6d68a852b13e83", + "sha256:1fa1a0fbee72b47dc339c87c890d3c03a72ea65c061ade3204f285582f2da30f", + "sha256:1ff863a20d1ff6ba2c24e22436a3daa3cd80be1dfb26891aae73f61b54b04aca", + "sha256:265ecbe2c6eafe82e104f994ddd7c811520acdd0647b73f65c24f51374cf9494", + "sha256:288943dec88e178bb2fd868adf491197cc0fc8b6810416b1c6775e686bab87fe", + "sha256:2e3ac25bfc4a0f29d2409650c7532d5ddfdbf29f16f8a256fc31c47d0dc05172", + "sha256:2fbd8187948284293f43533c150cd69a0e4192c83c377da837dbcd29f6b83084", + "sha256:4058564195b975ddc3f0462375c533cce310ccdd41b80ac9aed641c296c3eff4", + "sha256:4749a2b0c9ac52f864d13cee94546606f92b981b50e46226f7f830a56a9dc8e1", + "sha256:4d8938249a7cea45ab7e1e48b77685d0f2bab1ebfa9dde23e94ab97968996a7c", + "sha256:5194b4969f82ea842a4f6af2f82cd16ebdc3f1771fb2771796e6add9835c1973", + "sha256:55ce1e9925ce1765865442ede9dca0ba9bde10593fcd570b1f0fa25d3ec6b31c", + "sha256:589be87172b238f839e19f146b9ea47c71e413e951ef0dc6db4218ddacf3c202", + "sha256:5b932c3041aa924163f4aab970c2f0e6b4d9d773f4d50326e0ea1cd69240e5c5", + "sha256:5fb4f769c02f63fac90989711a3fee83919f47ae9afd4758ced5d86596318c65", + "sha256:630f6fe5e24a619ccf970c78e084319ee8be5be253ecc9b5b216b0f474f5ef18", + "sha256:65d884e22037b23fa25b2baf1a3316602ed5c5971eb3e9d771a38c3a69ce6e13", + "sha256:6c877691b90026670e5a70adfbcc735460a9f4c274d35ec5e8a43ce3f8443005", + "sha256:710e16fa3bade3b026907e4a5e841124983620046166f355bbb84be364bf2a02", + "sha256:7a55fe05f15c10e8c705038777656fe45e3bd676d49ad9ac8370b75c66dd7cd7", + "sha256:7aa0aac1711fadffc1d51e9d00a3bea61f68443d6ac0241a224e4d622489d665", + "sha256:7f0bed90a216ef28b9d227d8d73e28a8c9b88c0f48a082d13ab3fa83c581488f", + "sha256:7f2719a398e1a2c01c2a63bf30377a34d0b6ef61946ab9cf4d550733af8f1ef1", + "sha256:7fe9df7aeb8c64db6c34fc3b13271a363475d77bc157d3f00275a53910cb1989", + "sha256:8ff3c8cb26afaed25e8bca7b9dd0c1e36de71f35a3a0706b5c0d5172587a3827", + "sha256:9124bedd8006b0e04d4e8a71a0945da9b67e7a4ab88fdad7b1440dc5b6122c42", + "sha256:92c626edc66169a1b09e9541b9c0c9f10488447d8a2b1d87c8f0672e771bc927", + "sha256:a149a5f7f2c5a065d4e63cb0d7a4b6d3b66e6e80f12e3f8827c4f63974cbf122", + "sha256:a47d97a75fd2d10c37410b180f67a5835cb1d8fdea2648fd7f359d4277f180b9", + "sha256:a499fff50387c1520c085a07578a000123f519e5f3eee61dd68e1d301659651f", + "sha256:ab45146c71ca6592c9cc8b354a2cc9cc4843c33efcbe1d245d7d37ce9696552d", + "sha256:b2c9cb2705fc84cb8798f1ba74194f4c080aaef19d9dae843591c09b97678e98", + "sha256:b34baef93bfb20a8ecb930e395ccd2ae3268050d8cf4fe187de5e2bd806fd796", + "sha256:b617f12c9be61e8f4b2857be4a4319754756845dbbbd9c3718f468bbb1e17bcb", + "sha256:b9fb97907c830d260fa0658ed58afd48a86b2b88aac521135c352ff7fd3477fd", + "sha256:bae283e85fc084b18ffeb92e061ff7ac5af9e183c9d1345c93e178c3e5069cbe", + "sha256:c2c46200656280a064073447ebd363937562debef329482fd7e570c8d498f806", + "sha256:c8a09d570b39517de10ee5b718730e171251ce63bbb890c430c725c8c53d4484", + "sha256:c91b9bc8985d00239f7dc08a49927a7ca1ca8a6af2c3890feec3ed9665b6f91e", + "sha256:dad42e676c5261eb50fdb16bdf3e2771cf8f99a79ef69ba88729aeb3472d8575", + "sha256:de3a540cd1817ede31f530d20e6a4935bbc1b145fd8f8cf393903b1e02f1ae76", + "sha256:e00c9d5c541a2713ba0e657e0303bf96ddddc412ef4761676adc35df35d7c246", + "sha256:e1aafc91cbdacc9e5fe712c52077369168e6b6c346f3a9d51bf600b53eae56bb", + "sha256:e425be62524dc0c593985da794ee73eb8a17abb10fe692ee43bb39e201d7a099", + "sha256:e43f315c68aa61cbdef522a2272c5a5b9b8fd03c301d3167b5e1343ef50c676c", + "sha256:e49ae693306d7624015f31cb3e82708916759d592c2e5f72a35c8f4cc8aef258", + "sha256:e5c50e164cd2459bc5137c15288a9ef57160fd5cbf293265ea3c45efe7870865", + "sha256:e8579a43eafd246e285eb3a5b939e7158073d5087aacdd2308f23200eac2458b", + "sha256:e85e50b9c67854f89635a86247412f3ad66b132a4d8534ac017547197c88f27d", + "sha256:f0452282258dfcc01697026a8841258dd2057c4438b43914b611bccbcd048f10", + "sha256:f4bfc89bd33ebb8e4c0e9846a09b1f5a4a86f5cb7a317e75cc42fee1131b4f4f", + "sha256:fa2f50678f04fda7a75d0fe5dd02bbdd3b13cbe6ed4cf626e4472a7ccf47ae94", + "sha256:faec934cfe5fd6ac1151c02e67156c3f526e82f96b24d550b5d51efa4a5527c6", + "sha256:fcd3cf4286a60fdc95451d8d14e0389a6b4f5cebe02c7f2609325eb016535963", + "sha256:fe8a988c7220c002c45347430993830666e55bc350179d91fcee0feafe64e1d4", + "sha256:fed18e44711c5af4b681c2b3b18f85e6f0f1b2370a28854c645d636d5305ccd8", + "sha256:ffc61a388a5834a97953d6444a2888c24a05f2e333f9ed49f977a87bb1ad4761" + ], + "index": "pypi", + "version": "==1.3.2" + }, + "setuptools": { + "hashes": [ + "sha256:2e24e0bec025f035a2e72cdd1961119f557d78ad331bb00ff82efb2ab8da8e82", + "sha256:7732871f4f7fa58fb6bdcaeadb0161b2bd046c85905dbaa066bdcbcc81953b57" + ], + "markers": "python_version >= '3.7'", + "version": "==65.3.0" + }, + "shiboken2": { + "hashes": [ + "sha256:63debfcc531b6a2b4985aa9b71433d2ad3bac542acffc729cc0ecaa3854390c0", + "sha256:87079c07587859a525b9800d60b1be971338ce9b371d6ead81f15ee5a46d448b", + "sha256:a0d0fdeb12b72c8af349b9642ccc67afd783dca449309f45e78cda50272fd6b7", + "sha256:eb0da44b6fa60c6bd317b8f219e500595e94e0322b33ec5b4e9f406bedaee555", + "sha256:f890f5611ab8f48b88cfecb716da2ac55aef99e2923198cefcf781842888ea65", + "sha256:ffd3d0ec3d508e592d7ee3885d27fee1f279a49989f734eb130f46d9501273a9" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4' and python_version < '3.11'", + "version": "==5.15.2.1" + }, + "six": { + "hashes": [ + "sha256:1e61c37477a1626458e36f7b1d82aa5c9b094fa4802892072e49de9c60c4c926", + "sha256:8abb2f1d86890a2dfb989f9a77cfcfd3e47c2a354b01111771326f8aa26e0254" + ], + "index": "pypi", + "version": "==1.16.0" + }, + "smbus2": { + "hashes": [ + "sha256:50f3c78e436b42a9583948be06961a8104cf020ebad5edfaaf2657528bef0818", + "sha256:634541ed794068a822fe7499f1577468b9d4641b68dd9bfb6d0eb7270f4d2a32" + ], + "index": "pypi", + "version": "==0.4.2" + }, + "sympy": { + "hashes": [ + "sha256:1fe96b4c56bb7a7630cdf150a6cd98bc97a79e6be233e30502aba1cf54dee33d", + "sha256:b53069f5f30e4a4690b57cdb8e3d0d9065fff42627239db718214f804e442481" + ], + "index": "pypi", + "version": "==1.11" + }, + "timezonefinder": { + "hashes": [ + "sha256:2791ad459b85c110226057cb5ebdd6503f4fb0a33cc4f76fb93e98ed545edd68", + "sha256:406bea77a7baec5e2b1c2b4793ff8f40c174b6d8e894631e60864d956139afef" + ], + "index": "pypi", + "version": "==6.1.1" + }, + "tomli": { + "hashes": [ + "sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc", + "sha256:de526c12914f0c550d15924c62d72abc48d6fe7364aa87328337a31007fe8a4f" + ], + "markers": "python_version < '3.11'", + "version": "==2.0.1" + }, + "tomlkit": { + "hashes": [ + "sha256:25d4e2e446c453be6360c67ddfb88838cfc42026322770ba13d1fbd403a93a5c", + "sha256:3235a9010fae54323e727c3ac06fb720752fe6635b3426e379daec60fbd44a83" + ], + "markers": "python_version >= '3.6' and python_version < '4.0'", + "version": "==0.11.4" + }, + "tqdm": { + "hashes": [ + "sha256:40be55d30e200777a307a7585aee69e4eabb46b4ec6a4b4a5f2d9f11e7d5408d", + "sha256:74a2cdefe14d11442cedf3ba4e21a3b84ff9a2dbdc6cfae2c34addb2a14a5ea6" + ], + "index": "pypi", + "version": "==4.64.0" + }, + "typing-extensions": { + "hashes": [ + "sha256:25642c956049920a5aa49edcdd6ab1e06d7e5d467fc00e0506c44ac86fbfca02", + "sha256:e6d2677a32f47fc7eb2795db1dd15c1f34eff616bcaf2cfb5e997f854fa1c4a6" + ], + "markers": "python_version < '3.10'", + "version": "==4.3.0" + }, + "urllib3": { + "hashes": [ + "sha256:3fa96cf423e6987997fc326ae8df396db2a8b7c667747d47ddd8ecba91f4a74e", + "sha256:b930dd878d5a8afb066a637fbb35144fe7901e3b209d1cd4f524bd0e9deee997" + ], + "index": "pypi", + "version": "==1.26.12" + }, + "utm": { + "hashes": [ + "sha256:3c9a3650e98bb6eecec535418d0dfd4db8f88c8ceaca112a0ff0787e116566e2" + ], + "index": "pypi", + "version": "==0.7.0" + }, + "websocket-client": { + "hashes": [ + "sha256:33ad3cf0aef4270b95d10a5a66b670a66be1f5ccf10ce390b3644f9eddfdca9d", + "sha256:79d730c9776f4f112f33b10b78c8d209f23b5806d9a783e296b3813fc5add2f1" + ], + "index": "pypi", + "version": "==1.4.0" + }, + "werkzeug": { + "hashes": [ + "sha256:7ea2d48322cc7c0f8b3a215ed73eabd7b5d75d0b50e31ab006286ccff9e00b8f", + "sha256:f979ab81f58d7318e064e99c4506445d60135ac5cd2e177a2de0089bfd4c9bd5" + ], + "markers": "python_version >= '3.7'", + "version": "==2.2.2" + }, + "wrapt": { + "hashes": [ + "sha256:00b6d4ea20a906c0ca56d84f93065b398ab74b927a7a3dbd470f6fc503f95dc3", + "sha256:01c205616a89d09827986bc4e859bcabd64f5a0662a7fe95e0d359424e0e071b", + "sha256:02b41b633c6261feff8ddd8d11c711df6842aba629fdd3da10249a53211a72c4", + "sha256:07f7a7d0f388028b2df1d916e94bbb40624c59b48ecc6cbc232546706fac74c2", + "sha256:11871514607b15cfeb87c547a49bca19fde402f32e2b1c24a632506c0a756656", + "sha256:1b376b3f4896e7930f1f772ac4b064ac12598d1c38d04907e696cc4d794b43d3", + "sha256:21ac0156c4b089b330b7666db40feee30a5d52634cc4560e1905d6529a3897ff", + "sha256:257fd78c513e0fb5cdbe058c27a0624c9884e735bbd131935fd49e9fe719d310", + "sha256:2b39d38039a1fdad98c87279b48bc5dce2c0ca0d73483b12cb72aa9609278e8a", + "sha256:2cf71233a0ed05ccdabe209c606fe0bac7379fdcf687f39b944420d2a09fdb57", + "sha256:2fe803deacd09a233e4762a1adcea5db5d31e6be577a43352936179d14d90069", + "sha256:3232822c7d98d23895ccc443bbdf57c7412c5a65996c30442ebe6ed3df335383", + "sha256:34aa51c45f28ba7f12accd624225e2b1e5a3a45206aa191f6f9aac931d9d56fe", + "sha256:36f582d0c6bc99d5f39cd3ac2a9062e57f3cf606ade29a0a0d6b323462f4dd87", + "sha256:380a85cf89e0e69b7cfbe2ea9f765f004ff419f34194018a6827ac0e3edfed4d", + "sha256:40e7bc81c9e2b2734ea4bc1aceb8a8f0ceaac7c5299bc5d69e37c44d9081d43b", + "sha256:43ca3bbbe97af00f49efb06e352eae40434ca9d915906f77def219b88e85d907", + "sha256:4fcc4649dc762cddacd193e6b55bc02edca674067f5f98166d7713b193932b7f", + "sha256:5a0f54ce2c092aaf439813735584b9537cad479575a09892b8352fea5e988dc0", + "sha256:5a9a0d155deafd9448baff28c08e150d9b24ff010e899311ddd63c45c2445e28", + "sha256:5b02d65b9ccf0ef6c34cba6cf5bf2aab1bb2f49c6090bafeecc9cd81ad4ea1c1", + "sha256:60db23fa423575eeb65ea430cee741acb7c26a1365d103f7b0f6ec412b893853", + "sha256:642c2e7a804fcf18c222e1060df25fc210b9c58db7c91416fb055897fc27e8cc", + "sha256:6a9a25751acb379b466ff6be78a315e2b439d4c94c1e99cb7266d40a537995d3", + "sha256:6b1a564e6cb69922c7fe3a678b9f9a3c54e72b469875aa8018f18b4d1dd1adf3", + "sha256:6d323e1554b3d22cfc03cd3243b5bb815a51f5249fdcbb86fda4bf62bab9e164", + "sha256:6e743de5e9c3d1b7185870f480587b75b1cb604832e380d64f9504a0535912d1", + "sha256:709fe01086a55cf79d20f741f39325018f4df051ef39fe921b1ebe780a66184c", + "sha256:7b7c050ae976e286906dd3f26009e117eb000fb2cf3533398c5ad9ccc86867b1", + "sha256:7d2872609603cb35ca513d7404a94d6d608fc13211563571117046c9d2bcc3d7", + "sha256:7ef58fb89674095bfc57c4069e95d7a31cfdc0939e2a579882ac7d55aadfd2a1", + "sha256:80bb5c256f1415f747011dc3604b59bc1f91c6e7150bd7db03b19170ee06b320", + "sha256:81b19725065dcb43df02b37e03278c011a09e49757287dca60c5aecdd5a0b8ed", + "sha256:833b58d5d0b7e5b9832869f039203389ac7cbf01765639c7309fd50ef619e0b1", + "sha256:88bd7b6bd70a5b6803c1abf6bca012f7ed963e58c68d76ee20b9d751c74a3248", + "sha256:8ad85f7f4e20964db4daadcab70b47ab05c7c1cf2a7c1e51087bfaa83831854c", + "sha256:8c0ce1e99116d5ab21355d8ebe53d9460366704ea38ae4d9f6933188f327b456", + "sha256:8d649d616e5c6a678b26d15ece345354f7c2286acd6db868e65fcc5ff7c24a77", + "sha256:903500616422a40a98a5a3c4ff4ed9d0066f3b4c951fa286018ecdf0750194ef", + "sha256:9736af4641846491aedb3c3f56b9bc5568d92b0692303b5a305301a95dfd38b1", + "sha256:988635d122aaf2bdcef9e795435662bcd65b02f4f4c1ae37fbee7401c440b3a7", + "sha256:9cca3c2cdadb362116235fdbd411735de4328c61425b0aa9f872fd76d02c4e86", + "sha256:9e0fd32e0148dd5dea6af5fee42beb949098564cc23211a88d799e434255a1f4", + "sha256:9f3e6f9e05148ff90002b884fbc2a86bd303ae847e472f44ecc06c2cd2fcdb2d", + "sha256:a85d2b46be66a71bedde836d9e41859879cc54a2a04fad1191eb50c2066f6e9d", + "sha256:a9a52172be0b5aae932bef82a79ec0a0ce87288c7d132946d645eba03f0ad8a8", + "sha256:aa31fdcc33fef9eb2552cbcbfee7773d5a6792c137b359e82879c101e98584c5", + "sha256:b014c23646a467558be7da3d6b9fa409b2c567d2110599b7cf9a0c5992b3b471", + "sha256:b21bb4c09ffabfa0e85e3a6b623e19b80e7acd709b9f91452b8297ace2a8ab00", + "sha256:b5901a312f4d14c59918c221323068fad0540e34324925c8475263841dbdfe68", + "sha256:b9b7a708dd92306328117d8c4b62e2194d00c365f18eff11a9b53c6f923b01e3", + "sha256:d1967f46ea8f2db647c786e78d8cc7e4313dbd1b0aca360592d8027b8508e24d", + "sha256:d52a25136894c63de15a35bc0bdc5adb4b0e173b9c0d07a2be9d3ca64a332735", + "sha256:d77c85fedff92cf788face9bfa3ebaa364448ebb1d765302e9af11bf449ca36d", + "sha256:d79d7d5dc8a32b7093e81e97dad755127ff77bcc899e845f41bf71747af0c569", + "sha256:dbcda74c67263139358f4d188ae5faae95c30929281bc6866d00573783c422b7", + "sha256:ddaea91abf8b0d13443f6dac52e89051a5063c7d014710dcb4d4abb2ff811a59", + "sha256:dee0ce50c6a2dd9056c20db781e9c1cfd33e77d2d569f5d1d9321c641bb903d5", + "sha256:dee60e1de1898bde3b238f18340eec6148986da0455d8ba7848d50470a7a32fb", + "sha256:e2f83e18fe2f4c9e7db597e988f72712c0c3676d337d8b101f6758107c42425b", + "sha256:e3fb1677c720409d5f671e39bac6c9e0e422584e5f518bfd50aa4cbbea02433f", + "sha256:ee2b1b1769f6707a8a445162ea16dddf74285c3964f605877a20e38545c3c462", + "sha256:ee6acae74a2b91865910eef5e7de37dc6895ad96fa23603d1d27ea69df545015", + "sha256:ef3f72c9666bba2bab70d2a8b79f2c6d2c1a42a7f7e2b0ec83bb2f9e383950af" + ], + "markers": "python_version < '3.11'", + "version": "==1.14.1" + }, + "zipp": { + "hashes": [ + "sha256:05b45f1ee8f807d0cc928485ca40a07cb491cf092ff587c0df9cb1fd154848d2", + "sha256:47c40d7fe183a6f21403a199b3e4192cca5774656965b0a4988ad2f8feb5f009" + ], + "markers": "python_version >= '3.7'", + "version": "==3.8.1" + } + }, + "develop": { + "alabaster": { + "hashes": [ + "sha256:446438bdcca0e05bd45ea2de1668c1d9b032e1a9154c2c259092d77031ddd359", + "sha256:a661d72d58e6ea8a57f7a86e37d86716863ee5e92788398526d58b26a4e4dc02" + ], + "version": "==0.7.12" + }, + "attrs": { + "hashes": [ + "sha256:29adc2665447e5191d0e7c568fde78b21f9672d344281d0c6e1ab085429b22b6", + "sha256:86efa402f67bf2df34f51a335487cf46b1ec130d02b8d39fd248abfd30da551c" + ], + "markers": "python_version >= '3.5'", + "version": "==22.1.0" + }, + "av": { + "hashes": [ + "sha256:0340cc68f3d222bc9438b4fde12e3d68f949eeb5de9e090db182f2cb06e23d53", + "sha256:0d9cad890e6eccf2697b1c932761bee6f5e1e7faf9b8c03cf10f18f697d29ba3", + "sha256:109526152e658921731018c50a05db802e7c9f3eb04a7a5fcbd8321fb3b73134", + "sha256:17a7b6617d4201214f3dd5f628041b4fe56f4244dcd48399ed8d0cf324ca24d1", + "sha256:1cbf031f650f89943023eef80e8b2c99588bf9ba26ffef8b3b54bef7102ea3dc", + "sha256:1e2a50a146b3f33a24ea059af913ad368dbb61ed494234debe140a09f1076950", + "sha256:24dac414eafcc20423f2ec7e873706489433648f0e9af08a537996880aa55979", + "sha256:28d85b8476f7d8fb18e3af9bd6d22bb292f1d810a20f8910fe481f648372e798", + "sha256:29373aa86a055a07eebb14d253cb202033f63ba98c5a4b0233d6d4c07fc7a292", + "sha256:2b46b54ddf64409d4455f408b5970f8494c27c0273181b81c2f7d5072c9afb55", + "sha256:343b11d9b03e71da29f3ce56bc0a6c2d40aba448225dcf8296ab53c10527fff0", + "sha256:3ea180bfd89bc0a9e392c32de204cf4e51648aefe2f375d430ce39c04e3ed625", + "sha256:3facfe8dc5ba7f9ec7fd7e4c0466e577b84d5f2a1671428f7e28ebcd2cb0ccd3", + "sha256:45816a39255b39e514a72125e0b6e29eb24fe0994bef3f4f87f3b9d9960b3fa8", + "sha256:48819e401cea5be57bd03299d8e5f700082c411746d1ac23eb5e5a931d3d3ced", + "sha256:49481c2d5bc296f451ccd3f93b1cb692d7f58a804b794b99c8b7743e058cae71", + "sha256:587dd492a2ef3eb20324a0a8d67e6a2e686845d8c1dfdcad058377ac84268d67", + "sha256:6a1c2c1dcc1947473ea1e2cbbf50549e2655e49e08bdd2a6427a97276d7a92c8", + "sha256:6b01fbe8047da81892f8bd2aee5690f00465bf5215e3f6b6372863ac9408d75f", + "sha256:7a5dc26b9df656bed5e1bdeaf8bcc4ff4a2e009ee90b3b3024a86cf8476b2cbf", + "sha256:8671fa01648ce7aac76e71816c2421ddb1939bf706e2e14684608ab1ce9dbbbb", + "sha256:9b0f124e335561cf4de6b7cdc461283c5eba5f05cccb1a5e1b8ceb1cd15393d8", + "sha256:a616a6eb46b62f41ff69569cafe12b0005a6dd14389f597dee115340336a910f", + "sha256:a6a35e6028dec677caed97d19bfab3b66182690d43b0ec3c355778d740ce0509", + "sha256:a9983bc45dab65d2416d2f8a63785caa076a751590823fc8c199617d0dbad390", + "sha256:ab90aa3ac2cbdf1f22087fc0fa439f643e96979f169ecfa1d496e114c3c3a8b3", + "sha256:af951271d998f736a20e54fbc0d944f263db7b17592f11cd489947957bf46aa8", + "sha256:b07b91f534ee7a096068149404c67c3c0e5b4c373580b016151de0fcb440cd3f", + "sha256:b6be9388618af978304b56d1cf6b74c811db4f220dd320da5bd79640aa443358", + "sha256:ba3d9e3fe23fd8a14e810f291386225acbdef1c7e5376cc10c8e85c2d4280771", + "sha256:bf941896b4c800ee707211c802f94c6e0b4642d3000e25d1974d0b6032af4f66", + "sha256:d080f34ddfde551de3a5f2d0d06d7518718e3115af81e56182e158cc03111662", + "sha256:d380925732e7497c1c11545107eabe1f498cab214f49f32d1b5d6abe01a2b36b", + "sha256:d6a3c9126d658029b151484b48c656b73af1b145b143c50de5b8b983ac60e095", + "sha256:d730f3ed30eda46d06849bd71ad87d480cf0cad9fd064f33a0386dee95461e31", + "sha256:e3e4a28fa0eabd3ab5b0915e9c005e9155039f9e1a4466212434c40eb69a33fb", + "sha256:e59e4ab0e8832bf87707e5024283b3a24cc01784604f0b0e96fbfbadbd8d9fc0", + "sha256:f2a7c226724d7f7745b376b459c500d9d17bd8d0473b7ea6bf8ddb4f7957c69d" + ], + "index": "pypi", + "version": "==9.2.0" + }, + "azure-common": { + "hashes": [ + "sha256:4ac0cd3214e36b6a1b6a442686722a5d8cc449603aa833f3f0f40bda836704a3", + "sha256:5c12d3dcf4ec20599ca6b0d3e09e86e146353d443e7fcc050c9a19c1f9df20ad" + ], + "version": "==1.1.28" + }, + "azure-storage-blob": { + "hashes": [ + "sha256:a8e91a51d4f62d11127c7fd8ba0077385c5b11022f0269f8a2a71b9fc36bef31", + "sha256:b90323aad60f207f9f90a0c4cf94c10acc313c20b39403398dfba51f25f7b454" + ], + "index": "pypi", + "version": "==2.1.0" + }, + "azure-storage-common": { + "hashes": [ + "sha256:b01a491a18839b9d05a4fe3421458a0ddb5ab9443c14e487f40d16f9a1dc2fbe", + "sha256:ccedef5c67227bc4d6670ffd37cec18fb529a1b7c3a5e53e4096eb0cf23dc73f" + ], + "version": "==2.1.0" + }, + "babel": { + "hashes": [ + "sha256:7614553711ee97490f732126dc077f8d0ae084ebc6a96e23db1482afabdb2c51", + "sha256:ff56f4892c1c4bf0d814575ea23471c230d544203c7748e8c68f0089478d48eb" + ], + "markers": "python_version >= '3.6'", + "version": "==2.10.3" + }, + "bcrypt": { + "hashes": [ + "sha256:0b0f0c7141622a31e9734b7f649451147c04ebb5122327ac0bd23744df84be90", + "sha256:1c3334446fac200499e8bc04a530ce3cf0b3d7151e0e4ac5c0dddd3d95e97843", + "sha256:2d0dd19aad87e4ab882ef1d12df505f4c52b28b69666ce83c528f42c07379227", + "sha256:594780b364fb45f2634c46ec8d3e61c1c0f1811c4f2da60e8eb15594ecbf93ed", + "sha256:7c7dd6c1f05bf89e65261d97ac3a6520f34c2acb369afb57e3ea4449be6ff8fd", + "sha256:845b1daf4df2dd94d2fdbc9454953ca9dd0e12970a0bfc9f3dcc6faea3fa96e4", + "sha256:8780e69f9deec9d60f947b169507d2c9816e4f11548f1f7ebee2af38b9b22ae4", + "sha256:bf413f2a9b0a2950fc750998899013f2e718d20fa4a58b85ca50b6df5ed1bbf9", + "sha256:bfb67f6a6c72dfb0a02f3df51550aa1862708e55128b22543e2b42c74f3620d7", + "sha256:c59c170fc9225faad04dde1ba61d85b413946e8ce2e5f5f5ff30dfd67283f319", + "sha256:dc6ec3dc19b1c193b2f7cf279d3e32e7caf447532fbcb7af0906fe4398900c33", + "sha256:ede0f506554571c8eda80db22b83c139303ec6b595b8f60c4c8157bdd0bdee36" + ], + "markers": "python_version >= '3.6'", + "version": "==4.0.0" + }, + "breathe": { + "hashes": [ + "sha256:48804dcf0e607a89fb6ad88c729ef12743a42db03ae9489be4ef8f7c4011774a", + "sha256:ac0768a5e84addad3e632028fe67749c567aba2b29088493b64c2c1634bcdba1" + ], + "index": "pypi", + "version": "==4.34.0" + }, + "carla": { + "hashes": [ + "sha256:1210cce213e968a644effd4e2e48458a072481459d073424b05725056ba3d77d", + "sha256:339fcb1e392f3ade1be82b7258de19c533e2efae111e954a6eb174efb296903d", + "sha256:5f065825ce812343bf27a80a19d647b3200b31b44a9e80cea0340e3bd20cdf81", + "sha256:954ca34d5bdd4516ceca353db907fee8cec6630d6b31a732b17dd1554e0f0f94", + "sha256:a64ee78fe91137fa7d4828c7fc06d5824bd7312e29e4ea4f31a5d74dd28bff40", + "sha256:a95d2d4218ea388c863c66b7c2ab3fe49ffefe53999305cfcb6a8107042f79af", + "sha256:d2bfaea2d6824a2d758cbe813856c69420494f5c97d2a2dfb45653ccf976f1ce" + ], + "index": "pypi", + "markers": "platform_system != 'Darwin'", + "version": "==0.9.13" + }, + "certifi": { + "hashes": [ + "sha256:84c85a9078b11105f04f3036a9482ae10e4621616db313fe045dd24743a0820d", + "sha256:fe86415d55e84719d75f8b69414f6438ac3547d2078ab91b67e779ef69378412" + ], + "markers": "python_version >= '3.6'", + "version": "==2022.6.15" + }, + "cffi": { + "hashes": [ + "sha256:00a9ed42e88df81ffae7a8ab6d9356b371399b91dbdf0c3cb1e84c03a13aceb5", + "sha256:03425bdae262c76aad70202debd780501fabeaca237cdfddc008987c0e0f59ef", + "sha256:04ed324bda3cda42b9b695d51bb7d54b680b9719cfab04227cdd1e04e5de3104", + "sha256:0e2642fe3142e4cc4af0799748233ad6da94c62a8bec3a6648bf8ee68b1c7426", + "sha256:173379135477dc8cac4bc58f45db08ab45d228b3363adb7af79436135d028405", + "sha256:198caafb44239b60e252492445da556afafc7d1e3ab7a1fb3f0584ef6d742375", + "sha256:1e74c6b51a9ed6589199c787bf5f9875612ca4a8a0785fb2d4a84429badaf22a", + "sha256:2012c72d854c2d03e45d06ae57f40d78e5770d252f195b93f581acf3ba44496e", + "sha256:21157295583fe8943475029ed5abdcf71eb3911894724e360acff1d61c1d54bc", + "sha256:2470043b93ff09bf8fb1d46d1cb756ce6132c54826661a32d4e4d132e1977adf", + "sha256:285d29981935eb726a4399badae8f0ffdff4f5050eaa6d0cfc3f64b857b77185", + "sha256:30d78fbc8ebf9c92c9b7823ee18eb92f2e6ef79b45ac84db507f52fbe3ec4497", + "sha256:320dab6e7cb2eacdf0e658569d2575c4dad258c0fcc794f46215e1e39f90f2c3", + "sha256:33ab79603146aace82c2427da5ca6e58f2b3f2fb5da893ceac0c42218a40be35", + "sha256:3548db281cd7d2561c9ad9984681c95f7b0e38881201e157833a2342c30d5e8c", + "sha256:3799aecf2e17cf585d977b780ce79ff0dc9b78d799fc694221ce814c2c19db83", + "sha256:39d39875251ca8f612b6f33e6b1195af86d1b3e60086068be9cc053aa4376e21", + "sha256:3b926aa83d1edb5aa5b427b4053dc420ec295a08e40911296b9eb1b6170f6cca", + "sha256:3bcde07039e586f91b45c88f8583ea7cf7a0770df3a1649627bf598332cb6984", + "sha256:3d08afd128ddaa624a48cf2b859afef385b720bb4b43df214f85616922e6a5ac", + "sha256:3eb6971dcff08619f8d91607cfc726518b6fa2a9eba42856be181c6d0d9515fd", + "sha256:40f4774f5a9d4f5e344f31a32b5096977b5d48560c5592e2f3d2c4374bd543ee", + "sha256:4289fc34b2f5316fbb762d75362931e351941fa95fa18789191b33fc4cf9504a", + "sha256:470c103ae716238bbe698d67ad020e1db9d9dba34fa5a899b5e21577e6d52ed2", + "sha256:4f2c9f67e9821cad2e5f480bc8d83b8742896f1242dba247911072d4fa94c192", + "sha256:50a74364d85fd319352182ef59c5c790484a336f6db772c1a9231f1c3ed0cbd7", + "sha256:54a2db7b78338edd780e7ef7f9f6c442500fb0d41a5a4ea24fff1c929d5af585", + "sha256:5635bd9cb9731e6d4a1132a498dd34f764034a8ce60cef4f5319c0541159392f", + "sha256:59c0b02d0a6c384d453fece7566d1c7e6b7bae4fc5874ef2ef46d56776d61c9e", + "sha256:5d598b938678ebf3c67377cdd45e09d431369c3b1a5b331058c338e201f12b27", + "sha256:5df2768244d19ab7f60546d0c7c63ce1581f7af8b5de3eb3004b9b6fc8a9f84b", + "sha256:5ef34d190326c3b1f822a5b7a45f6c4535e2f47ed06fec77d3d799c450b2651e", + "sha256:6975a3fac6bc83c4a65c9f9fcab9e47019a11d3d2cf7f3c0d03431bf145a941e", + "sha256:6c9a799e985904922a4d207a94eae35c78ebae90e128f0c4e521ce339396be9d", + "sha256:70df4e3b545a17496c9b3f41f5115e69a4f2e77e94e1d2a8e1070bc0c38c8a3c", + "sha256:7473e861101c9e72452f9bf8acb984947aa1661a7704553a9f6e4baa5ba64415", + "sha256:8102eaf27e1e448db915d08afa8b41d6c7ca7a04b7d73af6514df10a3e74bd82", + "sha256:87c450779d0914f2861b8526e035c5e6da0a3199d8f1add1a665e1cbc6fc6d02", + "sha256:8b7ee99e510d7b66cdb6c593f21c043c248537a32e0bedf02e01e9553a172314", + "sha256:91fc98adde3d7881af9b59ed0294046f3806221863722ba7d8d120c575314325", + "sha256:94411f22c3985acaec6f83c6df553f2dbe17b698cc7f8ae751ff2237d96b9e3c", + "sha256:98d85c6a2bef81588d9227dde12db8a7f47f639f4a17c9ae08e773aa9c697bf3", + "sha256:9ad5db27f9cabae298d151c85cf2bad1d359a1b9c686a275df03385758e2f914", + "sha256:a0b71b1b8fbf2b96e41c4d990244165e2c9be83d54962a9a1d118fd8657d2045", + "sha256:a0f100c8912c114ff53e1202d0078b425bee3649ae34d7b070e9697f93c5d52d", + "sha256:a591fe9e525846e4d154205572a029f653ada1a78b93697f3b5a8f1f2bc055b9", + "sha256:a5c84c68147988265e60416b57fc83425a78058853509c1b0629c180094904a5", + "sha256:a66d3508133af6e8548451b25058d5812812ec3798c886bf38ed24a98216fab2", + "sha256:a8c4917bd7ad33e8eb21e9a5bbba979b49d9a97acb3a803092cbc1133e20343c", + "sha256:b3bbeb01c2b273cca1e1e0c5df57f12dce9a4dd331b4fa1635b8bec26350bde3", + "sha256:cba9d6b9a7d64d4bd46167096fc9d2f835e25d7e4c121fb2ddfc6528fb0413b2", + "sha256:cc4d65aeeaa04136a12677d3dd0b1c0c94dc43abac5860ab33cceb42b801c1e8", + "sha256:ce4bcc037df4fc5e3d184794f27bdaab018943698f4ca31630bc7f84a7b69c6d", + "sha256:cec7d9412a9102bdc577382c3929b337320c4c4c4849f2c5cdd14d7368c5562d", + "sha256:d400bfb9a37b1351253cb402671cea7e89bdecc294e8016a707f6d1d8ac934f9", + "sha256:d61f4695e6c866a23a21acab0509af1cdfd2c013cf256bbf5b6b5e2695827162", + "sha256:db0fbb9c62743ce59a9ff687eb5f4afbe77e5e8403d6697f7446e5f609976f76", + "sha256:dd86c085fae2efd48ac91dd7ccffcfc0571387fe1193d33b6394db7ef31fe2a4", + "sha256:e00b098126fd45523dd056d2efba6c5a63b71ffe9f2bbe1a4fe1716e1d0c331e", + "sha256:e229a521186c75c8ad9490854fd8bbdd9a0c9aa3a524326b55be83b54d4e0ad9", + "sha256:e263d77ee3dd201c3a142934a086a4450861778baaeeb45db4591ef65550b0a6", + "sha256:ed9cb427ba5504c1dc15ede7d516b84757c3e3d7868ccc85121d9310d27eed0b", + "sha256:fa6693661a4c91757f4412306191b6dc88c1703f780c8234035eac011922bc01", + "sha256:fcd131dd944808b5bdb38e6f5b53013c5aa4f334c5cad0c72742f6eba4b73db0" + ], + "index": "pypi", + "version": "==1.15.1" + }, + "cfgv": { + "hashes": [ + "sha256:c6a0883f3917a037485059700b9e75da2464e6c27051014ad85ba6aaa5884426", + "sha256:f5a830efb9ce7a445376bb66ec94c638a9787422f96264c98edc6bdeed8ab736" + ], + "markers": "python_full_version >= '3.6.1'", + "version": "==3.3.1" + }, + "charset-normalizer": { + "hashes": [ + "sha256:5a3d016c7c547f69d6f81fb0db9449ce888b418b5b9952cc5e6e66843e9dd845", + "sha256:83e9a75d1911279afd89352c68b45348559d1fc0506b054b346651b5e7fee29f" + ], + "markers": "python_version >= '3.6'", + "version": "==2.1.1" + }, + "control": { + "hashes": [ + "sha256:0891d2d32d6006ac1faa4e238ed8223ca342a4721d202dfeccae24fb02563183" + ], + "index": "pypi", + "version": "==0.9.2" + }, + "coverage": { + "hashes": [ + "sha256:01778769097dbd705a24e221f42be885c544bb91251747a8a3efdec6eb4788f2", + "sha256:08002f9251f51afdcc5e3adf5d5d66bb490ae893d9e21359b085f0e03390a820", + "sha256:1238b08f3576201ebf41f7c20bf59baa0d05da941b123c6656e42cdb668e9827", + "sha256:14a32ec68d721c3d714d9b105c7acf8e0f8a4f4734c811eda75ff3718570b5e3", + "sha256:15e38d853ee224e92ccc9a851457fb1e1f12d7a5df5ae44544ce7863691c7a0d", + "sha256:354df19fefd03b9a13132fa6643527ef7905712109d9c1c1903f2133d3a4e145", + "sha256:35ef1f8d8a7a275aa7410d2f2c60fa6443f4a64fae9be671ec0696a68525b875", + "sha256:4179502f210ebed3ccfe2f78bf8e2d59e50b297b598b100d6c6e3341053066a2", + "sha256:42c499c14efd858b98c4e03595bf914089b98400d30789511577aa44607a1b74", + "sha256:4b7101938584d67e6f45f0015b60e24a95bf8dea19836b1709a80342e01b472f", + "sha256:564cd0f5b5470094df06fab676c6d77547abfdcb09b6c29c8a97c41ad03b103c", + "sha256:5f444627b3664b80d078c05fe6a850dd711beeb90d26731f11d492dcbadb6973", + "sha256:6113e4df2fa73b80f77663445be6d567913fb3b82a86ceb64e44ae0e4b695de1", + "sha256:61b993f3998ee384935ee423c3d40894e93277f12482f6e777642a0141f55782", + "sha256:66e6df3ac4659a435677d8cd40e8eb1ac7219345d27c41145991ee9bf4b806a0", + "sha256:67f9346aeebea54e845d29b487eb38ec95f2ecf3558a3cffb26ee3f0dcc3e760", + "sha256:6913dddee2deff8ab2512639c5168c3e80b3ebb0f818fed22048ee46f735351a", + "sha256:6a864733b22d3081749450466ac80698fe39c91cb6849b2ef8752fd7482011f3", + "sha256:7026f5afe0d1a933685d8f2169d7c2d2e624f6255fb584ca99ccca8c0e966fd7", + "sha256:783bc7c4ee524039ca13b6d9b4186a67f8e63d91342c713e88c1865a38d0892a", + "sha256:7a98d6bf6d4ca5c07a600c7b4e0c5350cd483c85c736c522b786be90ea5bac4f", + "sha256:8d032bfc562a52318ae05047a6eb801ff31ccee172dc0d2504614e911d8fa83e", + "sha256:98c0b9e9b572893cdb0a00e66cf961a238f8d870d4e1dc8e679eb8bdc2eb1b86", + "sha256:9c7b9b498eb0c0d48b4c2abc0e10c2d78912203f972e0e63e3c9dc21f15abdaa", + "sha256:9cc4f107009bca5a81caef2fca843dbec4215c05e917a59dec0c8db5cff1d2aa", + "sha256:9d6e1f3185cbfd3d91ac77ea065d85d5215d3dfa45b191d14ddfcd952fa53796", + "sha256:a095aa0a996ea08b10580908e88fbaf81ecf798e923bbe64fb98d1807db3d68a", + "sha256:a3b2752de32c455f2521a51bd3ffb53c5b3ae92736afde67ce83477f5c1dd928", + "sha256:ab066f5ab67059d1f1000b5e1aa8bbd75b6ed1fc0014559aea41a9eb66fc2ce0", + "sha256:c1328d0c2f194ffda30a45f11058c02410e679456276bfa0bbe0b0ee87225fac", + "sha256:c35cca192ba700979d20ac43024a82b9b32a60da2f983bec6c0f5b84aead635c", + "sha256:cbbb0e4cd8ddcd5ef47641cfac97d8473ab6b132dd9a46bacb18872828031685", + "sha256:cdbb0d89923c80dbd435b9cf8bba0ff55585a3cdb28cbec65f376c041472c60d", + "sha256:cf2afe83a53f77aec067033199797832617890e15bed42f4a1a93ea24794ae3e", + "sha256:d5dd4b8e9cd0deb60e6fcc7b0647cbc1da6c33b9e786f9c79721fd303994832f", + "sha256:dfa0b97eb904255e2ab24166071b27408f1f69c8fbda58e9c0972804851e0558", + "sha256:e16c45b726acb780e1e6f88b286d3c10b3914ab03438f32117c4aa52d7f30d58", + "sha256:e1fabd473566fce2cf18ea41171d92814e4ef1495e04471786cbc943b89a3781", + "sha256:e3d3c4cc38b2882f9a15bafd30aec079582b819bec1b8afdbde8f7797008108a", + "sha256:e431e305a1f3126477abe9a184624a85308da8edf8486a863601d58419d26ffa", + "sha256:e7b4da9bafad21ea45a714d3ea6f3e1679099e420c8741c74905b92ee9bfa7cc", + "sha256:ee2b2fb6eb4ace35805f434e0f6409444e1466a47f620d1d5763a22600f0f892", + "sha256:ee6ae6bbcac0786807295e9687169fba80cb0617852b2fa118a99667e8e6815d", + "sha256:ef6f44409ab02e202b31a05dd6666797f9de2aa2b4b3534e9d450e42dea5e817", + "sha256:f67cf9f406cf0d2f08a3515ce2db5b82625a7257f88aad87904674def6ddaec1", + "sha256:f855b39e4f75abd0dfbcf74a82e84ae3fc260d523fcb3532786bcbbcb158322c", + "sha256:fc600f6ec19b273da1d85817eda339fb46ce9eef3e89f220055d8696e0a06908", + "sha256:fcbe3d9a53e013f8ab88734d7e517eb2cd06b7e689bedf22c0eb68db5e4a0a19", + "sha256:fde17bc42e0716c94bf19d92e4c9f5a00c5feb401f5bc01101fdf2a8b7cacf60", + "sha256:ff934ced84054b9018665ca3967fc48e1ac99e811f6cc99ea65978e1d384454b" + ], + "index": "pypi", + "version": "==6.4.4" + }, + "cryptography": { + "hashes": [ + "sha256:190f82f3e87033821828f60787cfa42bff98404483577b591429ed99bed39d59", + "sha256:2be53f9f5505673eeda5f2736bea736c40f051a739bfae2f92d18aed1eb54596", + "sha256:30788e070800fec9bbcf9faa71ea6d8068f5136f60029759fd8c3efec3c9dcb3", + "sha256:3d41b965b3380f10e4611dbae366f6dc3cefc7c9ac4e8842a806b9672ae9add5", + "sha256:4c590ec31550a724ef893c50f9a97a0c14e9c851c85621c5650d699a7b88f7ab", + "sha256:549153378611c0cca1042f20fd9c5030d37a72f634c9326e225c9f666d472884", + "sha256:63f9c17c0e2474ccbebc9302ce2f07b55b3b3fcb211ded18a42d5764f5c10a82", + "sha256:6bc95ed67b6741b2607298f9ea4932ff157e570ef456ef7ff0ef4884a134cc4b", + "sha256:7099a8d55cd49b737ffc99c17de504f2257e3787e02abe6d1a6d136574873441", + "sha256:75976c217f10d48a8b5a8de3d70c454c249e4b91851f6838a4e48b8f41eb71aa", + "sha256:7bc997818309f56c0038a33b8da5c0bfbb3f1f067f315f9abd6fc07ad359398d", + "sha256:80f49023dd13ba35f7c34072fa17f604d2f19bf0989f292cedf7ab5770b87a0b", + "sha256:91ce48d35f4e3d3f1d83e29ef4a9267246e6a3be51864a5b7d2247d5086fa99a", + "sha256:a958c52505c8adf0d3822703078580d2c0456dd1d27fabfb6f76fe63d2971cd6", + "sha256:b62439d7cd1222f3da897e9a9fe53bbf5c104fff4d60893ad1355d4c14a24157", + "sha256:b7f8dd0d4c1f21759695c05a5ec8536c12f31611541f8904083f3dc582604280", + "sha256:d204833f3c8a33bbe11eda63a54b1aad7aa7456ed769a982f21ec599ba5fa282", + "sha256:e007f052ed10cc316df59bc90fbb7ff7950d7e2919c9757fd42a2b8ecf8a5f67", + "sha256:f2dcb0b3b63afb6df7fd94ec6fbddac81b5492513f7b0436210d390c14d46ee8", + "sha256:f721d1885ecae9078c3f6bbe8a88bc0786b6e749bf32ccec1ef2b18929a05046", + "sha256:f7a6de3e98771e183645181b3627e2563dcde3ce94a9e42a3f427d2255190327", + "sha256:f8c0a6e9e1dd3eb0414ba320f85da6b0dcbd543126e30fcc546e7372a7fbf3b9" + ], + "index": "pypi", + "version": "==37.0.4" + }, + "cycler": { + "hashes": [ + "sha256:3a27e95f763a428a739d2add979fa7494c912a32c17c4c38c4d5f082cad165a3", + "sha256:9c87405839a19696e837b3b818fed3f5f69f16f1eec1a1ad77e043dcea9c772f" + ], + "markers": "python_version >= '3.6'", + "version": "==0.11.0" + }, + "dictdiffer": { + "hashes": [ + "sha256:17bacf5fbfe613ccf1b6d512bd766e6b21fb798822a133aa86098b8ac9997578", + "sha256:442bfc693cfcadaf46674575d2eba1c53b42f5e404218ca2c2ff549f2df56595" + ], + "index": "pypi", + "version": "==0.9.0" + }, + "distlib": { + "hashes": [ + "sha256:14bad2d9b04d3a36127ac97f30b12a19268f211063d8f8ee4f47108896e11b46", + "sha256:f35c4b692542ca110de7ef0bea44d73981caeb34ca0b9b6b2e6d7790dda8f80e" + ], + "version": "==0.3.6" + }, + "docutils": { + "hashes": [ + "sha256:686577d2e4c32380bb50cbb22f575ed742d58168cee37e99117a854bcd88f125", + "sha256:cf316c8370a737a022b72b56874f6602acf974a37a9fba42ec2876387549fc61" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", + "version": "==0.17.1" + }, + "execnet": { + "hashes": [ + "sha256:8f694f3ba9cc92cab508b152dcfe322153975c29bda272e2fd7f3f00f36e47c5", + "sha256:a295f7cc774947aac58dde7fdc85f4aa00c42adf5d8f5468fc630c1acf30a142" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", + "version": "==1.9.0" + }, + "fastcluster": { + "hashes": [ + "sha256:03f8efe6435a7b947fa4a420676942d0267dac0d323ec5ead50f1856cc7cf96f", + "sha256:060c1cb3c84942d8d3618385e2c25998ba690c46ec8c73d64477f808abfac3f2", + "sha256:06df1d97edca68b2ffa43d81c3b5f4e4147bc12ab241c6585fadcdeb0bfa23ca", + "sha256:0bb54283b4d5ec96f167c7fd31921f169226c1261637434fdae7a69ee3c69573", + "sha256:11748a4e245c745030e9ddd8c2c37e378f8ad8bd8e869d954c84ff674495499f", + "sha256:1801c9daa9aa5bbbb0830efe8bd3034b4b7a417e4b8dd353683999be29797df2", + "sha256:2fcb0973ca0e6978e3242046338c350cbed1493108929231fae9bd35ad05a6d6", + "sha256:4093d5454bcbe48b30e32da5db43056a08889480a96e4555f28c1f7004fc5323", + "sha256:4b9cfd426966b8037bec2fc03a0d7a9c87313482c699b36ffa1432b49f84ed2e", + "sha256:58958a0333e3dfbad198394e9b7dd6254de0a3e622019b057288405b2a4a6bba", + "sha256:5fe543b6d45da27e84e5af6248722475b88943d8ef40d835cbabbb0ba5ee786b", + "sha256:6a7c7f51a6d2f5ab58b1d85e9d0af2af9600ec13bb43bc6aafc9085d2c4ccd93", + "sha256:6cf156d4203708348522393c523c2e61c81f5a6a500e0411dcba2b064551ea2f", + "sha256:6e51db0067e65687a5c46f00a11843d0bb15ca759e8a1767eebac8c4f6e3f4df", + "sha256:72503e727887a61a15f9aaa13178798d3994dfec58aa7a943e42dcfda07c0149", + "sha256:7254f81dc71cd29ef6f2d9747cf97ff907b569c9ef9d9760352391be5b57118c", + "sha256:855ab2b7e6fa9b05f19c4f3023dedfb1a35a88d831933d65d0d9e10a070a9e85", + "sha256:86a1ad972e83ba48144884fa849f87626346308b650002157123aee67d3b16fe", + "sha256:8bac5cf64691060cf86b0752dd385ef1eccff6d24bdb8b60691cf8cbf0e4f9ef", + "sha256:8d3c9eab8e69cb36dcdd64c8b3200e008aedf88e34d39e01ae6af98a9605ad18", + "sha256:9020899b67fe492d0ed87a3e993ec9962c5a0b51ea2df71d86b1766f065f1cef", + "sha256:a085e7e13f1afc517358981b2b7ed774dc9abf95f2be0da9a495d9e6b58c4409", + "sha256:a5ceb39379327316d34613f7c16c06d7a3816aa38f4437b5e8433aa1bf149e2c", + "sha256:a6f8da329c0032f2acaf4beaef958a2db0dae43d3f946f592dad5c29aa82c832", + "sha256:a952a84453123db0c2336b9a9c86162e99ad0b897bae8213107c055a64effd41", + "sha256:aa4a4c01c5fbec3623e92bc33a9f712ca416ce93255c402f5c904ac4b890ac3c", + "sha256:aab886efa7b6bba7ac124f4498153d053e5a08b822d2254926b7206cdf5a8aa6", + "sha256:ab9337b0a6a9b07b6f86fc724972d1ad729c890e2f539c1b33271c2f1f00af8b", + "sha256:c12224da0b1f2f9d2b3d715dc82ecb1a3a33b990606f97da075cc46bc6d9576f", + "sha256:c61be0bad81a21ee3e5bef91469fdd11968f33d41d142c656accba9b2992babe", + "sha256:c8be01f97bc2bf11a9188537864f8e520e1103cdc6007aa2c5d7979b1363b121", + "sha256:cb27c13225f5f77f3c5986a27ca27277cce7db12844330cf535019cd38021257", + "sha256:cf5acfe1156849279ebd44a8d1fbcbe8b8e21334f7538eda782ae31e7dade9e2", + "sha256:d0e8faef0437a25fd083df70fb86cc65ce3c9c9780d4ae377cbe6521e7746ce0", + "sha256:e03a228e018457842eb81de85be7af0b5fe8065d666dd093193e3bdcf1f13d2e", + "sha256:eb3f98791427d5d5d02d023b66bcef61e48954edfadae6527ef72d70cf32ec86", + "sha256:ffdb00782cd63bbf2c45bb048897531e868326dff5081ab9b752d294b0426c1d" + ], + "index": "pypi", + "version": "==1.2.6" + }, + "filelock": { + "hashes": [ + "sha256:55447caa666f2198c5b6b13a26d2084d26fa5b115c00d065664b2124680c4edc", + "sha256:617eb4e5eedc82fc5f47b6d61e4d11cb837c56cb4544e39081099fa17ad109d4" + ], + "markers": "python_version >= '3.7'", + "version": "==3.8.0" + }, + "fonttools": { + "hashes": [ + "sha256:4606e1a88ee1f6699d182fea9511bd9a8a915d913eab4584e5226da1180fcce7", + "sha256:fff6b752e326c15756c819fe2fe7ceab69f96a1dbcfe8911d0941cdb49905007" + ], + "markers": "python_version >= '3.7'", + "version": "==4.37.1" + }, + "ft4222": { + "hashes": [ + "sha256:074f4eb234450306b9040d721eb5ab2a423d3b07d6f4a30824198afeb0a9bbb6", + "sha256:09f18a5610d0d81c568c9095798f542166b98025d119c9fa91555fa5bfddc2ee", + "sha256:0cceacb43d75a5cb3cdf2d95d4164fb2c1cfde007d9782538150bc10d7933680", + "sha256:10136015000719f68b2a4b319b612183a1601816870521e6a45c8c0b67d38325", + "sha256:1894469abffd739fc3a83f818ea29532283965c74c3c64e5c9b9e6b454971d03", + "sha256:24cdb72a7cd420c1ed009c0821555b9818b8828dc31a1a01224a35d4757c7e5d", + "sha256:286a2f3c023e9beb5c6ca6b692c3ed92a1fe44b326f7cb58807d0b99f372c7d7", + "sha256:28b472bbbb18e6f65dacd139231a53640827f2e486c54750fbcff7a16454cb6d", + "sha256:296555ca4096b5ce13e79b089622de5cf9236f3d32b074c8483ae72490e7448b", + "sha256:396f5c7c38e0c8dc1b31d5d9709d7eb86ef0ee75412867ccc352506aa3a29ae4", + "sha256:39a7d8795d32b8c126ed253a0c1a9d7971f3af83d2ceb796af47fbef03485741", + "sha256:50e37f59b8e553384cdbfab64096e3fb1c7ca9f15ae419ae0d0fdda3a2e05f54", + "sha256:511b785a23ba2fc8480dbaeda33e1f22ffe5dd58731e1c53e379989731fb42ad", + "sha256:5f91e530ee6fe6a13c08ec4dc3c7f54a704fc642b3d73f4392a9db22c5e243dd", + "sha256:65e5b7bfbe3552b771770807df0a251616bb2c8f7541e4e9f350715f225a71c0", + "sha256:6e208af13621b8a79a8c623c8deb3b971f4d4e4587156622a6558b91719f9d33", + "sha256:70aec6df75d1f8ee051c5d16a48363e4d3552feecf3466cec2700415c073e5e4", + "sha256:9670396daab3deb91847ee40c0338bca07f4041176b2aed1c49277dc1ef3497f", + "sha256:a300c2749adb674ef3d95b17a1311deaf0a3318e14ee7e9d7e56317e307b0012", + "sha256:a675b88124dfb1d2744f27823e3dfd094c0236674e453e83d486fc17358761ff", + "sha256:ab5ea9522bd0fd1b87348bf26d0a1131e87f586366271581c9b1d0acdf870173", + "sha256:baf80af3de3af376080bfb8f75f3d6aba9e9415001a6f72299cbb344e6b739cb", + "sha256:bb3cb6485d7a0d1eac0e0027eab6b9ec95e5f5722e853cdc2850d2ae70086eea", + "sha256:c5a993dd3af47ab69f5b58920dba15c98658de7a73b9c81d402fe6eaf292edab", + "sha256:c7e31cefcdcfe3653df35cd993f821b746e747b294ded8bff27d1f8bd8c5e43b", + "sha256:caec2458db0d8e29888da2c22aa4427c1d993e943ed325ef7a6b8eb24f55d163", + "sha256:d688830cf004cde39b3cc757fef8ec31ae266fceda1566ca53b3b79e5ab7b6e4" + ], + "index": "pypi", + "version": "==1.5.0" + }, + "hexdump": { + "hashes": [ + "sha256:d781a43b0c16ace3f9366aade73e8ad3a7bd5137d58f0b45ab2d3f54876f20db" + ], + "index": "pypi", + "version": "==3.3" + }, + "hypothesis": { + "hashes": [ + "sha256:2696cdb9005946bf1d2b215cc91d3fc01625e3342eb8743ddd04b667b2f1882b", + "sha256:4ad26c5d434171ffc02aba569dd52255573d615554c062bc30734dbe6f318c61", + "sha256:69978811f1d9c19710c7d2bf8233dc43c80efa964251b72efbe8274044e073b4", + "sha256:967009fa561b3a3f8363a73d71923357271c37dc7fa27b30c2d21a1b6092b240" + ], + "index": "pypi", + "version": "==6.46.7" + }, + "identify": { + "hashes": [ + "sha256:25851c8c1370effb22aaa3c987b30449e9ff0cece408f810ae6ce408fdd20893", + "sha256:887e7b91a1be152b0d46bbf072130235a8117392b9f1828446079a816a05ef44" + ], + "markers": "python_version >= '3.7'", + "version": "==2.5.3" + }, + "idna": { + "hashes": [ + "sha256:84d9dd047ffa80596e0f246e2eab0b391788b0503584e8945f2368256d2735ff", + "sha256:9d643ff0a55b762d5cdb124b8eaa99c66322e2157b69160bc32796e824360e6d" + ], + "markers": "python_version >= '3.5'", + "version": "==3.3" + }, + "imagesize": { + "hashes": [ + "sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b", + "sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3'", + "version": "==1.4.1" + }, + "importlib-metadata": { + "hashes": [ + "sha256:637245b8bab2b6502fcbc752cc4b7a6f6243bb02b31c5c26156ad103d3d45670", + "sha256:7401a975809ea1fdc658c3aa4f78cc2195a0e019c5cbc4c06122884e9ae80c23" + ], + "markers": "python_version < '3.10'", + "version": "==4.12.0" + }, + "iniconfig": { + "hashes": [ + "sha256:011e24c64b7f47f6ebd835bb12a743f2fbe9a26d4cecaa7f53bc4f35ee9da8b3", + "sha256:bc3af051d7d14b2ee5ef9969666def0cd1a000e121eaea580d4a313df4b37f32" + ], + "version": "==1.1.1" + }, + "inputs": { + "hashes": [ + "sha256:13f894564e52134cf1e3862b1811da034875eb1f2b62e6021e3776e9669a96ec", + "sha256:a31d5b96a3525f1232f326be9e7ce8ccaf873c6b1fb84d9f3c9bc3d79b23eae4" + ], + "index": "pypi", + "version": "==0.5" + }, + "jinja2": { + "hashes": [ + "sha256:31351a702a408a9e7595a8fc6150fc3f43bb6bf7e319770cbc0db9df9437e852", + "sha256:6088930bfe239f0e6710546ab9c19c9ef35e29792895fed6e6e31a023a182a61" + ], + "index": "pypi", + "version": "==3.1.2" + }, + "kiwisolver": { + "hashes": [ + "sha256:02f79693ec433cb4b5f51694e8477ae83b3205768a6fb48ffba60549080e295b", + "sha256:03baab2d6b4a54ddbb43bba1a3a2d1627e82d205c5cf8f4c924dc49284b87166", + "sha256:1041feb4cda8708ce73bb4dcb9ce1ccf49d553bf87c3954bdfa46f0c3f77252c", + "sha256:10ee06759482c78bdb864f4109886dff7b8a56529bc1609d4f1112b93fe6423c", + "sha256:1d1573129aa0fd901076e2bfb4275a35f5b7aa60fbfb984499d661ec950320b0", + "sha256:283dffbf061a4ec60391d51e6155e372a1f7a4f5b15d59c8505339454f8989e4", + "sha256:28bc5b299f48150b5f822ce68624e445040595a4ac3d59251703779836eceff9", + "sha256:2a66fdfb34e05b705620dd567f5a03f239a088d5a3f321e7b6ac3239d22aa286", + "sha256:2e307eb9bd99801f82789b44bb45e9f541961831c7311521b13a6c85afc09767", + "sha256:2e407cb4bd5a13984a6c2c0fe1845e4e41e96f183e5e5cd4d77a857d9693494c", + "sha256:2f5e60fabb7343a836360c4f0919b8cd0d6dbf08ad2ca6b9cf90bf0c76a3c4f6", + "sha256:36dafec3d6d6088d34e2de6b85f9d8e2324eb734162fba59d2ba9ed7a2043d5b", + "sha256:3fe20f63c9ecee44560d0e7f116b3a747a5d7203376abeea292ab3152334d004", + "sha256:41dae968a94b1ef1897cb322b39360a0812661dba7c682aa45098eb8e193dbdf", + "sha256:4bd472dbe5e136f96a4b18f295d159d7f26fd399136f5b17b08c4e5f498cd494", + "sha256:4ea39b0ccc4f5d803e3337dd46bcce60b702be4d86fd0b3d7531ef10fd99a1ac", + "sha256:5853eb494c71e267912275e5586fe281444eb5e722de4e131cddf9d442615626", + "sha256:5bce61af018b0cb2055e0e72e7d65290d822d3feee430b7b8203d8a855e78766", + "sha256:6295ecd49304dcf3bfbfa45d9a081c96509e95f4b9d0eb7ee4ec0530c4a96514", + "sha256:62ac9cc684da4cf1778d07a89bf5f81b35834cb96ca523d3a7fb32509380cbf6", + "sha256:70e7c2e7b750585569564e2e5ca9845acfaa5da56ac46df68414f29fea97be9f", + "sha256:7577c1987baa3adc4b3c62c33bd1118c3ef5c8ddef36f0f2c950ae0b199e100d", + "sha256:75facbe9606748f43428fc91a43edb46c7ff68889b91fa31f53b58894503a191", + "sha256:787518a6789009c159453da4d6b683f468ef7a65bbde796bcea803ccf191058d", + "sha256:78d6601aed50c74e0ef02f4204da1816147a6d3fbdc8b3872d263338a9052c51", + "sha256:7c43e1e1206cd421cd92e6b3280d4385d41d7166b3ed577ac20444b6995a445f", + "sha256:81e38381b782cc7e1e46c4e14cd997ee6040768101aefc8fa3c24a4cc58e98f8", + "sha256:841293b17ad704d70c578f1f0013c890e219952169ce8a24ebc063eecf775454", + "sha256:872b8ca05c40d309ed13eb2e582cab0c5a05e81e987ab9c521bf05ad1d5cf5cb", + "sha256:877272cf6b4b7e94c9614f9b10140e198d2186363728ed0f701c6eee1baec1da", + "sha256:8c808594c88a025d4e322d5bb549282c93c8e1ba71b790f539567932722d7bd8", + "sha256:8ed58b8acf29798b036d347791141767ccf65eee7f26bde03a71c944449e53de", + "sha256:91672bacaa030f92fc2f43b620d7b337fd9a5af28b0d6ed3f77afc43c4a64b5a", + "sha256:968f44fdbf6dd757d12920d63b566eeb4d5b395fd2d00d29d7ef00a00582aac9", + "sha256:9f85003f5dfa867e86d53fac6f7e6f30c045673fa27b603c397753bebadc3008", + "sha256:a553dadda40fef6bfa1456dc4be49b113aa92c2a9a9e8711e955618cd69622e3", + "sha256:a68b62a02953b9841730db7797422f983935aeefceb1679f0fc85cbfbd311c32", + "sha256:abbe9fa13da955feb8202e215c4018f4bb57469b1b78c7a4c5c7b93001699938", + "sha256:ad881edc7ccb9d65b0224f4e4d05a1e85cf62d73aab798943df6d48ab0cd79a1", + "sha256:b1792d939ec70abe76f5054d3f36ed5656021dcad1322d1cc996d4e54165cef9", + "sha256:b428ef021242344340460fa4c9185d0b1f66fbdbfecc6c63eff4b7c29fad429d", + "sha256:b533558eae785e33e8c148a8d9921692a9fe5aa516efbdff8606e7d87b9d5824", + "sha256:ba59c92039ec0a66103b1d5fe588fa546373587a7d68f5c96f743c3396afc04b", + "sha256:bc8d3bd6c72b2dd9decf16ce70e20abcb3274ba01b4e1c96031e0c4067d1e7cd", + "sha256:bc9db8a3efb3e403e4ecc6cd9489ea2bac94244f80c78e27c31dcc00d2790ac2", + "sha256:bf7d9fce9bcc4752ca4a1b80aabd38f6d19009ea5cbda0e0856983cf6d0023f5", + "sha256:c2dbb44c3f7e6c4d3487b31037b1bdbf424d97687c1747ce4ff2895795c9bf69", + "sha256:c79ebe8f3676a4c6630fd3f777f3cfecf9289666c84e775a67d1d358578dc2e3", + "sha256:c97528e64cb9ebeff9701e7938653a9951922f2a38bd847787d4a8e498cc83ae", + "sha256:d0611a0a2a518464c05ddd5a3a1a0e856ccc10e67079bb17f265ad19ab3c7597", + "sha256:d06adcfa62a4431d404c31216f0f8ac97397d799cd53800e9d3efc2fbb3cf14e", + "sha256:d41997519fcba4a1e46eb4a2fe31bc12f0ff957b2b81bac28db24744f333e955", + "sha256:d5b61785a9ce44e5a4b880272baa7cf6c8f48a5180c3e81c59553ba0cb0821ca", + "sha256:da152d8cdcab0e56e4f45eb08b9aea6455845ec83172092f09b0e077ece2cf7a", + "sha256:da7e547706e69e45d95e116e6939488d62174e033b763ab1496b4c29b76fabea", + "sha256:db5283d90da4174865d520e7366801a93777201e91e79bacbac6e6927cbceede", + "sha256:db608a6757adabb32f1cfe6066e39b3706d8c3aa69bbc353a5b61edad36a5cb4", + "sha256:e0ea21f66820452a3f5d1655f8704a60d66ba1191359b96541eaf457710a5fc6", + "sha256:e7da3fec7408813a7cebc9e4ec55afed2d0fd65c4754bc376bf03498d4e92686", + "sha256:e92a513161077b53447160b9bd8f522edfbed4bd9759e4c18ab05d7ef7e49408", + "sha256:ecb1fa0db7bf4cff9dac752abb19505a233c7f16684c5826d1f11ebd9472b871", + "sha256:efda5fc8cc1c61e4f639b8067d118e742b812c930f708e6667a5ce0d13499e29", + "sha256:f0a1dbdb5ecbef0d34eb77e56fcb3e95bbd7e50835d9782a45df81cc46949750", + "sha256:f0a71d85ecdd570ded8ac3d1c0f480842f49a40beb423bb8014539a9f32a5897", + "sha256:f4f270de01dd3e129a72efad823da90cc4d6aafb64c410c9033aba70db9f1ff0", + "sha256:f6cb459eea32a4e2cf18ba5fcece2dbdf496384413bc1bae15583f19e567f3b2", + "sha256:f8ad8285b01b0d4695102546b342b493b3ccc6781fc28c8c6a1bb63e95d22f09", + "sha256:f9f39e2f049db33a908319cf46624a569b36983c7c78318e9726a4cb8923b26c" + ], + "markers": "python_version >= '3.7'", + "version": "==1.4.4" + }, + "lru-dict": { + "hashes": [ + "sha256:075b9dd46d7022b675419bc6e3631748ae184bc8af195d20365a98b4f3bb2914", + "sha256:0972d669e9e207617e06416166718b073a49bf449abbd23940d9545c0847a4d9", + "sha256:0f83cd70a6d32f9018d471be609f3af73058f700691657db4a3d3dd78d3f96dd", + "sha256:10fe823ff90b655f0b6ba124e2b576ecda8c61b8ead76b456db67831942d22f2", + "sha256:163079dbda54c3e6422b23da39fb3ecc561035d65e8496ff1950cbdb376018e1", + "sha256:1fe16ade5fd0a57e9a335f69b8055aaa6fb278fbfa250458e4f6b8255115578f", + "sha256:262a4e622010ceb960a6a5222ed011090e50954d45070fd369c0fa4d2ed7d9a9", + "sha256:2f340b61f3cdfee71f66da7dbfd9a5ea2db6974502ccff2065cdb76619840dca", + "sha256:348167f110494cfafae70c066470a6f4e4d43523933edf16ccdb8947f3b5fae0", + "sha256:3b1692755fef288b67af5cd8a973eb331d1f44cb02cbdc13660040809c2bfec6", + "sha256:3ca497cb25f19f24171f9172805f3ff135b911aeb91960bd4af8e230421ccb51", + "sha256:3d003a864899c29b0379e412709a6e516cbd6a72ee10b09d0b33226343617412", + "sha256:3fef595c4f573141d54a38bda9221b9ee3cbe0acc73d67304a1a6d5972eb2a02", + "sha256:484ac524e4615f06dc72ffbfd83f26e073c9ec256de5413634fbd024c010a8bc", + "sha256:55aeda6b6789b2d030066b4f5f6fc3596560ba2a69028f35f3682a795701b5b1", + "sha256:5a592363c93d6fc6472d5affe2819e1c7590746aecb464774a4f67e09fbefdfc", + "sha256:5b09dbe47bc4b4d45ffe56067aff190bc3c0049575da6e52127e114236e0a6a7", + "sha256:6e2a7aa9e36626fb48fdc341c7e3685a31a7b50ea4918677ea436271ad0d904d", + "sha256:70364e3cbef536adab8762b4835e18f5ca8e3fddd8bd0ec9258c42bbebd0ee77", + "sha256:720f5728e537f11a311e8b720793a224e985d20e6b7c3d34a891a391865af1a2", + "sha256:7284bdbc5579bbdc3fc8f869ed4c169f403835566ab0f84567cdbfdd05241847", + "sha256:7be1b66926277993cecdc174c15a20c8ce785c1f8b39aa560714a513eef06473", + "sha256:86d32a4498b74a75340497890a260d37bf1560ad2683969393032977dd36b088", + "sha256:878bc8ef4073e5cfb953dfc1cf4585db41e8b814c0106abde34d00ee0d0b3115", + "sha256:881104711900af45967c2e5ce3e62291dd57d5b2a224d58b7c9f60bf4ad41b8c", + "sha256:8c50ab9edaa5da5838426816a2b7bcde9d576b4fc50e6a8c062073dbc4969d78", + "sha256:8f6561f9cd5a452cb84905c6a87aa944fdfdc0f41cc057d03b71f9b29b2cc4bd", + "sha256:93336911544ebc0e466272043adab9fb9f6e9dcba6024b639c32553a3790e089", + "sha256:9447214e4857e16d14158794ef01e4501d8fad07d298d03308d9f90512df02fa", + "sha256:97c24ffc55de6013075979f440acd174e88819f30387074639fb7d7178ca253e", + "sha256:99f6cfb3e28490357a0805b409caf693e46c61f8dbb789c51355adb693c568d3", + "sha256:9be6c4039ef328676b868acea619cd100e3de1a35b3be211cf0eaf9775563b65", + "sha256:9d70257246b8207e8ef3d8b18457089f5ff0dfb087bd36eb33bce6584f2e0b3a", + "sha256:a777d48319d293b1b6a933d606c0e4899690a139b4c81173451913bbcab6f44f", + "sha256:add762163f4af7f4173fafa4092eb7c7f023cf139ef6d2015cfea867e1440d82", + "sha256:b6f64005ede008b7a866be8f3f6274dbf74e656e15e4004e9d99ad65efb01809", + "sha256:beb089c46bd95243d1ac5b2bd13627317b08bf40dd8dc16d4b7ee7ecb3cf65ca", + "sha256:c07163c9dcbb2eca377f366b1331f46302fd8b6b72ab4d603087feca00044bb0", + "sha256:c2fe692332c2f1d81fd27457db4b35143801475bfc2e57173a2403588dd82a42", + "sha256:ca8f89361e0e7aad0bf93ae03a31502e96280faeb7fb92267f4998fb230d36b2", + "sha256:d2ed4151445c3f30423c2698f72197d64b27b1cd61d8d56702ffe235584e47c2", + "sha256:db20597c4e67b4095b376ce2e83930c560f4ce481e8d05737885307ed02ba7c1", + "sha256:de972c7f4bc7b6002acff2a8de984c55fbd7f2289dba659cfd90f7a0f5d8f5d1", + "sha256:f1df1da204a9f0b5eb8393a46070f1d984fa8559435ee790d7f8f5602038fc00", + "sha256:f4d0a6d733a23865019b1c97ed6fb1fdb739be923192abf4dbb644f697a26a69", + "sha256:f874e9c2209dada1a080545331aa1277ec060a13f61684a8642788bf44b2325f", + "sha256:f877f53249c3e49bbd7612f9083127290bede6c7d6501513567ab1bf9c581381", + "sha256:f9d5815c0e85922cd0fb8344ca8b1c7cf020bf9fc45e670d34d51932c91fd7ec" + ], + "index": "pypi", + "version": "==1.1.8" + }, + "markdown-it-py": { + "hashes": [ + "sha256:93de681e5c021a432c63147656fe21790bc01231e0cd2da73626f1aa3ac0fe27", + "sha256:cf7e59fed14b5ae17c0006eff14a2d9a00ed5f3a846148153899a0224e2c07da" + ], + "index": "pypi", + "version": "==2.1.0" + }, + "markupsafe": { + "hashes": [ + "sha256:0212a68688482dc52b2d45013df70d169f542b7394fc744c02a57374a4207003", + "sha256:089cf3dbf0cd6c100f02945abeb18484bd1ee57a079aefd52cffd17fba910b88", + "sha256:10c1bfff05d95783da83491be968e8fe789263689c02724e0c691933c52994f5", + "sha256:33b74d289bd2f5e527beadcaa3f401e0df0a89927c1559c8566c066fa4248ab7", + "sha256:3799351e2336dc91ea70b034983ee71cf2f9533cdff7c14c90ea126bfd95d65a", + "sha256:3ce11ee3f23f79dbd06fb3d63e2f6af7b12db1d46932fe7bd8afa259a5996603", + "sha256:421be9fbf0ffe9ffd7a378aafebbf6f4602d564d34be190fc19a193232fd12b1", + "sha256:43093fb83d8343aac0b1baa75516da6092f58f41200907ef92448ecab8825135", + "sha256:46d00d6cfecdde84d40e572d63735ef81423ad31184100411e6e3388d405e247", + "sha256:4a33dea2b688b3190ee12bd7cfa29d39c9ed176bda40bfa11099a3ce5d3a7ac6", + "sha256:4b9fe39a2ccc108a4accc2676e77da025ce383c108593d65cc909add5c3bd601", + "sha256:56442863ed2b06d19c37f94d999035e15ee982988920e12a5b4ba29b62ad1f77", + "sha256:671cd1187ed5e62818414afe79ed29da836dde67166a9fac6d435873c44fdd02", + "sha256:694deca8d702d5db21ec83983ce0bb4b26a578e71fbdbd4fdcd387daa90e4d5e", + "sha256:6a074d34ee7a5ce3effbc526b7083ec9731bb3cbf921bbe1d3005d4d2bdb3a63", + "sha256:6d0072fea50feec76a4c418096652f2c3238eaa014b2f94aeb1d56a66b41403f", + "sha256:6fbf47b5d3728c6aea2abb0589b5d30459e369baa772e0f37a0320185e87c980", + "sha256:7f91197cc9e48f989d12e4e6fbc46495c446636dfc81b9ccf50bb0ec74b91d4b", + "sha256:86b1f75c4e7c2ac2ccdaec2b9022845dbb81880ca318bb7a0a01fbf7813e3812", + "sha256:8dc1c72a69aa7e082593c4a203dcf94ddb74bb5c8a731e4e1eb68d031e8498ff", + "sha256:8e3dcf21f367459434c18e71b2a9532d96547aef8a871872a5bd69a715c15f96", + "sha256:8e576a51ad59e4bfaac456023a78f6b5e6e7651dcd383bcc3e18d06f9b55d6d1", + "sha256:96e37a3dc86e80bf81758c152fe66dbf60ed5eca3d26305edf01892257049925", + "sha256:97a68e6ada378df82bc9f16b800ab77cbf4b2fada0081794318520138c088e4a", + "sha256:99a2a507ed3ac881b975a2976d59f38c19386d128e7a9a18b7df6fff1fd4c1d6", + "sha256:a49907dd8420c5685cfa064a1335b6754b74541bbb3706c259c02ed65b644b3e", + "sha256:b09bf97215625a311f669476f44b8b318b075847b49316d3e28c08e41a7a573f", + "sha256:b7bd98b796e2b6553da7225aeb61f447f80a1ca64f41d83612e6139ca5213aa4", + "sha256:b87db4360013327109564f0e591bd2a3b318547bcef31b468a92ee504d07ae4f", + "sha256:bcb3ed405ed3222f9904899563d6fc492ff75cce56cba05e32eff40e6acbeaa3", + "sha256:d4306c36ca495956b6d568d276ac11fdd9c30a36f1b6eb928070dc5360b22e1c", + "sha256:d5ee4f386140395a2c818d149221149c54849dfcfcb9f1debfe07a8b8bd63f9a", + "sha256:dda30ba7e87fbbb7eab1ec9f58678558fd9a6b8b853530e176eabd064da81417", + "sha256:e04e26803c9c3851c931eac40c695602c6295b8d432cbe78609649ad9bd2da8a", + "sha256:e1c0b87e09fa55a220f058d1d49d3fb8df88fbfab58558f1198e08c1e1de842a", + "sha256:e72591e9ecd94d7feb70c1cbd7be7b3ebea3f548870aa91e2732960fa4d57a37", + "sha256:e8c843bbcda3a2f1e3c2ab25913c80a3c5376cd00c6e8c4a86a89a28c8dc5452", + "sha256:efc1913fd2ca4f334418481c7e595c00aad186563bbc1ec76067848c7ca0a933", + "sha256:f121a1420d4e173a5d96e47e9a0c0dcff965afdf1626d28de1460815f7c4ee7a", + "sha256:fc7b548b17d238737688817ab67deebb30e8073c95749d55538ed473130ec0c7" + ], + "markers": "python_version >= '3.7'", + "version": "==2.1.1" + }, + "matplotlib": { + "hashes": [ + "sha256:0bcdfcb0f976e1bac6721d7d457c17be23cf7501f977b6a38f9d38a3762841f7", + "sha256:1e64ac9be9da6bfff0a732e62116484b93b02a0b4d4b19934fb4f8e7ad26ad6a", + "sha256:22227c976ad4dc8c5a5057540421f0d8708c6560744ad2ad638d48e2984e1dbc", + "sha256:2886cc009f40e2984c083687251821f305d811d38e3df8ded414265e4583f0c5", + "sha256:2e6d184ebe291b9e8f7e78bbab7987d269c38ea3e062eace1fe7d898042ef804", + "sha256:3211ba82b9f1518d346f6309df137b50c3dc4421b4ed4815d1d7eadc617f45a1", + "sha256:339cac48b80ddbc8bfd05daae0a3a73414651a8596904c2a881cfd1edb65f26c", + "sha256:35a8ad4dddebd51f94c5d24bec689ec0ec66173bf614374a1244c6241c1595e0", + "sha256:3b4fa56159dc3c7f9250df88f653f085068bcd32dcd38e479bba58909254af7f", + "sha256:43e9d3fa077bf0cc95ded13d331d2156f9973dce17c6f0c8b49ccd57af94dbd9", + "sha256:57f1b4e69f438a99bb64d7f2c340db1b096b41ebaa515cf61ea72624279220ce", + "sha256:5c096363b206a3caf43773abebdbb5a23ea13faef71d701b21a9c27fdcef72f4", + "sha256:6bb93a0492d68461bd458eba878f52fdc8ac7bdb6c4acdfe43dba684787838c2", + "sha256:6ea6aef5c4338e58d8d376068e28f80a24f54e69f09479d1c90b7172bad9f25b", + "sha256:6fe807e8a22620b4cd95cfbc795ba310dc80151d43b037257250faf0bfcd82bc", + "sha256:73dd93dc35c85dece610cca8358003bf0760d7986f70b223e2306b4ea6d1406b", + "sha256:839d47b8ead7ad9669aaacdbc03f29656dc21f0d41a6fea2d473d856c39c8b1c", + "sha256:874df7505ba820e0400e7091199decf3ff1fde0583652120c50cd60d5820ca9a", + "sha256:879c7e5fce4939c6aa04581dfe08d57eb6102a71f2e202e3314d5fbc072fd5a0", + "sha256:94ff86af56a3869a4ae26a9637a849effd7643858a1a04dd5ee50e9ab75069a7", + "sha256:99482b83ebf4eb6d5fc6813d7aacdefdd480f0d9c0b52dcf9f1cc3b2c4b3361a", + "sha256:9ab29589cef03bc88acfa3a1490359000c18186fc30374d8aa77d33cc4a51a4a", + "sha256:9befa5954cdbc085e37d974ff6053da269474177921dd61facdad8023c4aeb51", + "sha256:a206a1b762b39398efea838f528b3a6d60cdb26fe9d58b48265787e29cd1d693", + "sha256:ab8d26f07fe64f6f6736d635cce7bfd7f625320490ed5bfc347f2cdb4fae0e56", + "sha256:b28de401d928890187c589036857a270a032961411934bdac4cf12dde3d43094", + "sha256:b428076a55fb1c084c76cb93e68006f27d247169f056412607c5c88828d08f88", + "sha256:bf618a825deb6205f015df6dfe6167a5d9b351203b03fab82043ae1d30f16511", + "sha256:c995f7d9568f18b5db131ab124c64e51b6820a92d10246d4f2b3f3a66698a15b", + "sha256:cd45a6f3e93a780185f70f05cf2a383daed13c3489233faad83e81720f7ede24", + "sha256:d2484b350bf3d32cae43f85dcfc89b3ed7bd2bcd781ef351f93eb6fb2cc483f9", + "sha256:d62880e1f60e5a30a2a8484432bcb3a5056969dc97258d7326ad465feb7ae069", + "sha256:dacddf5bfcec60e3f26ec5c0ae3d0274853a258b6c3fc5ef2f06a8eb23e042be", + "sha256:f3840c280ebc87a48488a46f760ea1c0c0c83fcf7abbe2e6baf99d033fd35fd8", + "sha256:f814504e459c68118bf2246a530ed953ebd18213dc20e3da524174d84ed010b2" + ], + "index": "pypi", + "version": "==3.5.3" + }, + "mdit-py-plugins": { + "hashes": [ + "sha256:b1279701cee2dbf50e188d3da5f51fee8d78d038cdf99be57c6b9d1aa93b4073", + "sha256:ecc24f51eeec6ab7eecc2f9724e8272c2fb191c2e93cf98109120c2cace69750" + ], + "markers": "python_version ~= '3.6'", + "version": "==0.3.0" + }, + "mdurl": { + "hashes": [ + "sha256:84008a41e51615a49fc9966191ff91509e3c40b939176e643fd50a5c2196b8f8", + "sha256:bb413d29f5eea38f31dd4754dd7377d4465116fb207585f97bf925588687c1ba" + ], + "markers": "python_version >= '3.7'", + "version": "==0.1.2" + }, + "mpld3": { + "hashes": [ + "sha256:1a167dbef836dd7c66d8aa71c06a32d50bffa18725f304d93cb74fdb3545043b", + "sha256:41938e65de4ba41a1b53d92e7c8609e7172e09b33ef5db42bb6f73701106c8b7" + ], + "index": "pypi", + "version": "==0.5.8" + }, + "mypy": { + "hashes": [ + "sha256:02ef476f6dcb86e6f502ae39a16b93285fef97e7f1ff22932b657d1ef1f28655", + "sha256:0d054ef16b071149917085f51f89555a576e2618d5d9dd70bd6eea6410af3ac9", + "sha256:19830b7dba7d5356d3e26e2427a2ec91c994cd92d983142cbd025ebe81d69cf3", + "sha256:1f7656b69974a6933e987ee8ffb951d836272d6c0f81d727f1d0e2696074d9e6", + "sha256:23488a14a83bca6e54402c2e6435467a4138785df93ec85aeff64c6170077fb0", + "sha256:23c7ff43fff4b0df93a186581885c8512bc50fc4d4910e0f838e35d6bb6b5e58", + "sha256:25c5750ba5609a0c7550b73a33deb314ecfb559c350bb050b655505e8aed4103", + "sha256:2ad53cf9c3adc43cf3bea0a7d01a2f2e86db9fe7596dfecb4496a5dda63cbb09", + "sha256:3fa7a477b9900be9b7dd4bab30a12759e5abe9586574ceb944bc29cddf8f0417", + "sha256:40b0f21484238269ae6a57200c807d80debc6459d444c0489a102d7c6a75fa56", + "sha256:4b21e5b1a70dfb972490035128f305c39bc4bc253f34e96a4adf9127cf943eb2", + "sha256:5a361d92635ad4ada1b1b2d3630fc2f53f2127d51cf2def9db83cba32e47c856", + "sha256:77a514ea15d3007d33a9e2157b0ba9c267496acf12a7f2b9b9f8446337aac5b0", + "sha256:855048b6feb6dfe09d3353466004490b1872887150c5bb5caad7838b57328cc8", + "sha256:9796a2ba7b4b538649caa5cecd398d873f4022ed2333ffde58eaf604c4d2cb27", + "sha256:98e02d56ebe93981c41211c05adb630d1d26c14195d04d95e49cd97dbc046dc5", + "sha256:b793b899f7cf563b1e7044a5c97361196b938e92f0a4343a5d27966a53d2ec71", + "sha256:d1ea5d12c8e2d266b5fb8c7a5d2e9c0219fedfeb493b7ed60cd350322384ac27", + "sha256:d2022bfadb7a5c2ef410d6a7c9763188afdb7f3533f22a0a32be10d571ee4bbe", + "sha256:d3348e7eb2eea2472db611486846742d5d52d1290576de99d59edeb7cd4a42ca", + "sha256:d744f72eb39f69312bc6c2abf8ff6656973120e2eb3f3ec4f758ed47e414a4bf", + "sha256:ef943c72a786b0f8d90fd76e9b39ce81fb7171172daf84bf43eaf937e9f220a9", + "sha256:f2899a3cbd394da157194f913a931edfd4be5f274a88041c9dc2d9cdcb1c315c" + ], + "index": "pypi", + "version": "==0.971" + }, + "mypy-extensions": { + "hashes": [ + "sha256:090fedd75945a69ae91ce1303b5824f428daf5a028d2f6ab8a299250a846f15d", + "sha256:2d82818f5bb3e369420cb3c4060a7970edba416647068eb4c5343488a6c604a8" + ], + "version": "==0.4.3" + }, + "myst-parser": { + "hashes": [ + "sha256:4965e51918837c13bf1c6f6fe2c6bddddf193148360fbdaefe743a4981358f6a", + "sha256:739a4d96773a8e55a2cacd3941ce46a446ee23dcd6b37e06f73f551ad7821d86" + ], + "index": "pypi", + "version": "==0.18.0" + }, + "natsort": { + "hashes": [ + "sha256:c7c1f3f27c375719a4dfcab353909fe39f26c2032a062a8c80cc844eaaca0445", + "sha256:f59988d2f24e77b6b56f8a8f882d5df6b3b637e09e075abc67b486d59fba1a4b" + ], + "index": "pypi", + "version": "==8.1.0" + }, + "nodeenv": { + "hashes": [ + "sha256:27083a7b96a25f2f5e1d8cb4b6317ee8aeda3bdd121394e5ac54e498028a042e", + "sha256:e0e7f7dfb85fc5394c6fe1e8fa98131a2473e04311a45afb6508f7cf1836fa2b" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4, 3.5, 3.6'", + "version": "==1.7.0" + }, + "numpy": { + "hashes": [ + "sha256:17e5226674f6ea79e14e3b91bfbc153fdf3ac13f5cc54ee7bc8fdbe820a32da0", + "sha256:2bd879d3ca4b6f39b7770829f73278b7c5e248c91d538aab1e506c628353e47f", + "sha256:4f41f5bf20d9a521f8cab3a34557cd77b6f205ab2116651f12959714494268b0", + "sha256:5593f67e66dea4e237f5af998d31a43e447786b2154ba1ad833676c788f37cde", + "sha256:5e28cd64624dc2354a349152599e55308eb6ca95a13ce6a7d5679ebff2962913", + "sha256:633679a472934b1c20a12ed0c9a6c9eb167fbb4cb89031939bfd03dd9dbc62b8", + "sha256:806970e69106556d1dd200e26647e9bee5e2b3f1814f9da104a943e8d548ca38", + "sha256:806cc25d5c43e240db709875e947076b2826f47c2c340a5a2f36da5bb10c58d6", + "sha256:8247f01c4721479e482cc2f9f7d973f3f47810cbc8c65e38fd1bbd3141cc9842", + "sha256:8ebf7e194b89bc66b78475bd3624d92980fca4e5bb86dda08d677d786fefc414", + "sha256:8ecb818231afe5f0f568c81f12ce50f2b828ff2b27487520d85eb44c71313b9e", + "sha256:8f9d84a24889ebb4c641a9b99e54adb8cab50972f0166a3abc14c3b93163f074", + "sha256:909c56c4d4341ec8315291a105169d8aae732cfb4c250fbc375a1efb7a844f8f", + "sha256:9b83d48e464f393d46e8dd8171687394d39bc5abfe2978896b77dc2604e8635d", + "sha256:ac987b35df8c2a2eab495ee206658117e9ce867acf3ccb376a19e83070e69418", + "sha256:b78d00e48261fbbd04aa0d7427cf78d18401ee0abd89c7559bbf422e5b1c7d01", + "sha256:b8b97a8a87cadcd3f94659b4ef6ec056261fa1e1c3317f4193ac231d4df70215", + "sha256:bd5b7ccae24e3d8501ee5563e82febc1771e73bd268eef82a1e8d2b4d556ae66", + "sha256:bdc02c0235b261925102b1bd586579b7158e9d0d07ecb61148a1799214a4afd5", + "sha256:be6b350dfbc7f708d9d853663772a9310783ea58f6035eec649fb9c4371b5389", + "sha256:c403c81bb8ffb1c993d0165a11493fd4bf1353d258f6997b3ee288b0a48fce77", + "sha256:cf8c6aed12a935abf2e290860af8e77b26a042eb7f2582ff83dc7ed5f963340c", + "sha256:d98addfd3c8728ee8b2c49126f3c44c703e2b005d4a95998e2167af176a9e722", + "sha256:dc76bca1ca98f4b122114435f83f1fcf3c0fe48e4e6f660e07996abf2f53903c", + "sha256:dec198619b7dbd6db58603cd256e092bcadef22a796f778bf87f8592b468441d", + "sha256:df28dda02c9328e122661f399f7655cdcbcf22ea42daa3650a26bce08a187450", + "sha256:e603ca1fb47b913942f3e660a15e55a9ebca906857edfea476ae5f0fe9b457d5", + "sha256:ecfdd68d334a6b97472ed032b5b37a30d8217c097acfff15e8452c710e775524" + ], + "index": "pypi", + "version": "==1.23.2" + }, + "opencv-python-headless": { + "hashes": [ + "sha256:21e70f8b0c04098cdf466d27184fe6c3820aaef944a22548db95099959c95889", + "sha256:2c032c373e447c3fc2a670bca20e2918a1205a6e72854df60425fd3f82c78c32", + "sha256:3bacd806cce1f1988e58f3d6f761538e0215d6621d316de94c009dc0acbd6ad3", + "sha256:d5291d7e10aa2c19cab6fd86f0d61af8617290ecd2d7ffcb051e446868d04cc5", + "sha256:e6c069bc963d7e8fcec21b3e33e594d35948badd63eccb2e80f88b0a12102c03", + "sha256:eec6281054346103d6af93f173b7c6a206beb2663d3adc04aa3ddc66e85093df", + "sha256:ffbf26fcd697af996408440a93bc69c49c05a36845771f984156dfbeaa95d497" + ], + "index": "pypi", + "version": "==4.6.0.66" + }, + "packaging": { + "hashes": [ + "sha256:dd47c42927d89ab911e606518907cc2d3a1f38bbd026385970643f9c5b8ecfeb", + "sha256:ef103e05f519cdc783ae24ea4e2e0f508a9c99b2d4969652eed6a2e1ea5bd522" + ], + "markers": "python_version >= '3.6'", + "version": "==21.3" + }, + "pandas": { + "hashes": [ + "sha256:07238a58d7cbc8a004855ade7b75bbd22c0db4b0ffccc721556bab8a095515f6", + "sha256:0daf876dba6c622154b2e6741f29e87161f844e64f84801554f879d27ba63c0d", + "sha256:16ad23db55efcc93fa878f7837267973b61ea85d244fc5ff0ccbcfa5638706c5", + "sha256:1d9382f72a4f0e93909feece6fef5500e838ce1c355a581b3d8f259839f2ea76", + "sha256:24ea75f47bbd5574675dae21d51779a4948715416413b30614c1e8b480909f81", + "sha256:2893e923472a5e090c2d5e8db83e8f907364ec048572084c7d10ef93546be6d1", + "sha256:2ff7788468e75917574f080cd4681b27e1a7bf36461fe968b49a87b5a54d007c", + "sha256:41fc406e374590a3d492325b889a2686b31e7a7780bec83db2512988550dadbf", + "sha256:48350592665ea3cbcd07efc8c12ff12d89be09cd47231c7925e3b8afada9d50d", + "sha256:605d572126eb4ab2eadf5c59d5d69f0608df2bf7bcad5c5880a47a20a0699e3e", + "sha256:6dfbf16b1ea4f4d0ee11084d9c026340514d1d30270eaa82a9f1297b6c8ecbf0", + "sha256:6f803320c9da732cc79210d7e8cc5c8019aad512589c910c66529eb1b1818230", + "sha256:721a3dd2f06ef942f83a819c0f3f6a648b2830b191a72bbe9451bcd49c3bd42e", + "sha256:755679c49460bd0d2f837ab99f0a26948e68fa0718b7e42afbabd074d945bf84", + "sha256:78b00429161ccb0da252229bcda8010b445c4bf924e721265bec5a6e96a92e92", + "sha256:958a0588149190c22cdebbc0797e01972950c927a11a900fe6c2296f207b1d6f", + "sha256:a3924692160e3d847e18702bb048dc38e0e13411d2b503fecb1adf0fcf950ba4", + "sha256:d51674ed8e2551ef7773820ef5dab9322be0828629f2cbf8d1fc31a0c4fed640", + "sha256:d5ebc990bd34f4ac3c73a2724c2dcc9ee7bf1ce6cf08e87bb25c6ad33507e318", + "sha256:d6c0106415ff1a10c326c49bc5dd9ea8b9897a6ca0c8688eb9c30ddec49535ef", + "sha256:e48fbb64165cda451c06a0f9e4c7a16b534fcabd32546d531b3c240ce2844112" + ], + "index": "pypi", + "version": "==1.4.3" + }, + "parameterized": { + "hashes": [ + "sha256:41bbff37d6186430f77f900d777e5bb6a24928a1c46fb1de692f8b52b8833b5c", + "sha256:9cbb0b69a03e8695d68b3399a8a5825200976536fe1cb79db60ed6a4c8c9efe9" + ], + "index": "pypi", + "version": "==0.8.1" + }, + "paramiko": { + "hashes": [ + "sha256:003e6bee7c034c21fbb051bf83dc0a9ee4106204dd3c53054c71452cc4ec3938", + "sha256:655f25dc8baf763277b933dfcea101d636581df8d6b9774d1fb653426b72c270" + ], + "index": "pypi", + "version": "==2.11.0" + }, + "pillow": { + "hashes": [ + "sha256:0030fdbd926fb85844b8b92e2f9449ba89607231d3dd597a21ae72dc7fe26927", + "sha256:030e3460861488e249731c3e7ab59b07c7853838ff3b8e16aac9561bb345da14", + "sha256:0ed2c4ef2451de908c90436d6e8092e13a43992f1860275b4d8082667fbb2ffc", + "sha256:136659638f61a251e8ed3b331fc6ccd124590eeff539de57c5f80ef3a9594e58", + "sha256:13b725463f32df1bfeacbf3dd197fb358ae8ebcd8c5548faa75126ea425ccb60", + "sha256:1536ad017a9f789430fb6b8be8bf99d2f214c76502becc196c6f2d9a75b01b76", + "sha256:15928f824870535c85dbf949c09d6ae7d3d6ac2d6efec80f3227f73eefba741c", + "sha256:17d4cafe22f050b46d983b71c707162d63d796a1235cdf8b9d7a112e97b15bac", + "sha256:1802f34298f5ba11d55e5bb09c31997dc0c6aed919658dfdf0198a2fe75d5490", + "sha256:1cc1d2451e8a3b4bfdb9caf745b58e6c7a77d2e469159b0d527a4554d73694d1", + "sha256:1fd6f5e3c0e4697fa7eb45b6e93996299f3feee73a3175fa451f49a74d092b9f", + "sha256:254164c57bab4b459f14c64e93df11eff5ded575192c294a0c49270f22c5d93d", + "sha256:2ad0d4df0f5ef2247e27fc790d5c9b5a0af8ade9ba340db4a73bb1a4a3e5fb4f", + "sha256:2c58b24e3a63efd22554c676d81b0e57f80e0a7d3a5874a7e14ce90ec40d3069", + "sha256:2d33a11f601213dcd5718109c09a52c2a1c893e7461f0be2d6febc2879ec2402", + "sha256:337a74fd2f291c607d220c793a8135273c4c2ab001b03e601c36766005f36885", + "sha256:37ff6b522a26d0538b753f0b4e8e164fdada12db6c6f00f62145d732d8a3152e", + "sha256:3d1f14f5f691f55e1b47f824ca4fdcb4b19b4323fe43cc7bb105988cad7496be", + "sha256:408673ed75594933714482501fe97e055a42996087eeca7e5d06e33218d05aa8", + "sha256:4134d3f1ba5f15027ff5c04296f13328fecd46921424084516bdb1b2548e66ff", + "sha256:4ad2f835e0ad81d1689f1b7e3fbac7b01bb8777d5a985c8962bedee0cc6d43da", + "sha256:50dff9cc21826d2977ef2d2a205504034e3a4563ca6f5db739b0d1026658e004", + "sha256:510cef4a3f401c246cfd8227b300828715dd055463cdca6176c2e4036df8bd4f", + "sha256:5aed7dde98403cd91d86a1115c78d8145c83078e864c1de1064f52e6feb61b20", + "sha256:69bd1a15d7ba3694631e00df8de65a8cb031911ca11f44929c97fe05eb9b6c1d", + "sha256:6bf088c1ce160f50ea40764f825ec9b72ed9da25346216b91361eef8ad1b8f8c", + "sha256:6e8c66f70fb539301e064f6478d7453e820d8a2c631da948a23384865cd95544", + "sha256:727dd1389bc5cb9827cbd1f9d40d2c2a1a0c9b32dd2261db522d22a604a6eec9", + "sha256:74a04183e6e64930b667d321524e3c5361094bb4af9083db5c301db64cd341f3", + "sha256:75e636fd3e0fb872693f23ccb8a5ff2cd578801251f3a4f6854c6a5d437d3c04", + "sha256:7761afe0126d046974a01e030ae7529ed0ca6a196de3ec6937c11df0df1bc91c", + "sha256:7888310f6214f19ab2b6df90f3f06afa3df7ef7355fc025e78a3044737fab1f5", + "sha256:7b0554af24df2bf96618dac71ddada02420f946be943b181108cac55a7a2dcd4", + "sha256:7c7b502bc34f6e32ba022b4a209638f9e097d7a9098104ae420eb8186217ebbb", + "sha256:808add66ea764ed97d44dda1ac4f2cfec4c1867d9efb16a33d158be79f32b8a4", + "sha256:831e648102c82f152e14c1a0938689dbb22480c548c8d4b8b248b3e50967b88c", + "sha256:93689632949aff41199090eff5474f3990b6823404e45d66a5d44304e9cdc467", + "sha256:96b5e6874431df16aee0c1ba237574cb6dff1dcb173798faa6a9d8b399a05d0e", + "sha256:9a54614049a18a2d6fe156e68e188da02a046a4a93cf24f373bffd977e943421", + "sha256:a138441e95562b3c078746a22f8fca8ff1c22c014f856278bdbdd89ca36cff1b", + "sha256:a647c0d4478b995c5e54615a2e5360ccedd2f85e70ab57fbe817ca613d5e63b8", + "sha256:a9c9bc489f8ab30906d7a85afac4b4944a572a7432e00698a7239f44a44e6efb", + "sha256:ad2277b185ebce47a63f4dc6302e30f05762b688f8dc3de55dbae4651872cdf3", + "sha256:b6d5e92df2b77665e07ddb2e4dbd6d644b78e4c0d2e9272a852627cdba0d75cf", + "sha256:bc431b065722a5ad1dfb4df354fb9333b7a582a5ee39a90e6ffff688d72f27a1", + "sha256:bdd0de2d64688ecae88dd8935012c4a72681e5df632af903a1dca8c5e7aa871a", + "sha256:c79698d4cd9318d9481d89a77e2d3fcaeff5486be641e60a4b49f3d2ecca4e28", + "sha256:cb6259196a589123d755380b65127ddc60f4c64b21fc3bb46ce3a6ea663659b0", + "sha256:d5b87da55a08acb586bad5c3aa3b86505f559b84f39035b233d5bf844b0834b1", + "sha256:dcd7b9c7139dc8258d164b55696ecd16c04607f1cc33ba7af86613881ffe4ac8", + "sha256:dfe4c1fedfde4e2fbc009d5ad420647f7730d719786388b7de0999bf32c0d9fd", + "sha256:ea98f633d45f7e815db648fd7ff0f19e328302ac36427343e4432c84432e7ff4", + "sha256:ec52c351b35ca269cb1f8069d610fc45c5bd38c3e91f9ab4cbbf0aebc136d9c8", + "sha256:eef7592281f7c174d3d6cbfbb7ee5984a671fcd77e3fc78e973d492e9bf0eb3f", + "sha256:f07f1f00e22b231dd3d9b9208692042e29792d6bd4f6639415d2f23158a80013", + "sha256:f3fac744f9b540148fa7715a435d2283b71f68bfb6d4aae24482a890aed18b59", + "sha256:fa768eff5f9f958270b081bb33581b4b569faabf8774726b283edb06617101dc", + "sha256:fac2d65901fb0fdf20363fbd345c01958a742f2dc62a8dd4495af66e3ff502a4" + ], + "index": "pypi", + "version": "==9.2.0" + }, + "platformdirs": { + "hashes": [ + "sha256:027d8e83a2d7de06bbac4e5ef7e023c02b863d7ea5d079477e722bb41ab25788", + "sha256:58c8abb07dcb441e6ee4b11d8df0ac856038f944ab98b7be6b27b2a3c7feef19" + ], + "markers": "python_version >= '3.7'", + "version": "==2.5.2" + }, + "pluggy": { + "hashes": [ + "sha256:4224373bacce55f955a878bf9cfa763c1e360858e330072059e10bad68531159", + "sha256:74134bbf457f031a36d68416e1509f34bd5ccc019f0bcc952c7b909d06b37bd3" + ], + "markers": "python_version >= '3.6'", + "version": "==1.0.0" + }, + "pprofile": { + "hashes": [ + "sha256:b2bb56603dadf40c0bc0f61621f22c20e41638425f729945d9b7f8e4ae8cdd4a" + ], + "index": "pypi", + "version": "==2.1.0" + }, + "pre-commit": { + "hashes": [ + "sha256:51a5ba7c480ae8072ecdb6933df22d2f812dc897d5fe848778116129a681aac7", + "sha256:a978dac7bc9ec0bcee55c18a277d553b0f419d259dadb4b9418ff2d00eb43959" + ], + "index": "pypi", + "version": "==2.20.0" + }, + "py": { + "hashes": [ + "sha256:51c75c4126074b472f746a24399ad32f6053d1b34b68d2fa41e558e6f4a98719", + "sha256:607c53218732647dff4acdfcd50cb62615cedf612e72d1724fb1a0cc6405b378" + ], + "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'", + "version": "==1.11.0" + }, + "pycparser": { + "hashes": [ + "sha256:8ee45429555515e1f6b185e78100aea234072576aa43ab53aefcae078162fca9", + "sha256:e644fdec12f7872f86c58ff790da456218b10f863970249516d60a5eaca77206" + ], + "version": "==2.21" + }, + "pycurl": { + "hashes": [ + "sha256:a863ad18ff478f5545924057887cdae422e1b2746e41674615f687498ea5b88a" + ], + "index": "pypi", + "version": "==7.45.1" + }, + "pygame": { + "hashes": [ + "sha256:0427c103f741234336e5606d2fad86f5403c1a3d1dc55c309fbff3c984f0c9ae", + "sha256:07ca9f683075aea9bd977af9f09a720ebf747343d3ea8103e4f1735283b02330", + "sha256:0e06ae8e1c830f1b9c36a2bc6bb11de840232e95b78e2c349c6ed803a303be19", + "sha256:0e97d38308c441942577fea7fcd1326308bc56d6be6c024218e94d075d322e0f", + "sha256:119dee20c372c85dc47b717119534d15a60c64ceab8b0eb09278866d10486afe", + "sha256:1219a963941bd53aa754e8449364c142004fe706c33a9c22ff2a76521a82d078", + "sha256:1fddec8829e96424800c806582d73a5173b7d48946cccf7d035839ca09850db8", + "sha256:20676da24e3e3e6b9fc4eecc7ba09d77ef46c3a83a028763ba1d222476c2e3fb", + "sha256:2405414d8c572668e04739875661e030a0c588e197fa95463fe301c3d0a0510b", + "sha256:24254c4244f0d9bdc904f5d3f38e86757ca4c6aa0e44a6d55ef5e016bc7274d6", + "sha256:24b4f7f30fa2b3d092b60be6fcc725fb91d569fc87a9bcc91614ee8b0c005726", + "sha256:3bb0674aa789848ddc264bfc60c54965bf3bb659c141de4f600e379acc9b944c", + "sha256:3c8d6637ff75351e581327efefa9d04eeb0f257b533392b6cc6b15ceca4f7c5e", + "sha256:40e4d8d65985bb467d9c5a1305fb53fd6820c61d764979600becab973339676f", + "sha256:4aa3ae32320cc704d63e185864e44f6265c2a6e52c9384afe152cc3d51b3a2ef", + "sha256:50d9a21edd551669862c27c9272747401b20b1939abaacb842c08ea1cdd1c04d", + "sha256:5c7600bf307de1ca1dca0cc7840e34604d5b0b0a5a5dad345c3fa62b054b886d", + "sha256:5d0c14152d0ca8ef5fbcc5ed9981462bdf59a9ae85a291e62d8a8d0b7e5cbe43", + "sha256:5e88b0d4338b94960686f59396f23f7f684fed4859fcc3b9f40286d72c1c61af", + "sha256:5ebbefb8b576572c8fc97a3321d37dc2b4afea6b6e3877a67f7158d8c2c4cefe", + "sha256:636f51f56615d67459b11918206bb4da30cd7d7042027bf997c218ccd6c77902", + "sha256:660c80c0b2e80f1f801715583b759fb4c7bc0c11eb3b534e89c9fc4bfbc38acd", + "sha256:6ecda8dd4583982bb65f9c682f244a5e94524dcf628379766227e9ed97201a49", + "sha256:754c2906f2ef47173a14493e1de116b2a56a2c8e1764f1202ba844d080248a5b", + "sha256:7889dce887ec83c9a0bef8d9eb3669d8863fdaf37c45bacec707d8ad90b24a38", + "sha256:7fdb93b4282962c9a2ebf1af994ee698be823dd913218ed97a5f2fb372b10b66", + "sha256:8e87716114e97322fb177e223d889a4be369a0f73212f4f8507fe0cd43253b23", + "sha256:93c4cbfc942dd00410eaa9e84252129f9f9993f37f683006d7b49ab245342254", + "sha256:9649419254d3282dae41f23837de4108b17bc62187c3acd8af2ae3801b765cbd", + "sha256:97a74ba186deee68318a52637012ef6abf5be6282c659e1d1ba6ad08cf35ec85", + "sha256:9d6452419e01a0f848aed0597f69fd10a4c2a7750c15d1b0607f86090a39dcf3", + "sha256:9d7b021b8dde5d528363e474bc18bd6f79a9666eef89fb4859bcb8f0a536c9de", + "sha256:a0ccf8e3dce7ca67d523a6020b7e3dbf4b26797a9a8db5cc4c7b5ef20fb64701", + "sha256:a56a811d8821f7b9a594e3d0e0dd8bd39b25e3eea8963d5963263b90fd2ea5c2", + "sha256:c5ea87da5fe4b6164c3854f3b0c9146811dbad0dd7fa74297683dfacc485ae1c", + "sha256:c99b95e62cdda29c2e60235d7763447c168a6a877403e6f9ca5b2e2bb297c2ce", + "sha256:c9ce7f3d8af14d7e04eb7eb41c5e5313c43508c252bb2b9eb53e51fc87ada9fd", + "sha256:ca5ef1315fa67c241a657ab077be44f230c05740c95f0b46409457dceefdc7e5", + "sha256:d2d3c50ee9847b743db6cd7b1bb17a94c2c2abc16679d70f5e745cabdf19e655", + "sha256:d6d0eca28f886f0477cd0721ac688189155a587f2bb8eae740e52ca56c3ad23c", + "sha256:dad6bf3fdd3752d7519422f3732be779b98fe7c87d32c3efe2fdffdcbeebb6ca", + "sha256:db2f40d5a75fd9cdda473c58b0d8b294da6e0179f00bb3b1fc2f7f29cac09bea", + "sha256:dc4444d61d48c5546df5137cdf81554887ddb6e2ef1be7f51eb77ea3b6bdd56f", + "sha256:dcc285ee1f1d0e2672cc52f880fd3f564b1505de710e817f692fbf64a72ca657", + "sha256:dd528dbb91eca16f7522c975d0f9e94b95f6b5024c82c3247dc0383d242d33c6", + "sha256:e09044e9e1aa8512d6a9c7ce5f94b881824bcfc401105f3c24f546dfc3bb4aa5", + "sha256:e18c9466131378421d00fc40b637425229238d506a073d9c537b230b355a25d6", + "sha256:e1bb25986db77a48f632469c6bc61baf7508ce945aa6161c02180d4ee5ac5b8d", + "sha256:e4b4cd440d50a9f8551b8989e856aab175593af07eb825cad22fd2f8f6f2ffce", + "sha256:e627300a66a90651fb39e41601d447b1fdbbfffca3f08ef0278d6cc0436b2160", + "sha256:e7a8e18677e0064b7a422f6653a622652d932826a27e50f279d55a8b122a1a83", + "sha256:e8632f6b2ddb90f6f3950744bd65d5ef15af615e3034057fa30ff836f48a7179", + "sha256:ea36f4f93524554a35cac2359df63b50af6556ed866830aa1f07f0d8580280ea", + "sha256:f149e182d0eeef15d8a9b4c9dad1b87dc6eba3a99bd3c44a777a3a2b053a3dca", + "sha256:fc2e5db54491e8f27785fc5204c96f540d3557dcf5b0a9a857b6594d6b32561b", + "sha256:fc30e834f65b893d1b4c230070183bf98e6b70c41c1511687e8436a33d5ce49d", + "sha256:fcc9586e17875c0cdf8764597955f9daa979098fd4f80be07ed68276ac225480", + "sha256:ff961c3280d6ee5f4163f4772f963d7a4dbe42e36c6dd54b79ad436c1f046e5d" + ], + "index": "pypi", + "version": "==2.1.2" + }, + "pygments": { + "hashes": [ + "sha256:56a8508ae95f98e2b9bdf93a6be5ae3f7d8af858b43e02c5a2ff083726be40c1", + "sha256:f643f331ab57ba3c9d89212ee4a2dabc6e94f117cf4eefde99a0574720d14c42" + ], + "markers": "python_version >= '3.6'", + "version": "==2.13.0" + }, + "pynacl": { + "hashes": [ + "sha256:06b8f6fa7f5de8d5d2f7573fe8c863c051225a27b61e6860fd047b1775807858", + "sha256:0c84947a22519e013607c9be43706dd42513f9e6ae5d39d3613ca1e142fba44d", + "sha256:20f42270d27e1b6a29f54032090b972d97f0a1b0948cc52392041ef7831fee93", + "sha256:401002a4aaa07c9414132aaed7f6836ff98f59277a234704ff66878c2ee4a0d1", + "sha256:52cb72a79269189d4e0dc537556f4740f7f0a9ec41c1322598799b0bdad4ef92", + "sha256:61f642bf2378713e2c2e1de73444a3778e5f0a38be6fee0fe532fe30060282ff", + "sha256:8ac7448f09ab85811607bdd21ec2464495ac8b7c66d146bf545b0f08fb9220ba", + "sha256:a36d4a9dda1f19ce6e03c9a784a2921a4b726b02e1c736600ca9c22029474394", + "sha256:a422368fc821589c228f4c49438a368831cb5bbc0eab5ebe1d7fac9dded6567b", + "sha256:e46dae94e34b085175f8abb3b0aaa7da40767865ac82c928eeb9e57e1ea8a543" + ], + "markers": "python_version >= '3.6'", + "version": "==1.5.0" + }, + "pyparsing": { + "hashes": [ + "sha256:2b020ecf7d21b687f219b71ecad3631f644a47f01403fa1d1036b0c6416d70fb", + "sha256:5026bae9a10eeaefb61dab2f09052b9f4307d44aee4eda64b309723d8d206bbc" + ], + "markers": "python_full_version >= '3.6.8'", + "version": "==3.0.9" + }, + "pyprof2calltree": { + "hashes": [ + "sha256:a635672ff31677486350b2be9a823ef92f740e6354a6aeda8fa4a8a3768e8f2f" + ], + "index": "pypi", + "version": "==1.4.5" + }, + "pytest": { + "hashes": [ + "sha256:13d0e3ccfc2b6e26be000cb6568c832ba67ba32e719443bfe725814d3c42433c", + "sha256:a06a0425453864a270bc45e71f783330a7428defb4230fb5e6a731fde06ecd45" + ], + "index": "pypi", + "version": "==7.1.2" + }, + "pytest-forked": { + "hashes": [ + "sha256:8b67587c8f98cbbadfdd804539ed5455b6ed03802203485dd2f53c1422d7440e", + "sha256:bbbb6717efc886b9d64537b41fb1497cfaf3c9601276be8da2cccfea5a3c8ad8" + ], + "markers": "python_version >= '3.6'", + "version": "==1.4.0" + }, + "pytest-xdist": { + "hashes": [ + "sha256:4580deca3ff04ddb2ac53eba39d76cb5dd5edeac050cb6fbc768b0dd712b4edf", + "sha256:6fe5c74fec98906deb8f2d2b616b5c782022744978e7bd4695d39c8f42d0ce65" + ], + "index": "pypi", + "version": "==2.5.0" + }, + "python-dateutil": { + "hashes": [ + "sha256:0123cacc1627ae19ddf3c27a5de5bd67ee4586fbdd6440d9748f8abb483d3e86", + "sha256:961d03dc3453ebbc59dbdea9e4e11c5651520a876d0f4db161e8674aae935da9" + ], + "index": "pypi", + "version": "==2.8.2" + }, + "pytz": { + "hashes": [ + "sha256:220f481bdafa09c3955dfbdddb7b57780e9a94f5127e35456a48589b9e0c0197", + "sha256:cea221417204f2d1a2aa03ddae3e867921971d0d76f14d87abb4414415bbdcf5" + ], + "version": "==2022.2.1" + }, + "pyyaml": { + "hashes": [ + "sha256:0283c35a6a9fbf047493e3a0ce8d79ef5030852c51e9d911a27badfde0605293", + "sha256:055d937d65826939cb044fc8c9b08889e8c743fdc6a32b33e2390f66013e449b", + "sha256:07751360502caac1c067a8132d150cf3d61339af5691fe9e87803040dbc5db57", + "sha256:0b4624f379dab24d3725ffde76559cff63d9ec94e1736b556dacdfebe5ab6d4b", + "sha256:0ce82d761c532fe4ec3f87fc45688bdd3a4c1dc5e0b4a19814b9009a29baefd4", + "sha256:1e4747bc279b4f613a09eb64bba2ba602d8a6664c6ce6396a4d0cd413a50ce07", + "sha256:213c60cd50106436cc818accf5baa1aba61c0189ff610f64f4a3e8c6726218ba", + "sha256:231710d57adfd809ef5d34183b8ed1eeae3f76459c18fb4a0b373ad56bedcdd9", + "sha256:277a0ef2981ca40581a47093e9e2d13b3f1fbbeffae064c1d21bfceba2030287", + "sha256:2cd5df3de48857ed0544b34e2d40e9fac445930039f3cfe4bcc592a1f836d513", + "sha256:40527857252b61eacd1d9af500c3337ba8deb8fc298940291486c465c8b46ec0", + "sha256:473f9edb243cb1935ab5a084eb238d842fb8f404ed2193a915d1784b5a6b5fc0", + "sha256:48c346915c114f5fdb3ead70312bd042a953a8ce5c7106d5bfb1a5254e47da92", + "sha256:50602afada6d6cbfad699b0c7bb50d5ccffa7e46a3d738092afddc1f9758427f", + "sha256:68fb519c14306fec9720a2a5b45bc9f0c8d1b9c72adf45c37baedfcd949c35a2", + "sha256:77f396e6ef4c73fdc33a9157446466f1cff553d979bd00ecb64385760c6babdc", + "sha256:819b3830a1543db06c4d4b865e70ded25be52a2e0631ccd2f6a47a2822f2fd7c", + "sha256:897b80890765f037df3403d22bab41627ca8811ae55e9a722fd0392850ec4d86", + "sha256:98c4d36e99714e55cfbaaee6dd5badbc9a1ec339ebfc3b1f52e293aee6bb71a4", + "sha256:9df7ed3b3d2e0ecfe09e14741b857df43adb5a3ddadc919a2d94fbdf78fea53c", + "sha256:9fa600030013c4de8165339db93d182b9431076eb98eb40ee068700c9c813e34", + "sha256:a80a78046a72361de73f8f395f1f1e49f956c6be882eed58505a15f3e430962b", + "sha256:b3d267842bf12586ba6c734f89d1f5b871df0273157918b0ccefa29deb05c21c", + "sha256:b5b9eccad747aabaaffbc6064800670f0c297e52c12754eb1d976c57e4f74dcb", + "sha256:c5687b8d43cf58545ade1fe3e055f70eac7a5a1a0bf42824308d868289a95737", + "sha256:cba8c411ef271aa037d7357a2bc8f9ee8b58b9965831d9e51baf703280dc73d3", + "sha256:d15a181d1ecd0d4270dc32edb46f7cb7733c7c508857278d3d378d14d606db2d", + "sha256:d4db7c7aef085872ef65a8fd7d6d09a14ae91f691dec3e87ee5ee0539d516f53", + "sha256:d4eccecf9adf6fbcc6861a38015c2a64f38b9d94838ac1810a9023a0609e1b78", + "sha256:d67d839ede4ed1b28a4e8909735fc992a923cdb84e618544973d7dfc71540803", + "sha256:daf496c58a8c52083df09b80c860005194014c3698698d1a57cbcfa182142a3a", + "sha256:e61ceaab6f49fb8bdfaa0f92c4b57bcfbea54c09277b1b4f7ac376bfb7a7c174", + "sha256:f84fbc98b019fef2ee9a1cb3ce93e3187a6df0b2538a651bfb890254ba9f90b5" + ], + "index": "pypi", + "version": "==6.0" + }, + "requests": { + "hashes": [ + "sha256:7c5599b102feddaa661c826c56ab4fee28bfd17f5abca1ebbe3e7f19d7c97983", + "sha256:8fefa2a1a1365bf5520aac41836fbee479da67864514bdb821f31ce07ce65349" + ], + "index": "pypi", + "version": "==2.28.1" + }, + "reverse-geocoder": { + "hashes": [ + "sha256:2a2e781b5f69376d922b78fe8978f1350c84fce0ddb07e02c834ecf98b57c75c" + ], + "index": "pypi", + "version": "==1.5.1" + }, + "scipy": { + "hashes": [ + "sha256:0419485dbcd0ed78c0d5bf234c5dd63e86065b39b4d669e45810d42199d49521", + "sha256:09412eb7fb60b8f00b328037fd814d25d261066ebc43a1e339cdce4f7502877e", + "sha256:26d28c468900e6d5fdb37d2812ab46db0ccd22c63baa095057871faa3a498bc9", + "sha256:34441dfbee5b002f9e15285014fd56e5e3372493c3e64ae297bae2c4b9659f5a", + "sha256:39ab9240cd215a9349c85ab908dda6d732f7d3b4b192fa05780812495536acc4", + "sha256:3bc1ab68b9a096f368ba06c3a5e1d1d50957a86665fc929c4332d21355e7e8f4", + "sha256:3c6f5d1d4b9a5e4fe5e14f26ffc9444fc59473bbf8d45dc4a9a15283b7063a72", + "sha256:47d1a95bd9d37302afcfe1b84c8011377c4f81e33649c5a5785db9ab827a6ade", + "sha256:71487c503e036740635f18324f62a11f283a632ace9d35933b2b0a04fd898c98", + "sha256:7a412c476a91b080e456229e413792bbb5d6202865dae963d1e6e28c2bb58691", + "sha256:825951b88f56765aeb6e5e38ac9d7d47407cfaaeb008d40aa1b45a2d7ea2731e", + "sha256:8cc81ac25659fec73599ccc52c989670e5ccd8974cf34bacd7b54a8d809aff1a", + "sha256:8d3faa40ac16c6357aaf7ea50394ea6f1e8e99d75e927a51102b1943b311b4d9", + "sha256:90c805f30c46cf60f1e76e947574f02954d25e3bb1e97aa8a07bc53aa31cf7d1", + "sha256:96d7cf7b25c9f23c59a766385f6370dab0659741699ecc7a451f9b94604938ce", + "sha256:b97b479f39c7e4aaf807efd0424dec74bbb379108f7d22cf09323086afcd312c", + "sha256:bc4e2c77d4cd015d739e75e74ebbafed59ba8497a7ed0fd400231ed7683497c4", + "sha256:c61b4a91a702e8e04aeb0bfc40460e1f17a640977c04dda8757efb0199c75332", + "sha256:d79da472015d0120ba9b357b28a99146cd6c17b9609403164b1a8ed149b4dfc8", + "sha256:e8fe305d9d67a81255e06203454729405706907dccbdfcc330b7b3482a6c371d", + "sha256:eb954f5aca4d26f468bbebcdc5448348eb287f7bea536c6306f62ea062f63d9a", + "sha256:f7c39f7dbb57cce00c108d06d731f3b0e2a4d3a95c66d96bce697684876ce4d4", + "sha256:f950a04b33e17b38ff561d5a0951caf3f5b47caa841edd772ffb7959f20a6af0" + ], + "index": "pypi", + "version": "==1.9.1" + }, + "setuptools": { + "hashes": [ + "sha256:2e24e0bec025f035a2e72cdd1961119f557d78ad331bb00ff82efb2ab8da8e82", + "sha256:7732871f4f7fa58fb6bdcaeadb0161b2bd046c85905dbaa066bdcbcc81953b57" + ], + "markers": "python_version >= '3.7'", + "version": "==65.3.0" + }, + "six": { + "hashes": [ + "sha256:1e61c37477a1626458e36f7b1d82aa5c9b094fa4802892072e49de9c60c4c926", + "sha256:8abb2f1d86890a2dfb989f9a77cfcfd3e47c2a354b01111771326f8aa26e0254" + ], + "index": "pypi", + "version": "==1.16.0" + }, + "snowballstemmer": { + "hashes": [ + "sha256:09b16deb8547d3412ad7b590689584cd0fe25ec8db3be37788be3810cbf19cb1", + "sha256:c8e1716e83cc398ae16824e5572ae04e0d9fc2c6b985fb0f900f5f0c96ecba1a" + ], + "version": "==2.2.0" + }, + "sortedcontainers": { + "hashes": [ + "sha256:25caa5a06cc30b6b83d11423433f65d1f9d76c4c6a0c90e3379eaa43b9bfdb88", + "sha256:a163dcaede0f1c021485e957a39245190e74249897e2ae4b2aa38595db237ee0" + ], + "version": "==2.4.0" + }, + "sphinx": { + "hashes": [ + "sha256:309a8da80cb6da9f4713438e5b55861877d5d7976b69d87e336733637ea12693", + "sha256:ba3224a4e206e1fbdecf98a4fae4992ef9b24b85ebf7b584bb340156eaf08d89" + ], + "index": "pypi", + "version": "==5.1.1" + }, + "sphinx-rtd-theme": { + "hashes": [ + "sha256:4d35a56f4508cfee4c4fb604373ede6feae2a306731d533f409ef5c3496fdbd8", + "sha256:eec6d497e4c2195fa0e8b2016b337532b8a699a68bcb22a512870e16925c6a5c" + ], + "index": "pypi", + "version": "==1.0.0" + }, + "sphinx-sitemap": { + "hashes": [ + "sha256:65adda39233cb17c0da10ba1cebaa2df73e271cdb6f8efd5cec8eef3b3cf7737" + ], + "index": "pypi", + "version": "==2.2.0" + }, + "sphinxcontrib-applehelp": { + "hashes": [ + "sha256:806111e5e962be97c29ec4c1e7fe277bfd19e9652fb1a4392105b43e01af885a", + "sha256:a072735ec80e7675e3f432fcae8610ecf509c5f1869d17e2eecff44389cdbc58" + ], + "markers": "python_version >= '3.5'", + "version": "==1.0.2" + }, + "sphinxcontrib-devhelp": { + "hashes": [ + "sha256:8165223f9a335cc1af7ffe1ed31d2871f325254c0423bc0c4c7cd1c1e4734a2e", + "sha256:ff7f1afa7b9642e7060379360a67e9c41e8f3121f2ce9164266f61b9f4b338e4" + ], + "markers": "python_version >= '3.5'", + "version": "==1.0.2" + }, + "sphinxcontrib-htmlhelp": { + "hashes": [ + "sha256:d412243dfb797ae3ec2b59eca0e52dac12e75a241bf0e4eb861e450d06c6ed07", + "sha256:f5f8bb2d0d629f398bf47d0d69c07bc13b65f75a81ad9e2f71a63d4b7a2f6db2" + ], + "markers": "python_version >= '3.6'", + "version": "==2.0.0" + }, + "sphinxcontrib-jsmath": { + "hashes": [ + "sha256:2ec2eaebfb78f3f2078e73666b1415417a116cc848b72e5172e596c871103178", + "sha256:a9925e4a4587247ed2191a22df5f6970656cb8ca2bd6284309578f2153e0c4b8" + ], + "markers": "python_version >= '3.5'", + "version": "==1.0.1" + }, + "sphinxcontrib-qthelp": { + "hashes": [ + "sha256:4c33767ee058b70dba89a6fc5c1892c0d57a54be67ddd3e7875a18d14cba5a72", + "sha256:bd9fc24bcb748a8d51fd4ecaade681350aa63009a347a8c14e637895444dfab6" + ], + "markers": "python_version >= '3.5'", + "version": "==1.0.3" + }, + "sphinxcontrib-serializinghtml": { + "hashes": [ + "sha256:352a9a00ae864471d3a7ead8d7d79f5fc0b57e8b3f95e9867eb9eb28999b92fd", + "sha256:aa5f6de5dfdf809ef505c4895e51ef5c9eac17d0f287933eb49ec495280b6952" + ], + "markers": "python_version >= '3.5'", + "version": "==1.1.5" + }, + "subprocess32": { + "hashes": [ + "sha256:88e37c1aac5388df41cc8a8456bb49ebffd321a3ad4d70358e3518176de3a56b", + "sha256:e45d985aef903c5b7444d34350b05da91a9e0ea015415ab45a21212786c649d0", + "sha256:eb2937c80497978d181efa1b839ec2d9622cf9600a039a79d0e108d1f9aec79d" + ], + "index": "pypi", + "version": "==3.5.4" + }, + "tabulate": { + "hashes": [ + "sha256:0ba055423dbaa164b9e456abe7920c5e8ed33fcc16f6d1b2f2d152c8e1e8b4fc", + "sha256:436f1c768b424654fce8597290d2764def1eea6a77cfa5c33be00b1bc0f4f63d", + "sha256:6c57f3f3dd7ac2782770155f3adb2db0b1a269637e42f27599925e64b114f519" + ], + "index": "pypi", + "version": "==0.8.10" + }, + "tenacity": { + "hashes": [ + "sha256:43242a20e3e73291a28bcbcacfd6e000b02d3857a9a9fff56b297a27afdc932f", + "sha256:f78f4ea81b0fabc06728c11dc2a8c01277bfc5181b321a4770471902e3eb844a" + ], + "index": "pypi", + "version": "==8.0.1" + }, + "toml": { + "hashes": [ + "sha256:806143ae5bfb6a3c6e736a764057db0e6a0e05e338b5630894a5f779cabb4f9b", + "sha256:b3bda1d108d5dd99f4a20d24d9c348e91c4db7ab1b749200bded2f839ccbe68f" + ], + "markers": "python_version >= '2.6' and python_version not in '3.0, 3.1, 3.2, 3.3'", + "version": "==0.10.2" + }, + "tomli": { + "hashes": [ + "sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc", + "sha256:de526c12914f0c550d15924c62d72abc48d6fe7364aa87328337a31007fe8a4f" + ], + "markers": "python_version < '3.11'", + "version": "==2.0.1" + }, + "typing-extensions": { + "hashes": [ + "sha256:25642c956049920a5aa49edcdd6ab1e06d7e5d467fc00e0506c44ac86fbfca02", + "sha256:e6d2677a32f47fc7eb2795db1dd15c1f34eff616bcaf2cfb5e997f854fa1c4a6" + ], + "markers": "python_version < '3.10'", + "version": "==4.3.0" + }, + "urllib3": { + "hashes": [ + "sha256:3fa96cf423e6987997fc326ae8df396db2a8b7c667747d47ddd8ecba91f4a74e", + "sha256:b930dd878d5a8afb066a637fbb35144fe7901e3b209d1cd4f524bd0e9deee997" + ], + "index": "pypi", + "version": "==1.26.12" + }, + "virtualenv": { + "hashes": [ + "sha256:4193b7bc8a6cd23e4eb251ac64f29b4398ab2c233531e66e40b19a6b7b0d30c1", + "sha256:d86ea0bb50e06252d79e6c241507cb904fcd66090c3271381372d6221a3970f9" + ], + "markers": "python_version >= '3.6'", + "version": "==20.16.3" + }, + "zipp": { + "hashes": [ + "sha256:05b45f1ee8f807d0cc928485ca40a07cb491cf092ff587c0df9cb1fd154848d2", + "sha256:47c40d7fe183a6f21403a199b3e4192cca5774656965b0a4988ad2f8feb5f009" + ], + "markers": "python_version >= '3.7'", + "version": "==3.8.1" + } + } +} diff --git a/README.md b/README.md old mode 100644 new mode 100755 index a77a80935d59ae..cfeb625bfebd6f --- a/README.md +++ b/README.md @@ -1,92 +1,134 @@ -
+![](https://i.imgur.com/b0ZyIx5.jpg) -

openpilot

+Table of Contents +======================= -

- openpilot is an operating system for robotics. -
- Currently, it upgrades the driver assistance system in 300+ supported cars. -

+* [What is openpilot?](#what-is-openpilot) +* [Running in a car](#running-on-a-dedicated-device-in-a-car) +* [Running on PC](#running-on-pc) +* [Community and Contributing](#community-and-contributing) +* [User Data and comma Account](#user-data-and-comma-account) +* [Safety and Testing](#safety-and-testing) +* [Directory Structure](#directory-structure) +* [Licensing](#licensing) -

- Docs - · - Roadmap - · - Contribute - · - Community - · - Try it on a comma 3X -

+--- -Quick start: `bash <(curl -fsSL openpilot.comma.ai)` - -[![openpilot tests](https://github.com/commaai/openpilot/actions/workflows/tests.yaml/badge.svg)](https://github.com/commaai/openpilot/actions/workflows/tests.yaml) -[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](LICENSE) -[![X Follow](https://img.shields.io/twitter/follow/comma_ai)](https://x.com/comma_ai) -[![Discord](https://img.shields.io/discord/469524606043160576)](https://discord.comma.ai) +What is openpilot? +------ -
+[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW), and Lane Departure Warning (LDW) for a growing variety of [supported car makes, models, and model years](docs/CARS.md). In addition, while openpilot is engaged, a camera-based Driver Monitoring (DM) feature alerts distracted and asleep drivers. See more about [the vehicle integration](docs/INTEGRATION.md) and [limitations](docs/LIMITATIONS.md). - - - + + + + + + + + + +
-Using openpilot in a car +Running on a dedicated device in a car ------ -To use openpilot in a car, you need four things: -1. **Supported Device:** a comma 3X, available at [comma.ai/shop](https://comma.ai/shop/comma-3x). -2. **Software:** The setup procedure for the comma 3X allows users to enter a URL for custom software. Use the URL `openpilot.comma.ai` to install the release version. -3. **Supported Car:** Ensure that you have one of [the 275+ supported cars](docs/CARS.md). -4. **Car Harness:** You will also need a [car harness](https://comma.ai/shop/car-harness) to connect your comma 3X to your car. +To use openpilot in a car, you need four things +* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three). +* This software. The setup procedure of the comma three allows the user to enter a URL for custom software. +The URL, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another GitHub username can install a fork. +* One of [the 200+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot. +* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car. + +We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup). + +Running on PC +------ + +All openpilot services can run as usual on a PC without requiring special hardware or a car. You can also run openpilot on recorded or simulated data to develop or experiment with openpilot. + +With openpilot's tools, you can plot logs, replay drives, and watch the full-res camera streams. See [the tools README](tools/README.md) for more information. + +You can also run openpilot in simulation [with the CARLA simulator](tools/sim/README.md). This allows openpilot to drive around a virtual car on your Ubuntu machine. The whole setup should only take a few minutes but does require a decent GPU. + +A PC running openpilot can also control your vehicle if it is connected to a [webcam](https://github.com/commaai/openpilot/tree/master/tools/webcam), a [black panda](https://comma.ai/shop/products/panda), and a [harness](https://comma.ai/shop/products/car-harness). + +Community and Contributing +------ -We have detailed instructions for [how to install the harness and device in a car](https://comma.ai/setup). Note that it's possible to run openpilot on [other hardware](https://blog.comma.ai/self-driving-car-for-free/), although it's not plug-and-play. +openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). Bug fixes and new car ports are encouraged. Check out [the contributing docs](docs/CONTRIBUTING.md). +Documentation related to openpilot development can be found on [docs.comma.ai](https://docs.comma.ai). Information about running openpilot (e.g. FAQ, fingerprinting, troubleshooting, custom forks, community hardware) should go on the [wiki](https://github.com/commaai/openpilot/wiki). -### Branches +You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel. -Running `master` and other branches directly is supported, but it's recommended to run one of the following prebuilt branches: +Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/). -| comma four branch | comma 3X branch | URL | description | -|------------------------|------------------------|----------------------------------------|-------------------------------------------------------------------------------------| -| `release-mici` | `release-tizi` | openpilot.comma.ai | This is openpilot's release branch. | -| `release-mici-staging` | `release-tizi-staging` | openpilot-test.comma.ai | This is the staging branch for releases. Use it to get new releases slightly early. | -| `nightly` | `nightly` | openpilot-nightly.comma.ai | This is the bleeding edge development branch. Do not expect this to be stable. | -| `nightly-dev` | `nightly-dev` | installer.comma.ai/commaai/nightly-dev | Same as nightly, but includes experimental development features for some cars. | +And [follow us on Twitter](https://twitter.com/comma_ai). -To start developing openpilot +User Data and comma Account ------ -openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). +By default, openpilot uploads the driving data to our servers. You can also access your data through [comma connect](https://connect.comma.ai/). We use your data to train better models and improve openpilot for everyone. -* Join the [community Discord](https://discord.comma.ai) -* Check out [the contributing docs](docs/CONTRIBUTING.md) -* Check out the [openpilot tools](tools/) -* Code documentation lives at https://docs.comma.ai -* Information about running openpilot lives on the [community wiki](https://github.com/commaai/openpilot/wiki) +openpilot is open source software: the user is free to disable data collection if they wish to do so. -Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs#open-positions) and offers lots of [bounties](https://comma.ai/bounties) for external contributors. +openpilot logs the road-facing cameras, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. +The driver-facing camera is only logged if you explicitly opt-in in settings. The microphone is not recorded. + +By using openpilot, you agree to [our Privacy Policy](https://comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data. Safety and Testing ---- -* openpilot observes [ISO26262](https://en.wikipedia.org/wiki/ISO_26262) guidelines, see [SAFETY.md](docs/SAFETY.md) for more details. -* openpilot has software-in-the-loop [tests](.github/workflows/tests.yaml) that run on every commit. +* openpilot observes ISO26262 guidelines, see [SAFETY.md](docs/SAFETY.md) for more details. +* openpilot has software-in-the-loop [tests](.github/workflows/selfdrive_tests.yaml) that run on every commit. * The code enforcing the safety model lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details. * panda has software-in-the-loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety). * Internally, we have a hardware-in-the-loop Jenkins test suite that builds and unit tests the various processes. * panda has additional hardware-in-the-loop [tests](https://github.com/commaai/panda/blob/master/Jenkinsfile). * We run the latest openpilot in a testing closet containing 10 comma devices continuously replaying routes. -
-MIT Licensed +Directory Structure +------ + . + ├── cereal # The messaging spec and libs used for all logs + ├── common # Library like functionality we've developed here + ├── docs # Documentation + ├── opendbc # Files showing how to interpret data from cars + ├── panda # Code used to communicate on CAN + ├── third_party # External libraries + ├── pyextra # Extra python packages + └── system # Generic services + ├── camerad # Driver to capture images from the camera sensors + ├── clocksd # Broadcasts current time + ├── hardware # Hardware abstraction classes + ├── logcatd # systemd journal as a service + └── proclogd # Logs information from /proc + └── selfdrive # Code needed to drive the car + ├── assets # Fonts, images, and sounds for UI + ├── athena # Allows communication with the app + ├── boardd # Daemon to talk to the board + ├── car # Car specific code to read states and control actuators + ├── controls # Planning and controls + ├── debug # Tools to help you debug and do car ports + ├── locationd # Precise localization and vehicle parameter estimation + ├── loggerd # Logger and uploader of car data + ├── manager # Daemon that starts/stops all other daemons as needed + ├── modeld # Driving and monitoring model runners + ├── monitoring # Daemon to determine driver attention + ├── navd # Turn-by-turn navigation + ├── sensord # IMU interface code + ├── test # Unit tests, system tests, and a car simulator + └── ui # The UI + +Licensing +------ openpilot is released under the MIT license. Some parts of the software are released under other licenses as specified. @@ -95,17 +137,13 @@ Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and i **THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT. YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS. NO WARRANTY EXPRESSED OR IMPLIED.** -
-
-User Data and comma Account +--- -By default, openpilot uploads the driving data to our servers. You can also access your data through [comma connect](https://connect.comma.ai/). We use your data to train better models and improve openpilot for everyone. - -openpilot is open source software: the user is free to disable data collection if they wish to do so. + -openpilot logs the road-facing cameras, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. -The driver-facing camera and microphone are only logged if you explicitly opt-in in settings. - -By using openpilot, you agree to [our Privacy Policy](https://comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data. -
+[![openpilot tests](https://github.com/commaai/openpilot/workflows/openpilot%20tests/badge.svg?event=push)](https://github.com/commaai/openpilot/actions) +[![Total alerts](https://img.shields.io/lgtm/alerts/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/alerts/) +[![Language grade: Python](https://img.shields.io/lgtm/grade/python/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/context:python) +[![Language grade: C/C++](https://img.shields.io/lgtm/grade/cpp/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/context:cpp) +[![codecov](https://codecov.io/gh/commaai/openpilot/branch/master/graph/badge.svg)](https://codecov.io/gh/commaai/openpilot) diff --git a/RELEASES.md b/RELEASES.md index 6191c6ba3d4e14..56cffeebe7e8d1 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,241 +1,7 @@ -Version 0.10.4 (2026-02-17) -======================== -* Lexus LS 2018 support thanks to Hacheoy! - -Version 0.10.3 (2025-12-17) -======================== -* New driving model #36249 - * New temporal policy architecture - * New on-policy training physics noise model -* New driver monitoring model #36409 - * Trained on a new dataset, including comma four data -* Improved inter-process communication memory efficiency - -Version 0.10.2 (2025-11-19) -======================== -* comma four support - -Version 0.10.1 (2025-09-08) -======================== -* New driving model #36276 - * World Model: removed global localization inputs - * World Model: 2x the number of parameters - * World Model: trained on 4x the number of segments - * VAE Compression Model: new architecture and training objective - * Driving Vision Model: trained on 4x the number of segments -* New Driver Monitoring model #36198 -* Acura TLX 2021 support thanks to MVL! -* 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! -* Honda Passport 2026 support thanks to vanillagorillaa and MVL! - -Version 0.10.0 (2025-08-05) -======================== -* New driving model - * New training architecture - * Described in our CVPR paper: "Learning to Drive from a World Model" - * Longitudinal MPC replaced by E2E planning from World Model in Experimental Mode - * Action from lateral MPC as training objective replaced by E2E planning from World Model - * Low-speed lead car ground-truth fixes -* Enable live-learned steering actuation delay -* Opt-in audio recording for dashcam video -* Acura MDX 2025 support thanks to vanillagorillaa and MVL! -* Honda Accord 2023-25 support thanks to vanillagorillaa and MVL! -* Honda CR-V 2023-25 support thanks to vanillagorillaa and MVL! -* Honda Pilot 2023-25 support thanks to vanillagorillaa and MVL! - -Version 0.9.9 (2025-05-23) -======================== -* New driving model - * New training architecture using parts from MLSIM -* Steering actuation delay is now learned online -* Ford Escape 2023-24 support thanks to incognitojam! -* Ford Kuga 2024 support thanks to incognitojam! -* Hyundai Nexo 2021 support thanks to sunnyhaibin! -* Tesla Model 3 and Y support thanks to lukasloetkolben! -* Lexus RC 2023 support thanks to nelsonjchen! - -Version 0.9.8 (2025-02-28) -======================== -* New driving model - * Model now gates applying positive acceleration in Chill mode -* New driver monitoring model - * Reduced false positives related to passengers -* Image processing pipeline moved to the ISP - * More GPU time for bigger driving models - * Power draw reduced 0.5W, which means your device runs cooler -* Added toggle to enable driver monitoring even when openpilot is not engaged -* Localizer rewritten to remove GPS dependency at runtime -* Firehose Mode for maximizing your training data uploads -* Enable openpilot longitudinal control for Ford Q3 vehicles -* New Toyota TSS2 longitudinal tune -* Rivian R1S and R1T support thanks to lukasloetkolben! -* Ford F-150, F-150 Hybrid, Mach-E, and Ranger support - -Version 0.9.7 (2024-06-13) -======================== -* New driving model - * Inputs the past curvature for smoother and more accurate lateral control - * Simplified neural network architecture in the model's last layers - * Minor fixes to desire augmentation and weight decay -* New driver monitoring model - * Improved end-to-end bit for phone detection -* Adjust driving personality with the follow distance button -* Support for hybrid variants of supported Ford models -* Fingerprinting without the OBD-II port on all cars -* Improved fuzzy fingerprinting for Ford and Volkswagen - -Version 0.9.6 (2024-02-27) +Version 0.8.17 (2022-XX-XX) ======================== * New driving model - * Vision model trained on more data - * Improved driving performance - * Directly outputs curvature for lateral control -* New driver monitoring model - * Trained on larger dataset -* Model path UI - * Shows where driving model wants to be - * Shows what model is seeing more clearly, but more jittery -* AGNOS 9 -* comma body streaming and controls over WebRTC -* Improved fuzzy fingerprinting for many makes and models -* Alpha longitudinal support for new Toyota models -* Chevrolet Equinox 2019-22 support thanks to JasonJShuler and nworb-cire! -* Dodge Durango 2020-21 support -* Hyundai Staria 2023 support thanks to sunnyhaibin! -* Kia Niro Plug-in Hybrid 2022 support thanks to sunnyhaibin! -* Lexus LC 2024 support thanks to nelsonjchen! -* Toyota RAV4 2023-24 support -* Toyota RAV4 Hybrid 2023-24 support - -Version 0.9.5 (2023-11-17) -======================== -* New driving model - * Improved navigate on openpilot performance using navigation instructions as an additional model input - * Do lateral planning inside the model - * New vision transformer architecture -* Cadillac Escalade ESV 2019 support thanks to twilsonco! -* Hyundai Azera 2022 support thanks to sunnyhaibin! -* Hyundai Azera Hybrid 2020 support thanks to chanhojung and haram-KONA! -* Hyundai Custin 2023 support thanks to sunnyhaibin and Saber422! -* Hyundai Ioniq 6 2023 support thanks to sunnyhaibin and alamo3! -* Hyundai Kona Electric 2023 (Korean version) support thanks to sunnyhaibin and haram-KONA! -* Kia K8 Hybrid (with HDA II) 2023 support thanks to sunnyhaibin! -* Kia Optima Hybrid 2019 support -* Kia Sorento Hybrid 2023 support thanks to sunnyhaibin! -* Lexus GS F 2016 support thanks to snyperifle! -* Lexus IS 2023 support thanks to L3R5! - -Version 0.9.4 (2023-07-27) -======================== -* comma 3X support -* Navigate on openpilot in Experimental mode - * When navigation has a destination, openpilot will input the map information into the model, which provides useful context to help the model understand the scene - * When navigating on openpilot, openpilot will keep left or right appropriately at forks and exits - * When navigating on openpilot, lane change behavior is unchanged and still activated by the driver - * When navigate on openpilot is active, the path on the map is green -* UI updates - * Navigation settings moved to home screen and map - * Border color always shows engagement status. Blue means disengaged, green means engaged, and grey means engaged with human overriding - * Alerts are shown inside the border. Black means info, orange means warning, and red means critical alert -* Bookmarked segments are preserved on the device's storage -* Ford Focus 2018 support -* Kia Carnival 2023 support thanks to sunnyhaibin! - -Version 0.9.3 (2023-06-29) -======================== -* New driving model - * Improved height estimation and added height tracking in liveCalibration - * Model inputs refactor -* New driving personality setting - * Three settings: aggressive, standard, and relaxed - * Standard is recommended and the default - * In aggressive mode, lead follow distance is shorter and acceleration response is quicker - * In relaxed mode, lead follow distance is longer -* Improved fuzzy fingerprinting for Hyundai, Kia, and Genesis -* Improved thermal management logic - -Version 0.9.2 (2023-05-22) -======================== -* New driving model - * Reduced turn diving - * Trained on a new dataset -* UI updates - * New experimental mode visualization - * Draw MPC path instead of model-predicted path -* AGNOS 7 - * Faster boot time - * Fixes rare no sounds bug - * Fixes bootsplash bug at extreme temperatures -* Buick LaCrosse 2017-19 support thanks to koch-cf! -* Chevrolet Trailblazer 2021-22 support thanks to TurboCE! -* Ford Bronco Sport 2021-22 support -* Ford Escape 2020-22 support -* Ford Explorer 2020-22 support -* Ford Kuga 2020-22 support -* Ford Maverick 2022-23 support -* Genesis GV80 2023 support thanks to JWingate80! -* Honda HR-V 2023 support thanks to AlexandreSato and galegozi! -* Kia Niro EV 2023 support thanks to JosselinLecocq! -* Lexus ES 2017-18 support -* Lincoln Aviator 2021 support -* Škoda Fabia 2022-23 support thanks to jyoung8607! - - -Version 0.9.1 (2023-02-28) -======================== -* New driving model - * 30% improved height estimation resulting in better driving performance for tall cars -* Driver monitoring: removed timer resetting on user interaction if distracted -* UI updates - * Adjust alert volume using ambient noise level - * Driver monitoring icon shows driver's head pose - * German translation thanks to Vrabetz and CzokNorris! -* Cadillac Escalade 2017 support thanks to rickygilleland! -* Chevrolet Bolt EV 2022-23 support thanks to JasonJShuler! -* Genesis GV60 2023 support thanks to sunnyhaibin! -* Hyundai Tucson 2022-23 support -* Kia K5 Hybrid 2020 support thanks to sunnyhaibin! -* Kia Niro Hybrid 2023 support thanks to sunnyhaibin! -* Kia Sorento 2022-23 support thanks to sunnyhaibin! -* Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! -* Toyota C-HR 2021 support thanks to eFiniLan! -* Toyota C-HR Hybrid 2022 support thanks to Korben00! -* Volkswagen Crafter and MAN TGE 2017-23 support thanks to jyoung8607! - -Version 0.9.0 (2022-11-21) -======================== -* New driving model - * Internal feature space information content increased tenfold during training to ~700 bits, which makes the model dramatically more accurate - * Less reliance on previous frames makes model more reactive and snappy - * Trained in new reprojective simulator - * Trained in 36 hours from scratch, compared to one week for previous releases - * Training now simulates both lateral and longitudinal behavior, which allows openpilot to slow down for turns, stop at traffic lights, and more in experimental mode -* Experimental driving mode - * End-to-end longitudinal control - * Stops for traffic lights and stop signs - * Slows down for turns - * openpilot defaults to chill mode, enable experimental mode in settings -* Driver monitoring updates - * New bigger model with added end-to-end distracted trigger - * Reduced false positives during driver calibration -* Self-tuning torque controller: learns parameters live for each car -* Torque controller used on all Toyota, Lexus, Hyundai, Kia, and Genesis models -* UI updates - * Matched speeds shown on car's dash - * Multi-language in navigation - * Improved update experience - * Border turns grey while overriding steering - * Bookmark events while driving; view them in comma connect - * New onroad visualization for experimental mode -* tools: new and improved cabana thanks to deanlee! -* Experimental longitudinal support for Volkswagen, CAN-FD Hyundai, and new GM models -* Genesis GV70 2022-23 support thanks to zunichky and sunnyhaibin! -* Hyundai Santa Cruz 2021-22 support thanks to sunnyhaibin! -* Kia Sportage 2023 support thanks to sunnyhaibin! -* Kia Sportage Hybrid 2023 support thanks to sunnyhaibin! -* Kia Stinger 2022 support thanks to sunnyhaibin! + * Internal feature space accuracy increased tenfold during training, this makes the model dramatically more accurate. Version 0.8.16 (2022-08-26) ======================== @@ -714,7 +480,7 @@ Version 0.5.13 (2019-05-31) * Reduce CPU utilization by 20% and improve stability * Temporarily remove mapd functionalities to improve stability * Add openpilot record-only mode for unsupported cars - * Synchronize controlsd to pandad to reduce latency + * Synchronize controlsd to boardd to reduce latency * Remove panda support for Subaru giraffe Version 0.5.12 (2019-05-16) @@ -1050,7 +816,7 @@ Version 0.2.8 (2017-02-27) Version 0.2.7 (2017-02-08) =========================== * Better performance and pictures at night - * Fix ptr alignment issue in pandad + * Fix ptr alignment issue in boardd * Fix brake error light, fix crash if too cold Version 0.2.6 (2017-01-31) @@ -1082,7 +848,7 @@ Version 0.2.2 (2017-01-10) Version 0.2.1 (2016-12-14) =========================== * Performance improvements, removal of more numpy - * Fix pandad process priority + * Fix boardd process priority * Make counter timer reset on use of steering wheel Version 0.2 (2016-12-12) diff --git a/SConstruct b/SConstruct index ca5b7b6cb720b1..178b0cc8722877 100644 --- a/SConstruct +++ b/SConstruct @@ -3,51 +3,175 @@ import subprocess import sys import sysconfig import platform -import shlex import numpy as np -import SCons.Errors - -SCons.Warnings.warningAsException(True) +TICI = os.path.isfile('/TICI') +AGNOS = TICI Decider('MD5-timestamp') -SetOption('num_jobs', max(1, int(os.cpu_count()/2))) +AddOption('--extras', + action='store_true', + help='build misc extras, like setup and installer files') + +AddOption('--kaitai', + action='store_true', + help='Regenerate kaitai struct parsers') + +AddOption('--asan', + action='store_true', + help='turn on ASAN') + +AddOption('--ubsan', + action='store_true', + help='turn on UBSan') + +AddOption('--clazy', + action='store_true', + help='build with clazy') + +AddOption('--compile_db', + action='store_true', + help='build clang compilation database') -AddOption('--asan', action='store_true', help='turn on ASAN') -AddOption('--ubsan', action='store_true', help='turn on UBSan') -AddOption('--mutation', action='store_true', help='generate mutation-ready code') -AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line') -AddOption('--minimal', +AddOption('--snpe', + action='store_true', + help='use SNPE on PC') + +AddOption('--external-sconscript', + action='store', + metavar='FILE', + dest='external_sconscript', + help='add an external SConscript to the build') + +AddOption('--no-thneed', + action='store_true', + dest='no_thneed', + help='avoid using thneed') + +AddOption('--pc-thneed', + action='store_true', + dest='pc_thneed', + help='use thneed on pc') + +AddOption('--no-test', action='store_false', - dest='extras', - default=os.path.exists(File('#.gitattributes').abspath), # minimal by default on release branch (where there's no LFS) - help='the minimum build to run openpilot. no tests, tools, etc.') + dest='test', + default=os.path.islink(Dir('#laika/').abspath), + help='skip building test files') -# Detect platform -arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() +real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" - brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip() -elif arch == "aarch64" and os.path.isfile('/TICI'): + +if arch == "aarch64" and AGNOS: arch = "larch64" -assert arch in [ - "larch64", # linux tici arm64 - "aarch64", # linux pc arm64 - "x86_64", # linux pc x64 - "Darwin", # macOS arm64 (x86 not supported) -] + + +lenv = { + "PATH": os.environ['PATH'], + "LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath], + "PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath, + + "ACADOS_SOURCE_DIR": Dir("#third_party/acados/include/acados").abspath, + "ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath, + "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer" +} + +rpath = lenv["LD_LIBRARY_PATH"].copy() + +if arch == "larch64": + lenv["LD_LIBRARY_PATH"] += ['/data/data/com.termux/files/usr/lib'] + + cpppath = [ + "#third_party/opencl/include", + ] + + libpath = [ + "/usr/local/lib", + "/usr/lib", + "/system/vendor/lib64", + f"#third_party/acados/{arch}/lib", + ] + + libpath += [ + "#third_party/snpe/larch64", + "#third_party/libyuv/larch64/lib", + "/usr/lib/aarch64-linux-gnu" + ] + cflags = ["-DQCOM2", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"] + rpath += ["/usr/local/lib"] +else: + cflags = [] + cxxflags = [] + cpppath = [] + + # MacOS + if arch == "Darwin": + if real_arch == "x86_64": + lenv["TERA_PATH"] = Dir("#").abspath + f"/third_party/acados/Darwin_x86_64/t_renderer" + + brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip() + yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64" + libpath = [ + f"#third_party/libyuv/{yuv_dir}/lib", + f"{brew_prefix}/lib", + f"{brew_prefix}/Library", + f"{brew_prefix}/opt/openssl/lib", + f"{brew_prefix}/Cellar", + "/System/Library/Frameworks/OpenGL.framework/Libraries", + ] + if real_arch == "x86_64": + libpath.append(f"#third_party/acados/Darwin_x86_64/lib") + else: + libpath.append(f"#third_party/acados/{arch}/lib") + + cflags += ["-DGL_SILENCE_DEPRECATION"] + cxxflags += ["-DGL_SILENCE_DEPRECATION"] + cpppath += [ + f"{brew_prefix}/include", + f"{brew_prefix}/opt/openssl/include", + ] + # Linux 86_64 + else: + libpath = [ + "#third_party/acados/x86_64/lib", + "#third_party/snpe/x86_64-linux-clang", + "#third_party/libyuv/x64/lib", + "#third_party/mapbox-gl-native-qt/x86_64", + "#cereal", + "#common", + "/usr/lib", + "/usr/local/lib", + ] + + rpath += [ + Dir("#third_party/snpe/x86_64-linux-clang").abspath, + Dir("#cereal").abspath, + Dir("#common").abspath + ] + +if GetOption('asan'): + ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"] + ldflags = ["-fsanitize=address"] +elif GetOption('ubsan'): + ccflags = ["-fsanitize=undefined"] + ldflags = ["-fsanitize=undefined"] +else: + ccflags = [] + ldflags = [] + +# no --as-needed on mac linker +if arch != "Darwin": + ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"] + +# Enable swaglog include in submodules +cflags += ['-DSWAGLOG="\\"common/swaglog.h\\""'] +cxxflags += ['-DSWAGLOG="\\"common/swaglog.h\\""'] env = Environment( - ENV={ - "PATH": os.environ['PATH'], - "PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath, - "ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath, - "ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath, - "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer" - }, - CC='clang', - CXX='clang++', + ENV=lenv, CCFLAGS=[ "-g", "-fPIC", @@ -56,166 +180,262 @@ env = Environment( "-Werror", "-Wshadow", "-Wno-unknown-warning-option", + "-Wno-deprecated-register", + "-Wno-register", "-Wno-inconsistent-missing-override", "-Wno-c99-designator", "-Wno-reorder-init-list", - "-Wno-vla-cxx-extension", - ], - CFLAGS=["-std=gnu11"], - CXXFLAGS=["-std=c++1z"], - CPPPATH=[ + "-Wno-error=unused-but-set-variable", + ] + cflags + ccflags, + + CPPPATH=cpppath + [ "#", - "#msgq", - "#third_party", - "#third_party/json11", - "#third_party/linux/include", "#third_party/acados/include", "#third_party/acados/include/blasfeo/include", "#third_party/acados/include/hpipm/include", "#third_party/catch2/include", "#third_party/libyuv/include", + "#third_party/json11", + "#third_party/curl/include", + "#third_party/libgralloc/include", + "#third_party/android_frameworks_native/include", + "#third_party/android_hardware_libhardware/include", + "#third_party/android_system_core/include", + "#third_party/linux/include", + "#third_party/snpe/include", + "#third_party/mapbox-gl-native-qt/include", + "#third_party/qrcode", + "#third_party", + "#cereal", + "#opendbc/can", ], - LIBPATH=[ - "#common", - "#msgq_repo", + + CC='clang', + CXX='clang++', + LINKFLAGS=ldflags, + + RPATH=rpath, + + CFLAGS=["-std=gnu11"] + cflags, + CXXFLAGS=["-std=c++1z"] + cxxflags, + LIBPATH=libpath + [ + "#cereal", "#third_party", - "#selfdrive/pandad", - "#rednose/helpers", - f"#third_party/libyuv/{arch}/lib", - f"#third_party/acados/{arch}/lib", + "#opendbc/can", + "#selfdrive/boardd", + "#common", ], - RPATH=[], CYTHONCFILESUFFIX=".cpp", COMPILATIONDB_USE_ABSPATH=True, - REDNOSE_ROOT="#", - tools=["default", "cython", "compilation_db", "rednose_filter"], - toolpath=["#site_scons/site_tools", "#rednose_repo/site_scons/site_tools"], + tools=["default", "cython", "compilation_db"], ) -# Arch-specific flags and paths -if arch == "larch64": - env.Append(CPPPATH=["#third_party/opencl/include"]) - env.Append(LIBPATH=[ - "/usr/local/lib", - "/system/vendor/lib64", - "/usr/lib/aarch64-linux-gnu", - ]) - arch_flags = ["-D__TICI__", "-mcpu=cortex-a57"] - env.Append(CCFLAGS=arch_flags) - env.Append(CXXFLAGS=arch_flags) -elif arch == "Darwin": - env.Append(LIBPATH=[ - f"{brew_prefix}/lib", - f"{brew_prefix}/opt/openssl@3.0/lib", - f"{brew_prefix}/opt/llvm/lib/c++", - "/System/Library/Frameworks/OpenGL.framework/Libraries", - ]) - env.Append(CCFLAGS=["-DGL_SILENCE_DEPRECATION"]) - env.Append(CXXFLAGS=["-DGL_SILENCE_DEPRECATION"]) - env.Append(CPPPATH=[ - f"{brew_prefix}/include", - f"{brew_prefix}/opt/openssl@3.0/include", - ]) -else: - env.Append(LIBPATH=[ - "/usr/lib", - "/usr/local/lib", - ]) - -# Sanitizers and extra CCFLAGS from CLI -if GetOption('asan'): - env.Append(CCFLAGS=["-fsanitize=address", "-fno-omit-frame-pointer"]) - env.Append(LINKFLAGS=["-fsanitize=address"]) -elif GetOption('ubsan'): - env.Append(CCFLAGS=["-fsanitize=undefined"]) - env.Append(LINKFLAGS=["-fsanitize=undefined"]) +if arch == "Darwin": + env['RPATHPREFIX'] = "-rpath " -_extra_cc = shlex.split(GetOption('ccflags') or '') -if _extra_cc: - env.Append(CCFLAGS=_extra_cc) +if GetOption('compile_db'): + env.CompilationDatabase('compile_commands.json') -# no --as-needed on mac linker -if arch != "Darwin": - env.Append(LINKFLAGS=["-Wl,--as-needed", "-Wl,--no-undefined"]) +# Setup cache dir +cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache' +CacheDir(cache_dir) +Clean(["."], cache_dir) -# progress output node_interval = 5 node_count = 0 def progress_function(node): global node_count node_count += node_interval sys.stderr.write("progress: %d\n" % node_count) + if os.environ.get('SCONS_PROGRESS'): Progress(progress_function, interval=node_interval) -# ********** Cython build environment ********** +SHARED = False + +# TODO: this can probably be removed +def abspath(x): + if arch == 'aarch64': + pth = os.path.join("/data/pythonpath", x[0].path) + env.Depends(pth, x) + return File(pth) + else: + # rpath works elsewhere + return x[0].path.rsplit("/", 1)[1][:-3] + +# Cython build environment py_include = sysconfig.get_paths()['include'] envCython = env.Clone() envCython["CPPPATH"] += [py_include, np.get_include()] envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"] -envCython["CCFLAGS"].remove("-Werror") envCython["LIBS"] = [] if arch == "Darwin": - envCython["LINKFLAGS"] = env["LINKFLAGS"] + ["-bundle", "-undefined", "dynamic_lookup"] + envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] +elif arch == "aarch64": + envCython["LINKFLAGS"] = ["-shared"] + envCython["LIBS"] = [os.path.basename(py_include)] else: envCython["LINKFLAGS"] = ["-pthread", "-shared"] -np_version = SCons.Script.Value(np.__version__) -Export('envCython', 'np_version') - -Export('env', 'arch') +Export('envCython') -# Setup cache dir -cache_dir = '/data/scons_cache' if arch == "larch64" else '/tmp/scons_cache' -CacheDir(cache_dir) -Clean(["."], cache_dir) +# Qt build environment +qt_env = env.Clone() +qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus"] -# ********** start building stuff ********** +qt_libs = [] +if arch == "Darwin": + if real_arch == "arm64": + qt_env['QTDIR'] = "/opt/homebrew/opt/qt@5" + else: + qt_env['QTDIR'] = "/usr/local/opt/qt@5" + qt_dirs = [ + os.path.join(qt_env['QTDIR'], "include"), + ] + qt_dirs += [f"{qt_env['QTDIR']}/include/Qt{m}" for m in qt_modules] + qt_env["LINKFLAGS"] += ["-F" + os.path.join(qt_env['QTDIR'], "lib")] + qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"] + qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin")) +else: + qt_env['QTDIR'] = "/usr" + qt_dirs = [ + f"/usr/include/{real_arch}-linux-gnu/qt5", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui", + ] + qt_dirs += [f"/usr/include/{real_arch}-linux-gnu/qt5/Qt{m}" for m in qt_modules] + + qt_libs = [f"Qt5{m}" for m in qt_modules] + if arch == "larch64": + qt_libs += ["GLESv2", "wayland-client"] + elif arch != "Darwin": + qt_libs += ["GL"] + +qt_env.Tool('qt') +qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"] +qt_flags = [ + "-D_REENTRANT", + "-DQT_NO_DEBUG", + "-DQT_WIDGETS_LIB", + "-DQT_GUI_LIB", + "-DQT_QUICK_LIB", + "-DQT_QUICKWIDGETS_LIB", + "-DQT_QML_LIB", + "-DQT_CORE_LIB", + "-DQT_MESSAGELOGCONTEXT", +] +qt_env['CXXFLAGS'] += qt_flags +qt_env['LIBPATH'] += ['#selfdrive/ui'] +qt_env['LIBS'] = qt_libs + +if GetOption("clazy"): + checks = [ + "level0", + "level1", + "no-range-loop", + "no-non-pod-global-static", + ] + qt_env['CXX'] = 'clazy' + qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0] + qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks) + +Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED') -# Build common module SConscript(['common/SConscript']) -Import('_common') -common = [_common, 'json11', 'zmq'] -Export('common') +Import('_common', '_gpucommon') -# Build messaging (cereal + msgq + socketmaster + their dependencies) -# Enable swaglog include in submodules -env_swaglog = env.Clone() -env_swaglog['CXXFLAGS'].append('-DSWAGLOG="\\"common/swaglog.h\\""') -SConscript(['msgq_repo/SConscript'], exports={'env': env_swaglog}) -SConscript(['opendbc_repo/SConscript'], exports={'env': env_swaglog}) +if SHARED: + common, gpucommon = abspath(common), abspath(gpucommon) +else: + common = [_common, 'json11'] + gpucommon = [_gpucommon] + +Export('common', 'gpucommon') +# cereal and messaging are shared with the system SConscript(['cereal/SConscript']) +if SHARED: + cereal = abspath([File('cereal/libcereal_shared.so')]) + messaging = abspath([File('cereal/libmessaging_shared.so')]) +else: + cereal = [File('#cereal/libcereal.a')] + messaging = [File('#cereal/libmessaging.a')] + visionipc = [File('#cereal/libvisionipc.a')] -Import('socketmaster', 'msgq') -messaging = [socketmaster, msgq, 'capnp', 'kj',] -Export('messaging') +Export('cereal', 'messaging', 'visionipc') +# Build rednose library and ekf models -# Build other submodules -SConscript(['panda/SConscript']) +rednose_deps = [ + "#selfdrive/locationd/models/constants.py", + "#selfdrive/locationd/models/gnss_helpers.py", +] -# Build rednose library +rednose_config = { + 'generated_folder': '#selfdrive/locationd/models/generated', + 'to_build': { + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, [], rednose_deps), + 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h'], rednose_deps), + 'car': ('#selfdrive/locationd/models/car_kf.py', True, [], rednose_deps), + }, +} + +if arch != "larch64": + rednose_config['to_build'].update({ + 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, [], rednose_deps), + 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []), + 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []), + 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []), + 'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, [], rednose_deps), + }) + +Export('rednose_config') SConscript(['rednose/SConscript']) # Build system services SConscript([ - 'system/loggerd/SConscript', + 'system/camerad/SConscript', + 'system/clocksd/SConscript', + 'system/proclogd/SConscript', ]) - -if arch == "larch64": - SConscript(['system/camerad/SConscript']) +if arch != "Darwin": + SConscript(['system/logcatd/SConscript']) # Build openpilot + +# build submodules +SConscript([ + 'cereal/SConscript', + 'body/board/SConscript', + 'panda/board/SConscript', + 'opendbc/can/SConscript', +]) + SConscript(['third_party/SConscript']) -SConscript(['selfdrive/SConscript']) +SConscript(['common/kalman/SConscript']) +SConscript(['common/transformations/SConscript']) + +SConscript(['selfdrive/modeld/SConscript']) + +SConscript(['selfdrive/controls/lib/cluster/SConscript']) +SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) +SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) + +SConscript(['selfdrive/boardd/SConscript']) + +SConscript(['selfdrive/loggerd/SConscript']) + +SConscript(['selfdrive/locationd/SConscript']) +SConscript(['selfdrive/sensord/SConscript']) +SConscript(['selfdrive/ui/SConscript']) +SConscript(['selfdrive/navd/SConscript']) -if Dir('#tools/cabana/').exists() and GetOption('extras'): - SConscript(['tools/replay/SConscript']) - if arch != "larch64": - SConscript(['tools/cabana/SConscript']) +SConscript(['tools/replay/SConscript']) +if GetOption('test'): + SConscript('panda/tests/safety/SConscript') -env.CompilationDatabase('compile_commands.json') +external_sconscript = GetOption('external_sconscript') +if external_sconscript: + SConscript([external_sconscript]) diff --git a/body b/body new file mode 160000 index 00000000000000..04aeb30ce0bb14 --- /dev/null +++ b/body @@ -0,0 +1 @@ +Subproject commit 04aeb30ce0bb14759989cd374158233877e1e151 diff --git a/cereal b/cereal new file mode 160000 index 00000000000000..cea51afd67b5c5 --- /dev/null +++ b/cereal @@ -0,0 +1 @@ +Subproject commit cea51afd67b5c56f7a18207ef91c5e45d6345526 diff --git a/cereal/README.md b/cereal/README.md deleted file mode 100644 index 45e859c09ca4a6..00000000000000 --- a/cereal/README.md +++ /dev/null @@ -1,95 +0,0 @@ -# What is cereal? - -cereal is the messaging system for openpilot. It uses [msgq](https://github.com/commaai/msgq) as a pub/sub backend, and [Cap'n proto](https://capnproto.org/capnp-tool.html) for serialization of the structs. - - -## Messaging Spec - -You'll find the message types in [log.capnp](log.capnp). It uses [Cap'n proto](https://capnproto.org/capnp-tool.html) and defines one struct called `Event`. - -All `Events` have a `logMonoTime` and a `valid`. Then a big union defines the packet type. - -### Best Practices - -- **All fields must describe quantities in SI units**, unless otherwise specified in the field name. -- In the context of the message they are in, field names should be completely unambiguous. -- All values should be easy to plot and be human-readable with minimal parsing. - -### Maintaining backwards-compatibility - -When making changes to the messaging spec you want to maintain backwards-compatibility, such that old logs can -be parsed with a new version of cereal. Adding structs and adding members to structs is generally safe, most other -things are not. Read more details [here](https://capnproto.org/language.html). - -### Custom forks - -Forks of [openpilot](https://github.com/commaai/openpilot) might want to add things to the messaging -spec, however this could conflict with future changes made in mainline cereal/openpilot. Rebasing against mainline openpilot -then means breaking backwards-compatibility with all old logs of your fork. So we added reserved events in -[custom.capnp](custom.capnp) that we will leave empty in mainline cereal/openpilot. **If you only modify those, you can ensure your -fork will remain backwards-compatible with all versions of mainline openpilot and your fork.** - -An example of compatible changes: -```diff -diff --git a/cereal/custom.capnp b/cereal/custom.capnp -index 3348e859e..3365c7b98 100644 ---- a/cereal/custom.capnp -+++ b/cereal/custom.capnp -@@ -10,7 +10,11 @@ $Cxx.namespace("cereal"); - # DO rename the structs - # DON'T change the identifier (e.g. @0x81c2f05a394cf4af) - --struct CustomReserved0 @0x81c2f05a394cf4af { -+struct SteeringInfo @0x81c2f05a394cf4af { -+ active @0 :Bool; -+ steeringAngleDeg @1 :Float32; -+ steeringRateDeg @2 :Float32; -+ steeringAccelDeg @3 :Float32; - } - - struct CustomReserved1 @0xaedffd8f31e7b55d { -diff --git a/cereal/log.capnp b/cereal/log.capnp -index 1209f3fd9..b189f58b6 100644 ---- a/cereal/log.capnp -+++ b/cereal/log.capnp -@@ -2558,14 +2558,14 @@ struct Event { - - # DO change the name of the field - # DON'T change anything after the "@" -- customReservedRawData0 @124 :Data; -+ rawCanData @124 :Data; - customReservedRawData1 @125 :Data; - customReservedRawData2 @126 :Data; - - # DO change the name of the field and struct - # DON'T change the ID (e.g. @107) - # DON'T change which struct it points to -- customReserved0 @107 :Custom.CustomReserved0; -+ steeringInfo @107 :Custom.SteeringInfo; - customReserved1 @108 :Custom.CustomReserved1; - customReserved2 @109 :Custom.CustomReserved2; - customReserved3 @110 :Custom.CustomReserved3; -``` - ---- - -Example ---- -```python -import cereal.messaging as messaging - -# in subscriber -sm = messaging.SubMaster(['sensorEvents']) -while 1: - sm.update() - print(sm['sensorEvents']) - -``` - -```python -# in publisher -pm = messaging.PubMaster(['sensorEvents']) -dat = messaging.new_message('sensorEvents', size=1) -dat.sensorEvents[0] = {"gyro": {"v": [0.1, -0.1, 0.1]}} -pm.send('sensorEvents', dat) -``` diff --git a/cereal/SConscript b/cereal/SConscript deleted file mode 100644 index a58a9490ce6488..00000000000000 --- a/cereal/SConscript +++ /dev/null @@ -1,20 +0,0 @@ -Import('env', 'common', 'msgq') - -cereal_dir = Dir('.') -gen_dir = Dir('gen') - -# Build cereal -schema_files = ['log.capnp', 'car.capnp', 'legacy.capnp', 'custom.capnp'] -env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s in schema_files], - schema_files, - f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/") - -cereal = env.Library('cereal', [f'gen/cpp/{s}.c++' for s in schema_files]) - -# Build messaging -services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET') -env.Program('messaging/bridge', ['messaging/bridge.cc', 'messaging/msgq_to_zmq.cc'], LIBS=[msgq, common, 'pthread']) - -socketmaster = env.Library('socketmaster', ['messaging/socketmaster.cc']) - -Export('cereal', 'socketmaster') diff --git a/cereal/__init__.py b/cereal/__init__.py deleted file mode 100644 index 93f4d772276d57..00000000000000 --- a/cereal/__init__.py +++ /dev/null @@ -1,11 +0,0 @@ -import os -import capnp -from importlib.resources import as_file, files - -capnp.remove_import_hook() - -with as_file(files("cereal")) as fspath: - CEREAL_PATH = fspath.as_posix() - log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) - car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) - custom = capnp.load(os.path.join(CEREAL_PATH, "custom.capnp")) diff --git a/cereal/car.capnp b/cereal/car.capnp deleted file mode 120000 index 4bc7f89b1fb5f2..00000000000000 --- a/cereal/car.capnp +++ /dev/null @@ -1 +0,0 @@ -../opendbc_repo/opendbc/car/car.capnp \ No newline at end of file diff --git a/cereal/custom.capnp b/cereal/custom.capnp deleted file mode 100644 index 3348e859efb8eb..00000000000000 --- a/cereal/custom.capnp +++ /dev/null @@ -1,71 +0,0 @@ -using Cxx = import "./include/c++.capnp"; -$Cxx.namespace("cereal"); - -@0xb526ba661d550a59; - -# custom.capnp: a home for empty structs reserved for custom forks -# These structs are guaranteed to remain reserved and empty in mainline -# cereal, so use these if you want custom events in your fork. - -# DO rename the structs -# DON'T change the identifier (e.g. @0x81c2f05a394cf4af) - -struct CustomReserved0 @0x81c2f05a394cf4af { -} - -struct CustomReserved1 @0xaedffd8f31e7b55d { -} - -struct CustomReserved2 @0xf35cc4560bbf6ec2 { -} - -struct CustomReserved3 @0xda96579883444c35 { -} - -struct CustomReserved4 @0x80ae746ee2596b11 { -} - -struct CustomReserved5 @0xa5cd762cd951a455 { -} - -struct CustomReserved6 @0xf98d843bfd7004a3 { -} - -struct CustomReserved7 @0xb86e6369214c01c8 { -} - -struct CustomReserved8 @0xf416ec09499d9d19 { -} - -struct CustomReserved9 @0xa1680744031fdb2d { -} - -struct CustomReserved10 @0xcb9fd56c7057593a { -} - -struct CustomReserved11 @0xc2243c65e0340384 { -} - -struct CustomReserved12 @0x9ccdc8676701b412 { -} - -struct CustomReserved13 @0xcd96dafb67a082d0 { -} - -struct CustomReserved14 @0xb057204d7deadf3f { -} - -struct CustomReserved15 @0xbd443b539493bc68 { -} - -struct CustomReserved16 @0xfc6241ed8877b611 { -} - -struct CustomReserved17 @0xa30662f84033036c { -} - -struct CustomReserved18 @0xc86a3d38d13eb3ef { -} - -struct CustomReserved19 @0xa4f1eb3323f5f582 { -} diff --git a/cereal/include/c++.capnp b/cereal/include/c++.capnp deleted file mode 100644 index 2bda54717920ab..00000000000000 --- a/cereal/include/c++.capnp +++ /dev/null @@ -1,26 +0,0 @@ -# Copyright (c) 2013-2014 Sandstorm Development Group, Inc. and contributors -# Licensed under the MIT License: -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -@0xbdf87d7bb8304e81; -$namespace("capnp::annotations"); - -annotation namespace(file): Text; -annotation name(field, enumerant, struct, enum, interface, method, param, group, union): Text; diff --git a/cereal/legacy.capnp b/cereal/legacy.capnp deleted file mode 100644 index a8fa5e4a1f7d5d..00000000000000 --- a/cereal/legacy.capnp +++ /dev/null @@ -1,574 +0,0 @@ -using Cxx = import "./include/c++.capnp"; -$Cxx.namespace("cereal"); - -@0x80ef1ec4889c2a63; - -# legacy.capnp: a home for deprecated structs - -struct LogRotate @0x9811e1f38f62f2d1 { - segmentNum @0 :Int32; - path @1 :Text; -} - -struct LiveUI @0xc08240f996aefced { - rearViewCam @0 :Bool; - alertText1 @1 :Text; - alertText2 @2 :Text; - awarenessStatus @3 :Float32; -} - -struct UiLayoutState @0x88dcce08ad29dda0 { - activeApp @0 :App; - sidebarCollapsed @1 :Bool; - mapEnabled @2 :Bool; - mockEngaged @3 :Bool; - - enum App @0x9917470acf94d285 { - home @0; - music @1; - nav @2; - settings @3; - none @4; - } -} - -struct OrbslamCorrection @0x8afd33dc9b35e1aa { - correctionMonoTime @0 :UInt64; - prePositionECEF @1 :List(Float64); - postPositionECEF @2 :List(Float64); - prePoseQuatECEF @3 :List(Float32); - postPoseQuatECEF @4 :List(Float32); - numInliers @5 :UInt32; -} - -struct EthernetPacket @0xa99a9d5b33cf5859 { - pkt @0 :Data; - ts @1 :Float32; -} - -struct CellInfo @0xcff7566681c277ce { - timestamp @0 :UInt64; - repr @1 :Text; # android toString() for now -} - -struct WifiScan @0xd4df5a192382ba0b { - bssid @0 :Text; - ssid @1 :Text; - capabilities @2 :Text; - frequency @3 :Int32; - level @4 :Int32; - timestamp @5 :Int64; - - centerFreq0 @6 :Int32; - centerFreq1 @7 :Int32; - channelWidth @8 :ChannelWidth; - operatorFriendlyName @9 :Text; - venueName @10 :Text; - is80211mcResponder @11 :Bool; - passpoint @12 :Bool; - - distanceCm @13 :Int32; - distanceSdCm @14 :Int32; - - enum ChannelWidth @0xcb6a279f015f6b51 { - w20Mhz @0; - w40Mhz @1; - w80Mhz @2; - w160Mhz @3; - w80Plus80Mhz @4; - } -} - -struct LiveEventData @0x94b7baa90c5c321e { - name @0 :Text; - value @1 :Int32; -} - -struct ModelData @0xb8aad62cffef28a9 { - frameId @0 :UInt32; - frameAge @12 :UInt32; - frameDropPerc @13 :Float32; - timestampEof @9 :UInt64; - modelExecutionTime @14 :Float32; - gpuExecutionTime @16 :Float32; - rawPred @15 :Data; - - path @1 :PathData; - leftLane @2 :PathData; - rightLane @3 :PathData; - lead @4 :LeadData; - freePath @6 :List(Float32); - - settings @5 :ModelSettings; - leadFuture @7 :LeadData; - speed @8 :List(Float32); - meta @10 :MetaData; - longitudinal @11 :LongitudinalData; - - struct PathData @0x8817eeea389e9f08 { - points @0 :List(Float32); - prob @1 :Float32; - std @2 :Float32; - stds @3 :List(Float32); - poly @4 :List(Float32); - validLen @5 :Float32; - } - - struct LeadData @0xd1c9bef96d26fa91 { - dist @0 :Float32; - prob @1 :Float32; - std @2 :Float32; - relVel @3 :Float32; - relVelStd @4 :Float32; - relY @5 :Float32; - relYStd @6 :Float32; - relA @7 :Float32; - relAStd @8 :Float32; - } - - struct ModelSettings @0xa26e3710efd3e914 { - bigBoxX @0 :UInt16; - bigBoxY @1 :UInt16; - bigBoxWidth @2 :UInt16; - bigBoxHeight @3 :UInt16; - boxProjection @4 :List(Float32); - yuvCorrection @5 :List(Float32); - inputTransform @6 :List(Float32); - } - - struct MetaData @0x9744f25fb60f2bf8 { - engagedProb @0 :Float32; - desirePrediction @1 :List(Float32); - brakeDisengageProb @2 :Float32; - gasDisengageProb @3 :Float32; - steerOverrideProb @4 :Float32; - desireState @5 :List(Float32); - } - - struct LongitudinalData @0xf98f999c6a071122 { - distances @2 :List(Float32); - speeds @0 :List(Float32); - accelerations @1 :List(Float32); - } -} - -struct ECEFPoint @0xc25bbbd524983447 { - x @0 :Float64; - y @1 :Float64; - z @2 :Float64; -} - -struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 { - x @0 :Float32; - y @1 :Float32; - z @2 :Float32; -} - -struct GPSPlannerPoints @0xab54c59699f8f9f3 { - curPosDEPRECATED @0 :ECEFPointDEPRECATED; - pointsDEPRECATED @1 :List(ECEFPointDEPRECATED); - curPos @6 :ECEFPoint; - points @7 :List(ECEFPoint); - valid @2 :Bool; - trackName @3 :Text; - speedLimit @4 :Float32; - accelTarget @5 :Float32; -} - -struct GPSPlannerPlan @0xf5ad1d90cdc1dd6b { - valid @0 :Bool; - poly @1 :List(Float32); - trackName @2 :Text; - speed @3 :Float32; - acceleration @4 :Float32; - pointsDEPRECATED @5 :List(ECEFPointDEPRECATED); - points @6 :List(ECEFPoint); - xLookahead @7 :Float32; -} - -struct UiNavigationEvent @0x90c8426c3eaddd3b { - type @0: Type; - status @1: Status; - distanceTo @2: Float32; - endRoadPointDEPRECATED @3: ECEFPointDEPRECATED; - endRoadPoint @4: ECEFPoint; - - enum Type @0xe8db07dcf8fcea05 { - none @0; - laneChangeLeft @1; - laneChangeRight @2; - mergeLeft @3; - mergeRight @4; - turnLeft @5; - turnRight @6; - } - - enum Status @0xb9aa88c75ef99a1f { - none @0; - passive @1; - approaching @2; - active @3; - } -} - -struct LiveLocationData @0xb99b2bc7a57e8128 { - status @0 :UInt8; - - # 3D fix - lat @1 :Float64; - lon @2 :Float64; - alt @3 :Float32; # m - - # speed - speed @4 :Float32; # m/s - - # NED velocity components - vNED @5 :List(Float32); - - # roll, pitch, heading (x,y,z) - roll @6 :Float32; # WRT to center of earth? - pitch @7 :Float32; # WRT to center of earth? - heading @8 :Float32; # WRT to north? - - # what are these? - wanderAngle @9 :Float32; - trackAngle @10 :Float32; - - # car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png - - # gyro, in car frame, deg/s - gyro @11 :List(Float32); - - # accel, in car frame, m/s^2 - accel @12 :List(Float32); - - accuracy @13 :Accuracy; - - source @14 :SensorSource; - # if we are fixing a location in the past - fixMonoTime @15 :UInt64; - - gpsWeek @16 :Int32; - timeOfWeek @17 :Float64; - - positionECEF @18 :List(Float64); - poseQuatECEF @19 :List(Float32); - pitchCalibration @20 :Float32; - yawCalibration @21 :Float32; - imuFrame @22 :List(Float32); - - struct Accuracy @0x943dc4625473b03f { - pNEDError @0 :List(Float32); - vNEDError @1 :List(Float32); - rollError @2 :Float32; - pitchError @3 :Float32; - headingError @4 :Float32; - ellipsoidSemiMajorError @5 :Float32; - ellipsoidSemiMinorError @6 :Float32; - ellipsoidOrientationError @7 :Float32; - } - - enum SensorSource @0xc871d3cc252af657 { - applanix @0; - kalman @1; - orbslam @2; - timing @3; - dummy @4; - } -} - -struct OrbOdometry @0xd7700859ed1f5b76 { - # timing first - startMonoTime @0 :UInt64; - endMonoTime @1 :UInt64; - - # fundamental matrix and error - f @2: List(Float64); - err @3: Float64; - - # number of inlier points - inliers @4: Int32; - - # for debug only - # indexed by endMonoTime features - # value is startMonoTime feature match - # -1 if no match - matches @5: List(Int16); -} - -struct OrbFeatures @0xcd60164a8a0159ef { - timestampEof @0 :UInt64; - # transposed arrays of normalized image coordinates - # len(xs) == len(ys) == len(descriptors) * 32 - xs @1 :List(Float32); - ys @2 :List(Float32); - descriptors @3 :Data; - octaves @4 :List(Int8); - - # match index to last OrbFeatures - # -1 if no match - timestampLastEof @5 :UInt64; - matches @6: List(Int16); -} - -struct OrbFeaturesSummary @0xd500d30c5803fa4f { - timestampEof @0 :UInt64; - timestampLastEof @1 :UInt64; - - featureCount @2 :UInt16; - matchCount @3 :UInt16; - computeNs @4 :UInt64; -} - -struct OrbKeyFrame @0xc8233c0345e27e24 { - # this is a globally unique id for the KeyFrame - id @0: UInt64; - - # this is the location of the KeyFrame - pos @1: ECEFPoint; - - # these are the features in the world - # len(dpos) == len(descriptors) * 32 - dpos @2 :List(ECEFPoint); - descriptors @3 :Data; -} - -struct KalmanOdometry @0x92e21bb7ea38793a { - trans @0 :List(Float32); # m/s in device frame - rot @1 :List(Float32); # rad/s in device frame - transStd @2 :List(Float32); # std m/s in device frame - rotStd @3 :List(Float32); # std rad/s in device frame -} - -struct OrbObservation @0x9b326d4e436afec7 { - observationMonoTime @0 :UInt64; - normalizedCoordinates @1 :List(Float32); - locationECEF @2 :List(Float64); - matchDistance @3: UInt32; -} - -struct CalibrationFeatures @0x8fdfadb254ea867a { - frameId @0 :UInt32; - - p0 @1 :List(Float32); - p1 @2 :List(Float32); - status @3 :List(Int8); -} - -struct NavStatus @0xbd8822120928120c { - isNavigating @0 :Bool; - currentAddress @1 :Address; - - struct Address @0xce7cd672cacc7814 { - title @0 :Text; - lat @1 :Float64; - lng @2 :Float64; - house @3 :Text; - address @4 :Text; - street @5 :Text; - city @6 :Text; - state @7 :Text; - country @8 :Text; - } -} - -struct NavUpdate @0xdb98be6565516acb { - isNavigating @0 :Bool; - curSegment @1 :Int32; - segments @2 :List(Segment); - - struct LatLng @0x9eaef9187cadbb9b { - lat @0 :Float64; - lng @1 :Float64; - } - - struct Segment @0xa5b39b4fc4d7da3f { - from @0 :LatLng; - to @1 :LatLng; - updateTime @2 :Int32; - distance @3 :Int32; - crossTime @4 :Int32; - exitNo @5 :Int32; - instruction @6 :Instruction; - - parts @7 :List(LatLng); - - enum Instruction @0xc5417a637451246f { - turnLeft @0; - turnRight @1; - keepLeft @2; - keepRight @3; - straight @4; - roundaboutExitNumber @5; - roundaboutExit @6; - roundaboutTurnLeft @7; - unkn8 @8; - roundaboutStraight @9; - unkn10 @10; - roundaboutTurnRight @11; - unkn12 @12; - roundaboutUturn @13; - unkn14 @14; - arrive @15; - exitLeft @16; - exitRight @17; - unkn18 @18; - uturn @19; - # ... - } - } -} - -struct TrafficEvent @0xacfa74a094e62626 { - type @0 :Type; - distance @1 :Float32; - action @2 :Action; - resuming @3 :Bool; - - enum Type @0xd85d75253435bf4b { - stopSign @0; - lightRed @1; - lightYellow @2; - lightGreen @3; - stopLight @4; - } - - enum Action @0xa6f6ce72165ccb49 { - none @0; - yield @1; - stop @2; - resumeReady @3; - } - -} - - -struct AndroidGnss @0xdfdf30d03fc485bd { - union { - measurements @0 :Measurements; - navigationMessage @1 :NavigationMessage; - } - - struct Measurements @0xa20710d4f428d6cd { - clock @0 :Clock; - measurements @1 :List(Measurement); - - struct Clock @0xa0e27b453a38f450 { - timeNanos @0 :Int64; - hardwareClockDiscontinuityCount @1 :Int32; - - hasTimeUncertaintyNanos @2 :Bool; - timeUncertaintyNanos @3 :Float64; - - hasLeapSecond @4 :Bool; - leapSecond @5 :Int32; - - hasFullBiasNanos @6 :Bool; - fullBiasNanos @7 :Int64; - - hasBiasNanos @8 :Bool; - biasNanos @9 :Float64; - - hasBiasUncertaintyNanos @10 :Bool; - biasUncertaintyNanos @11 :Float64; - - hasDriftNanosPerSecond @12 :Bool; - driftNanosPerSecond @13 :Float64; - - hasDriftUncertaintyNanosPerSecond @14 :Bool; - driftUncertaintyNanosPerSecond @15 :Float64; - } - - struct Measurement @0xd949bf717d77614d { - svId @0 :Int32; - constellation @1 :Constellation; - - timeOffsetNanos @2 :Float64; - state @3 :Int32; - receivedSvTimeNanos @4 :Int64; - receivedSvTimeUncertaintyNanos @5 :Int64; - cn0DbHz @6 :Float64; - pseudorangeRateMetersPerSecond @7 :Float64; - pseudorangeRateUncertaintyMetersPerSecond @8 :Float64; - accumulatedDeltaRangeState @9 :Int32; - accumulatedDeltaRangeMeters @10 :Float64; - accumulatedDeltaRangeUncertaintyMeters @11 :Float64; - - hasCarrierFrequencyHz @12 :Bool; - carrierFrequencyHz @13 :Float32; - hasCarrierCycles @14 :Bool; - carrierCycles @15 :Int64; - hasCarrierPhase @16 :Bool; - carrierPhase @17 :Float64; - hasCarrierPhaseUncertainty @18 :Bool; - carrierPhaseUncertainty @19 :Float64; - hasSnrInDb @20 :Bool; - snrInDb @21 :Float64; - - multipathIndicator @22 :MultipathIndicator; - - enum Constellation @0x9ef1f3ff0deb5ffb { - unknown @0; - gps @1; - sbas @2; - glonass @3; - qzss @4; - beidou @5; - galileo @6; - } - - enum State @0xcbb9490adce12d72 { - unknown @0; - codeLock @1; - bitSync @2; - subframeSync @3; - towDecoded @4; - msecAmbiguous @5; - symbolSync @6; - gloStringSync @7; - gloTodDecoded @8; - bdsD2BitSync @9; - bdsD2SubframeSync @10; - galE1bcCodeLock @11; - galE1c2ndCodeLock @12; - galE1bPageSync @13; - sbasSync @14; - } - - enum MultipathIndicator @0xc04e7b6231d4caa8 { - unknown @0; - detected @1; - notDetected @2; - } - } - } - - struct NavigationMessage @0xe2517b083095fd4e { - type @0 :Int32; - svId @1 :Int32; - messageId @2 :Int32; - submessageId @3 :Int32; - data @4 :Data; - status @5 :Status; - - enum Status @0xec1ff7996b35366f { - unknown @0; - parityPassed @1; - parityRebuilt @2; - } - } -} - -struct LidarPts @0xe3d6685d4e9d8f7a { - r @0 :List(UInt16); # uint16 m*500.0 - theta @1 :List(UInt16); # uint16 deg*100.0 - reflect @2 :List(UInt8); # uint8 0-255 - - # For storing out of file. - idx @3 :UInt64; - - # For storing in file - pkt @4 :Data; -} - - diff --git a/cereal/log.capnp b/cereal/log.capnp deleted file mode 100644 index 12bef17b95048a..00000000000000 --- a/cereal/log.capnp +++ /dev/null @@ -1,2698 +0,0 @@ -using Cxx = import "./include/c++.capnp"; -$Cxx.namespace("cereal"); - -using Car = import "car.capnp"; -using Legacy = import "legacy.capnp"; -using Custom = import "custom.capnp"; - -@0xf3b1f17e25a4285b; - -const logVersion :Int32 = 1; - -struct Map(Key, Value) { - entries @0 :List(Entry); - struct Entry { - key @0 :Key; - value @1 :Value; - } -} - -struct OnroadEvent @0xc4fa6047f024e718 { - name @0 :EventName; - - # event types - enable @1 :Bool; - noEntry @2 :Bool; - warning @3 :Bool; # alerts presented only when enabled or soft disabling - userDisable @4 :Bool; - softDisable @5 :Bool; - immediateDisable @6 :Bool; - preEnable @7 :Bool; - permanent @8 :Bool; # alerts presented regardless of openpilot state - overrideLateral @10 :Bool; - overrideLongitudinal @9 :Bool; - - enum EventName @0x91f1992a1f77fb03 { - canError @0; - steerUnavailable @1; - wrongGear @2; - doorOpen @3; - seatbeltNotLatched @4; - espDisabled @5; - wrongCarMode @6; - steerTempUnavailable @7; - reverseGear @8; - buttonCancel @9; - buttonEnable @10; - pedalPressed @11; # exits active state - preEnableStandstill @12; # added during pre-enable state with brake - gasPressedOverride @13; # added when user is pressing gas with no disengage on gas - steerOverride @14; - steerDisengage @94; # exits active state - cruiseDisabled @15; - speedTooLow @16; - outOfSpace @17; - overheat @18; - calibrationIncomplete @19; - calibrationInvalid @20; - calibrationRecalibrating @21; - controlsMismatch @22; - pcmEnable @23; - pcmDisable @24; - radarFault @25; - radarTempUnavailable @93; - brakeHold @26; - parkBrake @27; - manualRestart @28; - joystickDebug @29; - longitudinalManeuver @30; - steerTempUnavailableSilent @31; - resumeRequired @32; - preDriverDistracted @33; - promptDriverDistracted @34; - driverDistracted @35; - preDriverUnresponsive @36; - promptDriverUnresponsive @37; - driverUnresponsive @38; - belowSteerSpeed @39; - lowBattery @40; - accFaulted @41; - sensorDataInvalid @42; - commIssue @43; - commIssueAvgFreq @44; - tooDistracted @45; - posenetInvalid @46; - preLaneChangeLeft @48; - preLaneChangeRight @49; - laneChange @50; - lowMemory @51; - stockAeb @52; - stockLkas @98; - ldw @53; - carUnrecognized @54; - invalidLkasSetting @55; - speedTooHigh @56; - laneChangeBlocked @57; - relayMalfunction @58; - stockFcw @59; - startup @60; - startupNoCar @61; - startupNoControl @62; - startupNoSecOcKey @63; - startupMaster @64; - fcw @65; - steerSaturated @66; - belowEngageSpeed @67; - noGps @68; - wrongCruiseMode @69; - modeldLagging @70; - deviceFalling @71; - fanMalfunction @72; - cameraMalfunction @73; - cameraFrameRate @74; - processNotRunning @75; - dashcamMode @76; - selfdriveInitializing @77; - usbError @78; - cruiseMismatch @79; - canBusMissing @80; - selfdrivedLagging @81; - resumeBlocked @82; - steerTimeLimit @83; - vehicleSensorsInvalid @84; - locationdTemporaryError @85; - locationdPermanentError @86; - paramsdTemporaryError @87; - paramsdPermanentError @88; - actuatorsApiUnavailable @89; - espActive @90; - personalityChanged @91; - aeb @92; - userBookmark @95; - excessiveActuation @96; - audioFeedback @97; - - soundsUnavailableDEPRECATED @47; - } -} - -enum LongitudinalPersonality { - aggressive @0; - standard @1; - relaxed @2; -} - -struct InitData { - kernelArgs @0 :List(Text); - kernelVersion @15 :Text; - osVersion @18 :Text; - - dongleId @2 :Text; - bootlogId @22 :Text; - - deviceType @3 :DeviceType; - version @4 :Text; - gitCommit @10 :Text; - gitCommitDate @21 :Text; - gitBranch @11 :Text; - gitRemote @13 :Text; - - # this is source commit for prebuilt branches - gitSrcCommit @23 :Text; - gitSrcCommitDate @24 :Text; - - androidProperties @16 :Map(Text, Text); - - pandaInfo @8 :PandaInfo; - - dirty @9 :Bool; - passive @12 :Bool; - params @17 :Map(Text, Data); - - commands @19 :Map(Text, Data); - - wallTimeNanos @20 :UInt64; - - enum DeviceType { - unknown @0; - neo @1; - chffrAndroid @2; - chffrIos @3; - tici @4; - pc @5; - tizi @6; - mici @7; - } - - struct PandaInfo { - hasPanda @0 :Bool; - dongleId @1 :Text; - stVersion @2 :Text; - espVersion @3 :Text; - } - - # ***** deprecated stuff ***** - gctxDEPRECATED @1 :Text; - androidBuildInfo @5 :AndroidBuildInfo; - androidSensorsDEPRECATED @6 :List(AndroidSensor); - chffrAndroidExtraDEPRECATED @7 :ChffrAndroidExtra; - iosBuildInfoDEPRECATED @14 :IosBuildInfo; - - struct AndroidBuildInfo { - board @0 :Text; - bootloader @1 :Text; - brand @2 :Text; - device @3 :Text; - display @4 :Text; - fingerprint @5 :Text; - hardware @6 :Text; - host @7 :Text; - id @8 :Text; - manufacturer @9 :Text; - model @10 :Text; - product @11 :Text; - radioVersion @12 :Text; - serial @13 :Text; - supportedAbis @14 :List(Text); - tags @15 :Text; - time @16 :Int64; - type @17 :Text; - user @18 :Text; - - versionCodename @19 :Text; - versionRelease @20 :Text; - versionSdk @21 :Int32; - versionSecurityPatch @22 :Text; - } - - struct AndroidSensor { - id @0 :Int32; - name @1 :Text; - vendor @2 :Text; - version @3 :Int32; - handle @4 :Int32; - type @5 :Int32; - maxRange @6 :Float32; - resolution @7 :Float32; - power @8 :Float32; - minDelay @9 :Int32; - fifoReservedEventCount @10 :UInt32; - fifoMaxEventCount @11 :UInt32; - stringType @12 :Text; - maxDelay @13 :Int32; - } - - struct ChffrAndroidExtra { - allCameraCharacteristics @0 :Map(Text, Text); - } - - struct IosBuildInfo { - appVersion @0 :Text; - appBuild @1 :UInt32; - osVersion @2 :Text; - deviceModel @3 :Text; - } -} - -struct FrameData { - frameId @0 :UInt32; - frameIdSensor @25 :UInt32; - requestId @28 :UInt32; - encodeId @1 :UInt32; - - # Timestamps - timestampEof @2 :UInt64; - timestampSof @8 :UInt64; - processingTime @23 :Float32; - - # Exposure - integLines @4 :Int32; - highConversionGain @20 :Bool; - gain @15 :Float32; # This includes highConversionGain if enabled - measuredGreyFraction @21 :Float32; - targetGreyFraction @22 :Float32; - exposureValPercent @27 :Float32; - - transform @10 :List(Float32); - - image @6 :Data; - - temperaturesC @24 :List(Float32); - - enum FrameTypeDEPRECATED { - unknown @0; - neo @1; - chffrAndroid @2; - front @3; - } - - sensor @26 :ImageSensor; - enum ImageSensor { - unknown @0; - ar0231 @1; - ox03c10 @2; - os04c10 @3; - } - - frameLengthDEPRECATED @3 :Int32; - globalGainDEPRECATED @5 :Int32; - frameTypeDEPRECATED @7 :FrameTypeDEPRECATED; - androidCaptureResultDEPRECATED @9 :AndroidCaptureResult; - lensPosDEPRECATED @11 :Int32; - lensSagDEPRECATED @12 :Float32; - lensErrDEPRECATED @13 :Float32; - lensTruePosDEPRECATED @14 :Float32; - focusValDEPRECATED @16 :List(Int16); - focusConfDEPRECATED @17 :List(UInt8); - sharpnessScoreDEPRECATED @18 :List(UInt16); - recoverStateDEPRECATED @19 :Int32; - struct AndroidCaptureResult { - sensitivity @0 :Int32; - frameDuration @1 :Int64; - exposureTime @2 :Int64; - rollingShutterSkew @3 :UInt64; - colorCorrectionTransform @4 :List(Int32); - colorCorrectionGains @5 :List(Float32); - displayRotation @6 :Int8; - } -} - -struct Thumbnail { - frameId @0 :UInt32; - timestampEof @1 :UInt64; - thumbnail @2 :Data; - encoding @3 :Encoding; - - enum Encoding { - unknown @0; - jpeg @1; - keyframe @2; - } -} - -struct GPSNMEAData { - timestamp @0 :Int64; - localWallTime @1 :UInt64; - nmea @2 :Text; -} - -# android sensor_event_t -struct SensorEventData { - version @0 :Int32; - sensor @1 :Int32; - type @2 :Int32; - timestamp @3 :Int64; - uncalibratedDEPRECATED @10 :Bool; - - union { - acceleration @4 :SensorVec; - magnetic @5 :SensorVec; - orientation @6 :SensorVec; - gyro @7 :SensorVec; - pressure @9 :SensorVec; - magneticUncalibrated @11 :SensorVec; - gyroUncalibrated @12 :SensorVec; - proximity @13: Float32; - light @14: Float32; - temperature @15: Float32; - } - source @8 :SensorSource; - - struct SensorVec { - v @0 :List(Float32); - status @1 :Int8; - } - - enum SensorSource { - android @0; - iOS @1; - fiber @2; - velodyne @3; # Velodyne IMU - bno055 @4; # Bosch accelerometer - lsm6ds3 @5; # includes LSM6DS3 and LSM6DS3TR, TR = tape reel - bmp280 @6; # barometer - mmc3416x @7; # magnetometer - bmx055 @8; - rpr0521 @9; - lsm6ds3trc @10; - mmc5603nj @11; - } -} - -# android struct GpsLocation -struct GpsLocationData { - # Contains module-specific flags. - flags @0 :UInt16; - - # Represents latitude in degrees. - latitude @1 :Float64; - - # Represents longitude in degrees. - longitude @2 :Float64; - - # Represents altitude in meters above the WGS 84 reference ellipsoid. - altitude @3 :Float64; - - # Represents speed in meters per second. - speed @4 :Float32; - - # Represents heading in degrees. - bearingDeg @5 :Float32; - - # Represents expected horizontal accuracy in meters. - horizontalAccuracy @6 :Float32; - - unixTimestampMillis @7 :Int64; - - source @8 :SensorSource; - - # Represents NED velocity in m/s. - vNED @9 :List(Float32); - - # Represents expected vertical accuracy in meters. (presumably 1 sigma?) - verticalAccuracy @10 :Float32; - - # Represents bearing accuracy in degrees. (presumably 1 sigma?) - bearingAccuracyDeg @11 :Float32; - - # Represents velocity accuracy in m/s. (presumably 1 sigma?) - speedAccuracy @12 :Float32; - - hasFix @13 :Bool; - satelliteCount @14 :Int8; - - enum SensorSource { - android @0; - iOS @1; - car @2; - velodyne @3; # Velodyne IMU - fusion @4; - external @5; - ublox @6; - trimble @7; - qcomdiag @8; - unicore @9; - } -} - -enum Desire { - none @0; - turnLeft @1; - turnRight @2; - laneChangeLeft @3; - laneChangeRight @4; - keepLeft @5; - keepRight @6; -} - -enum LaneChangeState { - off @0; - preLaneChange @1; - laneChangeStarting @2; - laneChangeFinishing @3; -} - -enum LaneChangeDirection { - none @0; - left @1; - right @2; -} - -struct CanData { - address @0 :UInt32; - dat @2 :Data; - src @3 :UInt8; - busTimeDEPRECATED @1 :UInt16; -} - -struct DeviceState @0xa4d8b5af2aa492eb { - deviceType @45 :InitData.DeviceType; - - networkType @22 :NetworkType; - networkInfo @31 :NetworkInfo; - networkStrength @24 :NetworkStrength; - networkStats @43 :NetworkStats; - networkMetered @41 :Bool; - lastAthenaPingTime @32 :UInt64; - - started @11 :Bool; - startedMonoTime @13 :UInt64; - - # system utilization - freeSpacePercent @7 :Float32; - memoryUsagePercent @19 :Int8; - gpuUsagePercent @33 :Int8; - cpuUsagePercent @34 :List(Int8); # per-core cpu usage - - # power - offroadPowerUsageUwh @23 :UInt32; - carBatteryCapacityUwh @25 :UInt32; - powerDrawW @40 :Float32; - somPowerDrawW @42 :Float32; - - # device thermals - cpuTempC @26 :List(Float32); - gpuTempC @27 :List(Float32); - dspTempC @49 :Float32; - memoryTempC @28 :Float32; - modemTempC @36 :List(Float32); - pmicTempC @39 :List(Float32); - intakeTempC @46 :Float32; - exhaustTempC @47 :Float32; - caseTempC @48 :Float32; - maxTempC @44 :Float32; # max of other temps, used to control fan - thermalZones @38 :List(ThermalZone); - thermalStatus @14 :ThermalStatus; - - fanSpeedPercentDesired @10 :UInt16; - screenBrightnessPercent @37 :Int8; - - struct ThermalZone { - name @0 :Text; - temp @1 :Float32; - } - - enum ThermalStatus { - green @0; - yellow @1; - red @2; - danger @3; - } - - enum NetworkType { - none @0; - wifi @1; - cell2G @2; - cell3G @3; - cell4G @4; - cell5G @5; - ethernet @6; - } - - enum NetworkStrength { - unknown @0; - poor @1; - moderate @2; - good @3; - great @4; - } - - struct NetworkInfo { - technology @0 :Text; - operator @1 :Text; - band @2 :Text; - channel @3 :UInt16; - extra @4 :Text; - state @5 :Text; - } - - struct NetworkStats { - wwanTx @0 :Int64; - wwanRx @1 :Int64; - } - - # deprecated - cpu0DEPRECATED @0 :UInt16; - cpu1DEPRECATED @1 :UInt16; - cpu2DEPRECATED @2 :UInt16; - cpu3DEPRECATED @3 :UInt16; - memDEPRECATED @4 :UInt16; - gpuDEPRECATED @5 :UInt16; - batDEPRECATED @6 :UInt32; - pa0DEPRECATED @21 :UInt16; - cpuUsagePercentDEPRECATED @20 :Int8; - batteryStatusDEPRECATED @9 :Text; - batteryVoltageDEPRECATED @16 :Int32; - batteryTempCDEPRECATED @29 :Float32; - batteryPercentDEPRECATED @8 :Int16; - batteryCurrentDEPRECATED @15 :Int32; - chargingErrorDEPRECATED @17 :Bool; - chargingDisabledDEPRECATED @18 :Bool; - usbOnlineDEPRECATED @12 :Bool; - ambientTempCDEPRECATED @30 :Float32; - nvmeTempCDEPRECATED @35 :List(Float32); -} - -struct PandaState @0xa7649e2575e4591e { - ignitionLine @2 :Bool; - rxBufferOverflow @7 :UInt32; - txBufferOverflow @8 :UInt32; - pandaType @10 :PandaType; - ignitionCan @13 :Bool; - faultStatus @15 :FaultStatus; - powerSaveEnabled @16 :Bool; - uptime @17 :UInt32; - faults @18 :List(FaultType); - heartbeatLost @22 :Bool; - interruptLoad @25 :Float32; - fanPower @28 :UInt8; - - spiErrorCount @33 :UInt16; - - harnessStatus @21 :HarnessStatus; - sbu1Voltage @35 :Float32; - sbu2Voltage @36 :Float32; - - # can health - canState0 @29 :PandaCanState; - canState1 @30 :PandaCanState; - canState2 @31 :PandaCanState; - - # safety stuff - controlsAllowed @3 :Bool; - safetyRxInvalid @19 :UInt32; - safetyTxBlocked @24 :UInt32; - safetyModel @14 :Car.CarParams.SafetyModel; - safetyParam @27 :UInt16; - alternativeExperience @23 :Int16; - safetyRxChecksInvalid @32 :Bool; - - voltage @0 :UInt32; - current @1 :UInt32; - - enum FaultStatus { - none @0; - faultTemp @1; - faultPerm @2; - } - - enum FaultType { - relayMalfunction @0; - unusedInterruptHandled @1; - interruptRateCan1 @2; - interruptRateCan2 @3; - interruptRateCan3 @4; - interruptRateTach @5; - interruptRateGmlanDEPRECATED @6; - interruptRateInterrupts @7; - interruptRateSpiDma @8; - interruptRateSpiCs @9; - interruptRateUart1 @10; - interruptRateUart2 @11; - interruptRateUart3 @12; - interruptRateUart5 @13; - interruptRateUartDma @14; - interruptRateUsb @15; - interruptRateTim1 @16; - interruptRateTim3 @17; - registerDivergent @18; - interruptRateKlineInit @19; - interruptRateClockSource @20; - interruptRateTick @21; - interruptRateExti @22; - interruptRateSpi @23; - interruptRateUart7 @24; - sirenMalfunction @25; - heartbeatLoopWatchdog @26; - # Update max fault type in boardd when adding faults - } - - enum PandaType @0x8a58adf93e5b3751 { - unknown @0; - whitePanda @1; - greyPanda @2; - blackPanda @3; - pedal @4; - uno @5; - dos @6; - redPanda @7; - redPandaV2 @8; - tres @9; - cuatro @10; - } - - enum HarnessStatus { - notConnected @0; - normal @1; - flipped @2; - } - - struct PandaCanState { - busOff @0 :Bool; - busOffCnt @1 :UInt32; - errorWarning @2 :Bool; - errorPassive @3 :Bool; - lastError @4 :LecErrorCode; - lastStoredError @5 :LecErrorCode; - lastDataError @6 :LecErrorCode; - lastDataStoredError @7 :LecErrorCode; - receiveErrorCnt @8 :UInt8; - transmitErrorCnt @9 :UInt8; - totalErrorCnt @10 :UInt32; - totalTxLostCnt @11 :UInt32; - totalRxLostCnt @12 :UInt32; - totalTxCnt @13 :UInt32; - totalRxCnt @14 :UInt32; - totalFwdCnt @15 :UInt32; - canSpeed @16 :UInt16; - canDataSpeed @17 :UInt16; - canfdEnabled @18 :Bool; - brsEnabled @19 :Bool; - canfdNonIso @20 :Bool; - irq0CallRate @21 :UInt32; - irq1CallRate @22 :UInt32; - irq2CallRate @23 :UInt32; - canCoreResetCnt @24 :UInt32; - - enum LecErrorCode { - noError @0; - stuffError @1; - formError @2; - ackError @3; - bit1Error @4; - bit0Error @5; - crcError @6; - noChange @7; - } - } - - gasInterceptorDetectedDEPRECATED @4 :Bool; - startedSignalDetectedDEPRECATED @5 :Bool; - hasGpsDEPRECATED @6 :Bool; - gmlanSendErrsDEPRECATED @9 :UInt32; - fanSpeedRpmDEPRECATED @11 :UInt16; - usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; - safetyParamDEPRECATED @20 :Int16; - safetyParam2DEPRECATED @26 :UInt32; - fanStallCountDEPRECATED @34 :UInt8; -} - -struct PeripheralState { - pandaType @0 :PandaState.PandaType; - voltage @1 :UInt32; - current @2 :UInt32; - fanSpeedRpm @3 :UInt16; - - usbPowerModeDEPRECATED @4 :UsbPowerModeDEPRECATED; - enum UsbPowerModeDEPRECATED @0xa8883583b32c9877 { - none @0; - client @1; - cdp @2; - dcp @3; - } -} - -struct RadarState @0x9a185389d6fdd05f { - mdMonoTime @6 :UInt64; - carStateMonoTime @11 :UInt64; - radarErrors @13 :Car.RadarData.Error; - - leadOne @3 :LeadData; - leadTwo @4 :LeadData; - - struct LeadData { - dRel @0 :Float32; - yRel @1 :Float32; - vRel @2 :Float32; - aRel @3 :Float32; - vLead @4 :Float32; - dPath @6 :Float32; - vLat @7 :Float32; - vLeadK @8 :Float32; - aLeadK @9 :Float32; - fcw @10 :Bool; - status @11 :Bool; - aLeadTau @12 :Float32; - modelProb @13 :Float32; - radar @14 :Bool; - radarTrackId @15 :Int32 = -1; - - aLeadDEPRECATED @5 :Float32; - } - - # deprecated - ftMonoTimeDEPRECATED @7 :UInt64; - warpMatrixDEPRECATED @0 :List(Float32); - angleOffsetDEPRECATED @1 :Float32; - calStatusDEPRECATED @2 :Int8; - calCycleDEPRECATED @8 :Int32; - calPercDEPRECATED @9 :Int8; - canMonoTimesDEPRECATED @10 :List(UInt64); - cumLagMsDEPRECATED @5 :Float32; - radarErrorsDEPRECATED @12 :List(Car.RadarData.ErrorDEPRECATED); -} - -struct LiveCalibrationData { - calStatus @11 :Status; - calCycle @2 :Int32; - calPerc @3 :Int8; - validBlocks @9 :Int32; - - # view_frame_from_road_frame - # ui's is inversed needs new - extrinsicMatrix @4 :List(Float32); - # the direction of travel vector in device frame - rpyCalib @7 :List(Float32); - rpyCalibSpread @8 :List(Float32); - wideFromDeviceEuler @10 :List(Float32); - height @12 :List(Float32); - - warpMatrixDEPRECATED @0 :List(Float32); - calStatusDEPRECATED @1 :Int8; - warpMatrix2DEPRECATED @5 :List(Float32); - warpMatrixBigDEPRECATED @6 :List(Float32); - - enum Status { - uncalibrated @0; - calibrated @1; - invalid @2; - recalibrating @3; - } -} - -struct LiveTracksDEPRECATED { - trackId @0 :Int32; - dRel @1 :Float32; - yRel @2 :Float32; - vRel @3 :Float32; - aRel @4 :Float32; - timeStamp @5 :Float32; - status @6 :Float32; - currentTime @7 :Float32; - stationary @8 :Bool; - oncoming @9 :Bool; -} - -struct SelfdriveState { - # high level system state - state @0 :OpenpilotState; - enabled @1 :Bool; - active @2 :Bool; - engageable @9 :Bool; # can OP be engaged? - - # UI alerts - alertText1 @3 :Text; - alertText2 @4 :Text; - alertStatus @5 :AlertStatus; - alertSize @6 :AlertSize; - alertType @7 :Text; - alertSound @8 :Car.CarControl.HUDControl.AudibleAlert; - alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert; - - # configurable driving settings - experimentalMode @10 :Bool; - personality @11 :LongitudinalPersonality; - - enum OpenpilotState @0xdbe58b96d2d1ac61 { - disabled @0; - preEnabled @1; - enabled @2; - softDisabling @3; - overriding @4; # superset of overriding with steering or accelerator - } - - enum AlertStatus @0xa0d0dcd113193c62 { - normal @0; - userPrompt @1; - critical @2; - } - - enum AlertSize @0xe98bb99d6e985f64 { - none @0; - small @1; - mid @2; - full @3; - } -} - -struct ControlsState @0x97ff69c53601abf1 { - longitudinalPlanMonoTime @28 :UInt64; - lateralPlanMonoTime @50 :UInt64; - - longControlState @30 :Car.CarControl.Actuators.LongControlState; - upAccelCmd @4 :Float32; - uiAccelCmd @5 :Float32; - ufAccelCmd @33 :Float32; - curvature @37 :Float32; # path curvature from vehicle model - desiredCurvature @61 :Float32; # lag adjusted curvatures used by lateral controllers - forceDecel @51 :Bool; - - lateralControlState :union { - pidState @53 :LateralPIDState; - angleState @58 :LateralAngleState; - debugState @59 :LateralDebugState; - torqueState @60 :LateralTorqueState; - - curvatureStateDEPRECATED @65 :LateralCurvatureState; - lqrStateDEPRECATED @55 :LateralLQRState; - indiStateDEPRECATED @52 :LateralINDIState; - } - - struct LateralINDIState { - active @0 :Bool; - steeringAngleDeg @1 :Float32; - steeringRateDeg @2 :Float32; - steeringAccelDeg @3 :Float32; - rateSetPoint @4 :Float32; - accelSetPoint @5 :Float32; - accelError @6 :Float32; - delayedOutput @7 :Float32; - delta @8 :Float32; - output @9 :Float32; - saturated @10 :Bool; - steeringAngleDesiredDeg @11 :Float32; - steeringRateDesiredDeg @12 :Float32; - } - - struct LateralPIDState { - active @0 :Bool; - steeringAngleDeg @1 :Float32; - steeringRateDeg @2 :Float32; - angleError @3 :Float32; - p @4 :Float32; - i @5 :Float32; - f @6 :Float32; - output @7 :Float32; - saturated @8 :Bool; - steeringAngleDesiredDeg @9 :Float32; - } - - struct LateralTorqueState { - active @0 :Bool; - error @1 :Float32; - errorRate @8 :Float32; - p @2 :Float32; - i @3 :Float32; - d @4 :Float32; - f @5 :Float32; - output @6 :Float32; - saturated @7 :Bool; - actualLateralAccel @9 :Float32; - desiredLateralAccel @10 :Float32; - desiredLateralJerk @11 :Float32; - version @12 :Int32; - } - - struct LateralLQRState { - active @0 :Bool; - steeringAngleDeg @1 :Float32; - i @2 :Float32; - output @3 :Float32; - lqrOutput @4 :Float32; - saturated @5 :Bool; - steeringAngleDesiredDeg @6 :Float32; - } - - struct LateralAngleState { - active @0 :Bool; - steeringAngleDeg @1 :Float32; - output @2 :Float32; - saturated @3 :Bool; - steeringAngleDesiredDeg @4 :Float32; - } - - struct LateralCurvatureState { - active @0 :Bool; - actualCurvature @1 :Float32; - desiredCurvature @2 :Float32; - error @3 :Float32; - p @4 :Float32; - i @5 :Float32; - f @6 :Float32; - output @7 :Float32; - saturated @8 :Bool; - } - - struct LateralDebugState { - active @0 :Bool; - steeringAngleDeg @1 :Float32; - output @2 :Float32; - saturated @3 :Bool; - } - - # deprecated - vEgoDEPRECATED @0 :Float32; - vEgoRawDEPRECATED @32 :Float32; - aEgoDEPRECATED @1 :Float32; - canMonoTimeDEPRECATED @16 :UInt64; - radarStateMonoTimeDEPRECATED @17 :UInt64; - mdMonoTimeDEPRECATED @18 :UInt64; - yActualDEPRECATED @6 :Float32; - yDesDEPRECATED @7 :Float32; - upSteerDEPRECATED @8 :Float32; - uiSteerDEPRECATED @9 :Float32; - ufSteerDEPRECATED @34 :Float32; - aTargetMinDEPRECATED @10 :Float32; - aTargetMaxDEPRECATED @11 :Float32; - rearViewCamDEPRECATED @23 :Bool; - driverMonitoringOnDEPRECATED @43 :Bool; - hudLeadDEPRECATED @14 :Int32; - alertSoundDEPRECATED @45 :Text; - angleModelBiasDEPRECATED @27 :Float32; - gpsPlannerActiveDEPRECATED @40 :Bool; - decelForTurnDEPRECATED @47 :Bool; - decelForModelDEPRECATED @54 :Bool; - awarenessStatusDEPRECATED @26 :Float32; - angleSteersDEPRECATED @13 :Float32; - vCurvatureDEPRECATED @46 :Float32; - mapValidDEPRECATED @49 :Bool; - jerkFactorDEPRECATED @12 :Float32; - steerOverrideDEPRECATED @20 :Bool; - steeringAngleDesiredDegDEPRECATED @29 :Float32; - canMonoTimesDEPRECATED @21 :List(UInt64); - desiredCurvatureRateDEPRECATED @62 :Float32; - canErrorCounterDEPRECATED @57 :UInt32; - vPidDEPRECATED @2 :Float32; - alertBlinkingRateDEPRECATED @42 :Float32; - alertText1DEPRECATED @24 :Text; - alertText2DEPRECATED @25 :Text; - alertStatusDEPRECATED @38 :SelfdriveState.AlertStatus; - alertSizeDEPRECATED @39 :SelfdriveState.AlertSize; - alertTypeDEPRECATED @44 :Text; - alertSound2DEPRECATED @56 :Car.CarControl.HUDControl.AudibleAlert; - engageableDEPRECATED @41 :Bool; # can OP be engaged? - stateDEPRECATED @31 :SelfdriveState.OpenpilotState; - enabledDEPRECATED @19 :Bool; - activeDEPRECATED @36 :Bool; - experimentalModeDEPRECATED @64 :Bool; - personalityDEPRECATED @66 :LongitudinalPersonality; - vCruiseDEPRECATED @22 :Float32; # actual set speed - vCruiseClusterDEPRECATED @63 :Float32; # set speed to display in the UI - startMonoTimeDEPRECATED @48 :UInt64; - cumLagMsDEPRECATED @15 :Float32; - aTargetDEPRECATED @35 :Float32; - vTargetLeadDEPRECATED @3 :Float32; -} - -struct DrivingModelData { - frameId @0 :UInt32; - frameIdExtra @1 :UInt32; - frameDropPerc @6 :Float32; - modelExecutionTime @7 :Float32; - - action @2 :ModelDataV2.Action; - - laneLineMeta @3 :LaneLineMeta; - meta @4 :MetaData; - - path @5 :PolyPath; - - struct PolyPath { - xCoefficients @0 :List(Float32); - yCoefficients @1 :List(Float32); - zCoefficients @2 :List(Float32); - } - - struct LaneLineMeta { - leftY @0 :Float32; - rightY @1 :Float32; - leftProb @2 :Float32; - rightProb @3 :Float32; - } - - struct MetaData { - laneChangeState @0 :LaneChangeState; - laneChangeDirection @1 :LaneChangeDirection; - } -} - -# All SI units and in device frame -struct XYZTData @0xc3cbae1fd505ae80 { - x @0 :List(Float32); - y @1 :List(Float32); - z @2 :List(Float32); - t @3 :List(Float32); - xStd @4 :List(Float32); - yStd @5 :List(Float32); - zStd @6 :List(Float32); -} - -struct ModelDataV2 { - frameId @0 :UInt32; - frameIdExtra @20 :UInt32; - frameAge @1 :UInt32; - frameDropPerc @2 :Float32; - timestampEof @3 :UInt64; - modelExecutionTime @15 :Float32; - rawPredictions @16 :Data; - - # predicted future position, orientation, etc.. - position @4 :XYZTData; - orientation @5 :XYZTData; - velocity @6 :XYZTData; - orientationRate @7 :XYZTData; - acceleration @19 :XYZTData; - - # prediction lanelines and road edges - laneLines @8 :List(XYZTData); - laneLineProbs @9 :List(Float32); - laneLineStds @13 :List(Float32); - roadEdges @10 :List(XYZTData); - roadEdgeStds @14 :List(Float32); - - # predicted lead cars - leads @11 :List(LeadDataV2); - leadsV3 @18 :List(LeadDataV3); - - meta @12 :MetaData; - confidence @23: ConfidenceClass; - - # Model perceived motion - temporalPoseDEPRECATED @21 :Pose; - - # e2e lateral planner - action @26: Action; - - gpuExecutionTimeDEPRECATED @17 :Float32; - navEnabledDEPRECATED @22 :Bool; - locationMonoTimeDEPRECATED @24 :UInt64; - lateralPlannerSolutionDEPRECATED @25: LateralPlannerSolution; - - struct LeadDataV2 { - prob @0 :Float32; # probability that car is your lead at time t - t @1 :Float32; - - # x and y are relative position in device frame - # v is norm relative speed - # a is norm relative acceleration - xyva @2 :List(Float32); - xyvaStd @3 :List(Float32); - } - - struct LeadDataV3 { - prob @0 :Float32; # probability that car is your lead at time t - probTime @1 :Float32; - t @2 :List(Float32); - - # x and y are relative position in device frame - # v absolute norm speed - # a is derivative of v - x @3 :List(Float32); - xStd @4 :List(Float32); - y @5 :List(Float32); - yStd @6 :List(Float32); - v @7 :List(Float32); - vStd @8 :List(Float32); - a @9 :List(Float32); - aStd @10 :List(Float32); - } - - - struct MetaData { - engagedProb @0 :Float32; - desirePrediction @1 :List(Float32); - desireState @5 :List(Float32); - disengagePredictions @6 :DisengagePredictions; - hardBrakePredicted @7 :Bool; - laneChangeState @8 :LaneChangeState; - laneChangeDirection @9 :LaneChangeDirection; - - - # deprecated - brakeDisengageProbDEPRECATED @2 :Float32; - gasDisengageProbDEPRECATED @3 :Float32; - steerOverrideProbDEPRECATED @4 :Float32; - } - - enum ConfidenceClass { - red @0; - yellow @1; - green @2; - } - - struct DisengagePredictions { - t @0 :List(Float32); - brakeDisengageProbs @1 :List(Float32); - gasDisengageProbs @2 :List(Float32); - steerOverrideProbs @3 :List(Float32); - brake3MetersPerSecondSquaredProbs @4 :List(Float32); - brake4MetersPerSecondSquaredProbs @5 :List(Float32); - brake5MetersPerSecondSquaredProbs @6 :List(Float32); - gasPressProbs @7 :List(Float32); - brakePressProbs @8 :List(Float32); - } - - struct Pose { - trans @0 :List(Float32); # m/s in device frame - rot @1 :List(Float32); # rad/s in device frame - transStd @2 :List(Float32); # std m/s in device frame - rotStd @3 :List(Float32); # std rad/s in device frame - } - - struct LateralPlannerSolution { - x @0 :List(Float32); - y @1 :List(Float32); - yaw @2 :List(Float32); - yawRate @3 :List(Float32); - xStd @4 :List(Float32); - yStd @5 :List(Float32); - yawStd @6 :List(Float32); - yawRateStd @7 :List(Float32); - } - - struct Action { - desiredCurvature @0 :Float32; - desiredAcceleration @1 :Float32; - shouldStop @2 :Bool; - } -} - -struct EncodeIndex { - # picture from camera - frameId @0 :UInt32; - type @1 :Type; - # index of encoder from start of route - encodeId @2 :UInt32; - # minute long segment this frame is in - segmentNum @3 :Int32; - # index into camera file in segment in presentation order - segmentId @4 :UInt32; - # index into camera file in segment in encode order - segmentIdEncode @5 :UInt32; - timestampSof @6 :UInt64; - timestampEof @7 :UInt64; - - # encoder metadata - flags @8 :UInt32; - len @9 :UInt32; - - enum Type { - bigBoxLossless @0; - fullHEVC @1; - qcameraH264 @6; - livestreamH264 @7; - - # deprecated - bigBoxHEVCDEPRECATED @2; - chffrAndroidH264DEPRECATED @3; - fullLosslessClipDEPRECATED @4; - frontDEPRECATED @5; - - } -} - -struct AndroidLogEntry { - id @0 :UInt8; - ts @1 :UInt64; - priority @2 :UInt8; - pid @3 :Int32; - tid @4 :Int32; - tag @5 :Text; - message @6 :Text; -} - -struct DriverAssistance { - # Lane Departure Warnings - leftLaneDeparture @0 :Bool; - rightLaneDeparture @1 :Bool; - - # FCW, AEB, etc. will go here -} - -struct LongitudinalPlan @0xe00b5b3eba12876c { - modelMonoTime @9 :UInt64; - hasLead @7 :Bool; - fcw @8 :Bool; - longitudinalPlanSource @15 :LongitudinalPlanSource; - processingDelay @29 :Float32; - - # desired speed/accel/jerk over next 2.5s - accels @32 :List(Float32); - speeds @33 :List(Float32); - jerks @34 :List(Float32); - aTarget @18 :Float32; - shouldStop @37: Bool; - allowThrottle @38: Bool; - allowBrake @39: Bool; - - - solverExecutionTime @35 :Float32; - - enum LongitudinalPlanSource { - cruise @0; - lead0 @1; - lead1 @2; - lead2 @3; - e2e @4; - } - - # deprecated - vCruiseDEPRECATED @16 :Float32; - aCruiseDEPRECATED @17 :Float32; - vTargetDEPRECATED @3 :Float32; - vTargetFutureDEPRECATED @14 :Float32; - vStartDEPRECATED @26 :Float32; - aStartDEPRECATED @27 :Float32; - vMaxDEPRECATED @20 :Float32; - radarStateMonoTimeDEPRECATED @10 :UInt64; - jerkFactorDEPRECATED @6 :Float32; - hasLeftLaneDEPRECATED @23 :Bool; - hasRightLaneDEPRECATED @24 :Bool; - aTargetMinDEPRECATED @4 :Float32; - aTargetMaxDEPRECATED @5 :Float32; - lateralValidDEPRECATED @0 :Bool; - longitudinalValidDEPRECATED @2 :Bool; - dPolyDEPRECATED @1 :List(Float32); - laneWidthDEPRECATED @11 :Float32; - vCurvatureDEPRECATED @21 :Float32; - decelForTurnDEPRECATED @22 :Bool; - mapValidDEPRECATED @25 :Bool; - radarValidDEPRECATED @28 :Bool; - radarCanErrorDEPRECATED @30 :Bool; - commIssueDEPRECATED @31 :Bool; - eventsDEPRECATED @13 :List(Car.OnroadEventDEPRECATED); - gpsTrajectoryDEPRECATED @12 :GpsTrajectory; - gpsPlannerActiveDEPRECATED @19 :Bool; - personalityDEPRECATED @36 :LongitudinalPersonality; - - struct GpsTrajectory { - x @0 :List(Float32); - y @1 :List(Float32); - } -} -struct UiPlan { - frameId @2 :UInt32; - position @0 :XYZTData; - accel @1 :List(Float32); -} - -struct LateralPlan @0xe1e9318e2ae8b51e { - modelMonoTime @31 :UInt64; - laneWidthDEPRECATED @0 :Float32; - lProbDEPRECATED @5 :Float32; - rProbDEPRECATED @7 :Float32; - dPathPoints @20 :List(Float32); - dProbDEPRECATED @21 :Float32; - - mpcSolutionValid @9 :Bool; - desire @17 :Desire; - laneChangeState @18 :LaneChangeState; - laneChangeDirection @19 :LaneChangeDirection; - useLaneLines @29 :Bool; - - # desired curvatures over next 2.5s in rad/m - psis @26 :List(Float32); - curvatures @27 :List(Float32); - curvatureRates @28 :List(Float32); - - solverExecutionTime @30 :Float32; - solverCost @32 :Float32; - solverState @33 :SolverState; - - struct SolverState { - x @0 :List(List(Float32)); - u @1 :List(Float32); - } - - # deprecated - curvatureDEPRECATED @22 :Float32; - curvatureRateDEPRECATED @23 :Float32; - rawCurvatureDEPRECATED @24 :Float32; - rawCurvatureRateDEPRECATED @25 :Float32; - cProbDEPRECATED @3 :Float32; - dPolyDEPRECATED @1 :List(Float32); - cPolyDEPRECATED @2 :List(Float32); - lPolyDEPRECATED @4 :List(Float32); - rPolyDEPRECATED @6 :List(Float32); - modelValidDEPRECATED @12 :Bool; - commIssueDEPRECATED @15 :Bool; - posenetValidDEPRECATED @16 :Bool; - sensorValidDEPRECATED @14 :Bool; - paramsValidDEPRECATED @10 :Bool; - steeringAngleDegDEPRECATED @8 :Float32; # deg - steeringRateDegDEPRECATED @13 :Float32; # deg/s - angleOffsetDegDEPRECATED @11 :Float32; -} - -struct LiveLocationKalman { - - # More info on reference frames: - # https://github.com/commaai/openpilot/tree/master/common/transformations - - positionECEF @0 : Measurement; - positionGeodetic @1 : Measurement; - velocityECEF @2 : Measurement; - velocityNED @3 : Measurement; - velocityDevice @4 : Measurement; - accelerationDevice @5: Measurement; - - - # These angles are all eulers and roll, pitch, yaw - # orientationECEF transforms to rot matrix: ecef_from_device - orientationECEF @6 : Measurement; - calibratedOrientationECEF @20 : Measurement; - orientationNED @7 : Measurement; - angularVelocityDevice @8 : Measurement; - - # orientationNEDCalibrated transforms to rot matrix: NED_from_calibrated - calibratedOrientationNED @9 : Measurement; - - # Calibrated frame is simply device frame - # aligned with the vehicle - velocityCalibrated @10 : Measurement; - accelerationCalibrated @11 : Measurement; - angularVelocityCalibrated @12 : Measurement; - - gpsWeek @13 :Int32; - gpsTimeOfWeek @14 :Float64; - status @15 :Status; - unixTimestampMillis @16 :Int64; - inputsOK @17 :Bool = true; - posenetOK @18 :Bool = true; - gpsOK @19 :Bool = true; - sensorsOK @21 :Bool = true; - deviceStable @22 :Bool = true; - timeSinceReset @23 :Float64; - excessiveResets @24 :Bool; - timeToFirstFix @25 :Float32; - - filterState @26 : Measurement; - - enum Status { - uninitialized @0; - uncalibrated @1; - valid @2; - } - - struct Measurement { - value @0 : List(Float64); - std @1 : List(Float64); - valid @2 : Bool; - } -} - - -struct LivePose { - # More info on reference frames: - # https://github.com/commaai/openpilot/tree/master/common/transformations - orientationNED @0 :XYZMeasurement; - velocityDevice @1 :XYZMeasurement; - accelerationDevice @2 :XYZMeasurement; - angularVelocityDevice @3 :XYZMeasurement; - - inputsOK @4 :Bool = false; - posenetOK @5 :Bool = false; - sensorsOK @6 :Bool = false; - - debugFilterState @7 :FilterState; - - struct XYZMeasurement { - x @0 :Float32; - y @1 :Float32; - z @2 :Float32; - xStd @3 :Float32; - yStd @4 :Float32; - zStd @5 :Float32; - valid @6 :Bool; - } - - struct FilterState { - value @0 : List(Float64); - std @1 : List(Float64); - valid @2 : Bool; - - observations @3 :List(Observation); - - struct Observation { - kind @0 :Int32; - value @1 :List(Float32); - error @2 :List(Float32); - } - } -} - -struct ProcLog { - cpuTimes @0 :List(CPUTimes); - mem @1 :Mem; - procs @2 :List(Process); - - struct Process { - pid @0 :Int32; - name @1 :Text; - state @2 :UInt8; - ppid @3 :Int32; - - cpuUser @4 :Float32; - cpuSystem @5 :Float32; - cpuChildrenUser @6 :Float32; - cpuChildrenSystem @7 :Float32; - priority @8 :Int64; - nice @9 :Int32; - numThreads @10 :Int32; - startTime @11 :Float64; - - memVms @12 :UInt64; - memRss @13 :UInt64; - - processor @14 :Int32; - - cmdline @15 :List(Text); - exe @16 :Text; - } - - struct CPUTimes { - cpuNum @0 :Int64; - user @1 :Float32; - nice @2 :Float32; - system @3 :Float32; - idle @4 :Float32; - iowait @5 :Float32; - irq @6 :Float32; - softirq @7 :Float32; - } - - struct Mem { - total @0 :UInt64; - free @1 :UInt64; - available @2 :UInt64; - buffers @3 :UInt64; - cached @4 :UInt64; - active @5 :UInt64; - inactive @6 :UInt64; - shared @7 :UInt64; - } -} - -struct GnssMeasurements { - measTime @0 :UInt64; - gpsWeek @1 :Int16; - gpsTimeOfWeek @2 :Float64; - - correctedMeasurements @3 :List(CorrectedMeasurement); - ephemerisStatuses @9 :List(EphemerisStatus); - - kalmanPositionECEF @4 :LiveLocationKalman.Measurement; - kalmanVelocityECEF @5 :LiveLocationKalman.Measurement; - positionECEF @6 :LiveLocationKalman.Measurement; - velocityECEF @7 :LiveLocationKalman.Measurement; - timeToFirstFix @8 :Float32; - # Todo sync this with timing pulse of ublox - - struct EphemerisStatus { - constellationId @0 :ConstellationId; - svId @1 :UInt8; - type @2 :EphemerisType; - source @3 :EphemerisSource; - gpsWeek @4 : UInt16; - tow @5 :Float64; - } - - struct CorrectedMeasurement { - constellationId @0 :ConstellationId; - svId @1 :UInt8; - # Is 0 when not Glonass constellation. - glonassFrequency @2 :Int8; - pseudorange @3 :Float64; - pseudorangeStd @4 :Float64; - pseudorangeRate @5 :Float64; - pseudorangeRateStd @6 :Float64; - # Satellite position and velocity [x,y,z] - satPos @7 :List(Float64); - satVel @8 :List(Float64); - ephemerisSourceDEPRECATED @9 :EphemerisSourceDEPRECATED; - } - - struct EphemerisSourceDEPRECATED { - type @0 :EphemerisType; - # first epoch in file: - gpsWeek @1 :Int16; # -1 if Nav - gpsTimeOfWeek @2 :Int32; # -1 if Nav. Integer for seconds is good enough for logs. - } - - enum ConstellationId { - # Satellite Constellation using the Ublox gnssid as index - gps @0; - sbas @1; - galileo @2; - beidou @3; - imes @4; - qznss @5; - glonass @6; - } - - enum EphemerisType { - nav @0; - # Different ultra-rapid files: - nasaUltraRapid @1; - glonassIacUltraRapid @2; - qcom @3; - } - - enum EphemerisSource { - gnssChip @0; - internet @1; - cache @2; - unknown @3; - } -} - -struct UbloxGnss { - union { - measurementReport @0 :MeasurementReport; - ephemeris @1 :Ephemeris; - ionoData @2 :IonoData; - hwStatus @3 :HwStatus; - hwStatus2 @4 :HwStatus2; - glonassEphemeris @5 :GlonassEphemeris; - satReport @6 :SatReport; - } - - struct SatReport { - #received time of week in gps time in seconds and gps week - iTow @0 :UInt32; - svs @1 :List(SatInfo); - - struct SatInfo { - svId @0 :UInt8; - gnssId @1 :UInt8; - flagsBitfield @2 :UInt32; - cno @3 :UInt8; - elevationDeg @4 :Int8; - azimuthDeg @5 :Int16; - pseudorangeResidual @6 :Float32; - } - } - - struct MeasurementReport { - #received time of week in gps time in seconds and gps week - rcvTow @0 :Float64; - gpsWeek @1 :UInt16; - # leap seconds in seconds - leapSeconds @2 :UInt16; - # receiver status - receiverStatus @3 :ReceiverStatus; - # num of measurements to follow - numMeas @4 :UInt8; - measurements @5 :List(Measurement); - - struct ReceiverStatus { - # leap seconds have been determined - leapSecValid @0 :Bool; - # Clock reset applied - clkReset @1 :Bool; - } - - struct Measurement { - svId @0 :UInt8; - trackingStatus @1 :TrackingStatus; - # pseudorange in meters - pseudorange @2 :Float64; - # carrier phase measurement in cycles - carrierCycles @3 :Float64; - # doppler measurement in Hz - doppler @4 :Float32; - # GNSS id, 0 is gps - gnssId @5 :UInt8; - glonassFrequencyIndex @6 :UInt8; - # carrier phase locktime counter in ms - locktime @7 :UInt16; - # Carrier-to-noise density ratio (signal strength) in dBHz - cno @8 :UInt8; - # pseudorange standard deviation in meters - pseudorangeStdev @9 :Float32; - # carrier phase standard deviation in cycles - carrierPhaseStdev @10 :Float32; - # doppler standard deviation in Hz - dopplerStdev @11 :Float32; - sigId @12 :UInt8; - - struct TrackingStatus { - # pseudorange valid - pseudorangeValid @0 :Bool; - # carrier phase valid - carrierPhaseValid @1 :Bool; - # half cycle valid - halfCycleValid @2 :Bool; - # half cycle subtracted from phase - halfCycleSubtracted @3 :Bool; - } - } - } - - struct Ephemeris { - # This is according to the rinex (2?) format - svId @0 :UInt16; - year @1 :UInt16; - month @2 :UInt16; - day @3 :UInt16; - hour @4 :UInt16; - minute @5 :UInt16; - second @6 :Float32; - af0 @7 :Float64; - af1 @8 :Float64; - af2 @9 :Float64; - - iode @10 :Float64; - crs @11 :Float64; - deltaN @12 :Float64; - m0 @13 :Float64; - - cuc @14 :Float64; - ecc @15 :Float64; - cus @16 :Float64; - a @17 :Float64; # note that this is not the root!! - - toe @18 :Float64; - cic @19 :Float64; - omega0 @20 :Float64; - cis @21 :Float64; - - i0 @22 :Float64; - crc @23 :Float64; - omega @24 :Float64; - omegaDot @25 :Float64; - - iDot @26 :Float64; - codesL2 @27 :Float64; - gpsWeekDEPRECATED @28 :Float64; - l2 @29 :Float64; - - svAcc @30 :Float64; - svHealth @31 :Float64; - tgd @32 :Float64; - iodc @33 :Float64; - - transmissionTime @34 :Float64; - fitInterval @35 :Float64; - - toc @36 :Float64; - - ionoCoeffsValid @37 :Bool; - ionoAlpha @38 :List(Float64); - ionoBeta @39 :List(Float64); - - towCount @40 :UInt32; - toeWeek @41 :UInt16; - tocWeek @42 :UInt16; - } - - struct IonoData { - svHealth @0 :UInt32; - tow @1 :Float64; - gpsWeek @2 :Float64; - - ionoAlpha @3 :List(Float64); - ionoBeta @4 :List(Float64); - - healthValid @5 :Bool; - ionoCoeffsValid @6 :Bool; - } - - struct HwStatus { - noisePerMS @0 :UInt16; - agcCnt @1 :UInt16; - aStatus @2 :AntennaSupervisorState; - aPower @3 :AntennaPowerStatus; - jamInd @4 :UInt8; - flags @5 :UInt8; - - enum AntennaSupervisorState { - init @0; - dontknow @1; - ok @2; - short @3; - open @4; - } - - enum AntennaPowerStatus { - off @0; - on @1; - dontknow @2; - } - } - - struct HwStatus2 { - ofsI @0 :Int8; - magI @1 :UInt8; - ofsQ @2 :Int8; - magQ @3 :UInt8; - cfgSource @4 :ConfigSource; - lowLevCfg @5 :UInt32; - postStatus @6 :UInt32; - - enum ConfigSource { - undefined @0; - rom @1; - otp @2; - configpins @3; - flash @4; - } - } - - struct GlonassEphemeris { - svId @0 :UInt16; - year @1 :UInt16; - dayInYear @2 :UInt16; - hour @3 :UInt16; - minute @4 :UInt16; - second @5 :Float32; - - x @6 :Float64; - xVel @7 :Float64; - xAccel @8 :Float64; - y @9 :Float64; - yVel @10 :Float64; - yAccel @11 :Float64; - z @12 :Float64; - zVel @13 :Float64; - zAccel @14 :Float64; - - svType @15 :UInt8; - svURA @16 :Float32; - age @17 :UInt8; - - svHealth @18 :UInt8; - tkDEPRECATED @19 :UInt16; - tb @20 :UInt16; - - tauN @21 :Float64; - deltaTauN @22 :Float64; - gammaN @23 :Float64; - - p1 @24 :UInt8; - p2 @25 :UInt8; - p3 @26 :UInt8; - p4 @27 :UInt8; - - freqNumDEPRECATED @28 :UInt32; - - n4 @29 :UInt8; - nt @30 :UInt16; - freqNum @31 :Int16; - tkSeconds @32 :UInt32; - } -} - -struct QcomGnss @0xde94674b07ae51c1 { - logTs @0 :UInt64; - union { - measurementReport @1 :MeasurementReport; - clockReport @2 :ClockReport; - drMeasurementReport @3 :DrMeasurementReport; - drSvPoly @4 :DrSvPolyReport; - rawLog @5 :Data; - } - - enum MeasurementSource @0xd71a12b6faada7ee { - gps @0; - glonass @1; - beidou @2; - unknown3 @3; - unknown4 @4; - unknown5 @5; - sbas @6; - } - - enum SVObservationState @0xe81e829a0d6c83e9 { - idle @0; - search @1; - searchVerify @2; - bitEdge @3; - trackVerify @4; - track @5; - restart @6; - dpo @7; - glo10msBe @8; - glo10msAt @9; - } - - struct MeasurementStatus @0xe501010e1bcae83b { - subMillisecondIsValid @0 :Bool; - subBitTimeIsKnown @1 :Bool; - satelliteTimeIsKnown @2 :Bool; - bitEdgeConfirmedFromSignal @3 :Bool; - measuredVelocity @4 :Bool; - fineOrCoarseVelocity @5 :Bool; - lockPointValid @6 :Bool; - lockPointPositive @7 :Bool; - lastUpdateFromDifference @8 :Bool; - lastUpdateFromVelocityDifference @9 :Bool; - strongIndicationOfCrossCorelation @10 :Bool; - tentativeMeasurement @11 :Bool; - measurementNotUsable @12 :Bool; - sirCheckIsNeeded @13 :Bool; - probationMode @14 :Bool; - - glonassMeanderBitEdgeValid @15 :Bool; - glonassTimeMarkValid @16 :Bool; - - gpsRoundRobinRxDiversity @17 :Bool; - gpsRxDiversity @18 :Bool; - gpsLowBandwidthRxDiversityCombined @19 :Bool; - gpsHighBandwidthNu4 @20 :Bool; - gpsHighBandwidthNu8 @21 :Bool; - gpsHighBandwidthUniform @22 :Bool; - multipathIndicator @23 :Bool; - - imdJammingIndicator @24 :Bool; - lteB13TxJammingIndicator @25 :Bool; - freshMeasurementIndicator @26 :Bool; - - multipathEstimateIsValid @27 :Bool; - directionIsValid @28 :Bool; - } - - struct MeasurementReport @0xf580d7d86b7b8692 { - source @0 :MeasurementSource; - - fCount @1 :UInt32; - - gpsWeek @2 :UInt16; - glonassCycleNumber @3 :UInt8; - glonassNumberOfDays @4 :UInt16; - - milliseconds @5 :UInt32; - timeBias @6 :Float32; - clockTimeUncertainty @7 :Float32; - clockFrequencyBias @8 :Float32; - clockFrequencyUncertainty @9 :Float32; - - sv @10 :List(SV); - - struct SV @0xf10c595ae7bb2c27 { - svId @0 :UInt8; - observationState @2 :SVObservationState; - observations @3 :UInt8; - goodObservations @4 :UInt8; - gpsParityErrorCount @5 :UInt16; - glonassFrequencyIndex @1 :Int8; - glonassHemmingErrorCount @6 :UInt8; - filterStages @7 :UInt8; - carrierNoise @8 :UInt16; - latency @9 :Int16; - predetectInterval @10 :UInt8; - postdetections @11 :UInt16; - - unfilteredMeasurementIntegral @12 :UInt32; - unfilteredMeasurementFraction @13 :Float32; - unfilteredTimeUncertainty @14 :Float32; - unfilteredSpeed @15 :Float32; - unfilteredSpeedUncertainty @16 :Float32; - measurementStatus @17 :MeasurementStatus; - multipathEstimate @18 :UInt32; - azimuth @19 :Float32; - elevation @20 :Float32; - carrierPhaseCyclesIntegral @21 :Int32; - carrierPhaseCyclesFraction @22 :UInt16; - fineSpeed @23 :Float32; - fineSpeedUncertainty @24 :Float32; - cycleSlipCount @25 :UInt8; - } - - } - - struct ClockReport @0xca965e4add8f4f0b { - hasFCount @0 :Bool; - fCount @1 :UInt32; - - hasGpsWeek @2 :Bool; - gpsWeek @3 :UInt16; - hasGpsMilliseconds @4 :Bool; - gpsMilliseconds @5 :UInt32; - gpsTimeBias @6 :Float32; - gpsClockTimeUncertainty @7 :Float32; - gpsClockSource @8 :UInt8; - - hasGlonassYear @9 :Bool; - glonassYear @10 :UInt8; - hasGlonassDay @11 :Bool; - glonassDay @12 :UInt16; - hasGlonassMilliseconds @13 :Bool; - glonassMilliseconds @14 :UInt32; - glonassTimeBias @15 :Float32; - glonassClockTimeUncertainty @16 :Float32; - glonassClockSource @17 :UInt8; - - bdsWeek @18 :UInt16; - bdsMilliseconds @19 :UInt32; - bdsTimeBias @20 :Float32; - bdsClockTimeUncertainty @21 :Float32; - bdsClockSource @22 :UInt8; - - galWeek @23 :UInt16; - galMilliseconds @24 :UInt32; - galTimeBias @25 :Float32; - galClockTimeUncertainty @26 :Float32; - galClockSource @27 :UInt8; - - clockFrequencyBias @28 :Float32; - clockFrequencyUncertainty @29 :Float32; - frequencySource @30 :UInt8; - gpsLeapSeconds @31 :UInt8; - gpsLeapSecondsUncertainty @32 :UInt8; - gpsLeapSecondsSource @33 :UInt8; - - gpsToGlonassTimeBiasMilliseconds @34 :Float32; - gpsToGlonassTimeBiasMillisecondsUncertainty @35 :Float32; - gpsToBdsTimeBiasMilliseconds @36 :Float32; - gpsToBdsTimeBiasMillisecondsUncertainty @37 :Float32; - bdsToGloTimeBiasMilliseconds @38 :Float32; - bdsToGloTimeBiasMillisecondsUncertainty @39 :Float32; - gpsToGalTimeBiasMilliseconds @40 :Float32; - gpsToGalTimeBiasMillisecondsUncertainty @41 :Float32; - galToGloTimeBiasMilliseconds @42 :Float32; - galToGloTimeBiasMillisecondsUncertainty @43 :Float32; - galToBdsTimeBiasMilliseconds @44 :Float32; - galToBdsTimeBiasMillisecondsUncertainty @45 :Float32; - - hasRtcTime @46 :Bool; - systemRtcTime @47 :UInt32; - fCountOffset @48 :UInt32; - lpmRtcCount @49 :UInt32; - clockResets @50 :UInt32; - } - - struct DrMeasurementReport @0x8053c39445c6c75c { - - reason @0 :UInt8; - seqNum @1 :UInt8; - seqMax @2 :UInt8; - rfLoss @3 :UInt16; - - systemRtcValid @4 :Bool; - fCount @5 :UInt32; - clockResets @6 :UInt32; - systemRtcTime @7 :UInt64; - - gpsLeapSeconds @8 :UInt8; - gpsLeapSecondsUncertainty @9 :UInt8; - gpsToGlonassTimeBiasMilliseconds @10 :Float32; - gpsToGlonassTimeBiasMillisecondsUncertainty @11 :Float32; - - gpsWeek @12 :UInt16; - gpsMilliseconds @13 :UInt32; - gpsTimeBiasMs @14 :UInt32; - gpsClockTimeUncertaintyMs @15 :UInt32; - gpsClockSource @16 :UInt8; - - glonassClockSource @17 :UInt8; - glonassYear @18 :UInt8; - glonassDay @19 :UInt16; - glonassMilliseconds @20 :UInt32; - glonassTimeBias @21 :Float32; - glonassClockTimeUncertainty @22 :Float32; - - clockFrequencyBias @23 :Float32; - clockFrequencyUncertainty @24 :Float32; - frequencySource @25 :UInt8; - - source @26 :MeasurementSource; - - sv @27 :List(SV); - - struct SV @0xf08b81df8cbf459c { - svId @0 :UInt8; - glonassFrequencyIndex @1 :Int8; - observationState @2 :SVObservationState; - observations @3 :UInt8; - goodObservations @4 :UInt8; - filterStages @5 :UInt8; - predetectInterval @6 :UInt8; - cycleSlipCount @7 :UInt8; - postdetections @8 :UInt16; - - measurementStatus @9 :MeasurementStatus; - - carrierNoise @10 :UInt16; - rfLoss @11 :UInt16; - latency @12 :Int16; - - filteredMeasurementFraction @13 :Float32; - filteredMeasurementIntegral @14 :UInt32; - filteredTimeUncertainty @15 :Float32; - filteredSpeed @16 :Float32; - filteredSpeedUncertainty @17 :Float32; - - unfilteredMeasurementFraction @18 :Float32; - unfilteredMeasurementIntegral @19 :UInt32; - unfilteredTimeUncertainty @20 :Float32; - unfilteredSpeed @21 :Float32; - unfilteredSpeedUncertainty @22 :Float32; - - multipathEstimate @23 :UInt32; - azimuth @24 :Float32; - elevation @25 :Float32; - dopplerAcceleration @26 :Float32; - fineSpeed @27 :Float32; - fineSpeedUncertainty @28 :Float32; - - carrierPhase @29 :Float64; - fCount @30 :UInt32; - - parityErrorCount @31 :UInt16; - goodParity @32 :Bool; - } - } - - struct DrSvPolyReport @0xb1fb80811a673270 { - svId @0 :UInt16; - frequencyIndex @1 :Int8; - - hasPosition @2 :Bool; - hasIono @3 :Bool; - hasTropo @4 :Bool; - hasElevation @5 :Bool; - polyFromXtra @6 :Bool; - hasSbasIono @7 :Bool; - - iode @8 :UInt16; - t0 @9 :Float64; - xyz0 @10 :List(Float64); - xyzN @11 :List(Float64); - other @12 :List(Float32); - - positionUncertainty @13 :Float32; - ionoDelay @14 :Float32; - ionoDot @15 :Float32; - sbasIonoDelay @16 :Float32; - sbasIonoDot @17 :Float32; - tropoDelay @18 :Float32; - elevation @19 :Float32; - elevationDot @20 :Float32; - elevationUncertainty @21 :Float32; - velocityCoeff @22 :List(Float64); - - gpsWeek @23 :UInt16; - gpsTow @24 :Float64; - } -} - -struct Clocks { - wallTimeNanos @3 :UInt64; # unix epoch time - - bootTimeNanosDEPRECATED @0 :UInt64; - monotonicNanosDEPRECATED @1 :UInt64; - monotonicRawNanosDEPRECATD @2 :UInt64; - modemUptimeMillisDEPRECATED @4 :UInt64; -} - -struct LiveMpcData { - x @0 :List(Float32); - y @1 :List(Float32); - psi @2 :List(Float32); - curvature @3 :List(Float32); - qpIterations @4 :UInt32; - calculationTime @5 :UInt64; - cost @6 :Float64; -} - -struct LiveLongitudinalMpcData { - xEgo @0 :List(Float32); - vEgo @1 :List(Float32); - aEgo @2 :List(Float32); - xLead @3 :List(Float32); - vLead @4 :List(Float32); - aLead @5 :List(Float32); - aLeadTau @6 :Float32; # lead accel time constant - qpIterations @7 :UInt32; - mpcId @8 :UInt32; - calculationTime @9 :UInt64; - cost @10 :Float64; -} - -struct Joystick { - # convenient for debug and live tuning - axes @0: List(Float32); - buttons @1: List(Bool); -} - -struct DriverStateV2 { - frameId @0 :UInt32; - modelExecutionTime @1 :Float32; - gpuExecutionTime @8 :Float32; - rawPredictions @3 :Data; - - wheelOnRightProb @5 :Float32; - leftDriverData @6 :DriverData; - rightDriverData @7 :DriverData; - - struct DriverData { - faceOrientation @0 :List(Float32); - faceOrientationStd @1 :List(Float32); - facePosition @2 :List(Float32); - facePositionStd @3 :List(Float32); - faceProb @4 :Float32; - leftEyeProb @5 :Float32; - rightEyeProb @6 :Float32; - leftBlinkProb @7 :Float32; - rightBlinkProb @8 :Float32; - sunglassesProb @9 :Float32; - phoneProb @13 :Float32; - notReadyProbDEPRECATED @12 :List(Float32); - occludedProbDEPRECATED @10 :Float32; - readyProbDEPRECATED @11 :List(Float32); - } - - dspExecutionTimeDEPRECATED @2 :Float32; - poorVisionProbDEPRECATED @4 :Float32; -} - -struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 { - frameId @0 :UInt32; - modelExecutionTime @14 :Float32; - dspExecutionTime @16 :Float32; - rawPredictions @15 :Data; - - faceOrientation @3 :List(Float32); - facePosition @4 :List(Float32); - faceProb @5 :Float32; - leftEyeProb @6 :Float32; - rightEyeProb @7 :Float32; - leftBlinkProb @8 :Float32; - rightBlinkProb @9 :Float32; - faceOrientationStd @11 :List(Float32); - facePositionStd @12 :List(Float32); - sunglassesProb @13 :Float32; - poorVision @17 :Float32; - partialFace @18 :Float32; - distractedPose @19 :Float32; - distractedEyes @20 :Float32; - eyesOnRoad @21 :Float32; - phoneUse @22 :Float32; - occludedProb @23 :Float32; - - readyProb @24 :List(Float32); - notReadyProb @25 :List(Float32); - - irPwrDEPRECATED @10 :Float32; - descriptorDEPRECATED @1 :List(Float32); - stdDEPRECATED @2 :Float32; -} - -struct DriverMonitoringState @0xb83cda094a1da284 { - events @18 :List(OnroadEvent); - faceDetected @1 :Bool; - isDistracted @2 :Bool; - distractedType @17 :UInt32; - awarenessStatus @3 :Float32; - posePitchOffset @6 :Float32; - posePitchValidCount @7 :UInt32; - poseYawOffset @8 :Float32; - poseYawValidCount @9 :UInt32; - stepChange @10 :Float32; - awarenessActive @11 :Float32; - awarenessPassive @12 :Float32; - isLowStd @13 :Bool; - hiStdCount @14 :UInt32; - isActiveMode @16 :Bool; - isRHD @4 :Bool; - uncertainCount @19 :UInt32; - - phoneProbOffsetDEPRECATED @20 :Float32; - phoneProbValidCountDEPRECATED @21 :UInt32; - isPreviewDEPRECATED @15 :Bool; - rhdCheckedDEPRECATED @5 :Bool; - eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED); -} - -struct Boot { - wallTimeNanos @0 :UInt64; - pstore @4 :Map(Text, Data); - commands @5 :Map(Text, Data); - launchLog @3 :Text; - - lastKmsgDEPRECATED @1 :Data; - lastPmsgDEPRECATED @2 :Data; -} - -struct LiveParametersData { - valid @0 :Bool; - gyroBias @1 :Float32; - angleOffsetDeg @2 :Float32; - angleOffsetAverageDeg @3 :Float32; - stiffnessFactor @4 :Float32; - steerRatio @5 :Float32; - sensorValid @6 :Bool; - posenetSpeed @8 :Float32; - posenetValid @9 :Bool; - angleOffsetFastStd @10 :Float32; - angleOffsetAverageStd @11 :Float32; - stiffnessFactorStd @12 :Float32; - steerRatioStd @13 :Float32; - roll @14 :Float32; - debugFilterState @16 :FilterState; - - angleOffsetValid @17 :Bool = true; - angleOffsetAverageValid @18 :Bool = true; - steerRatioValid @19 :Bool = true; - stiffnessFactorValid @20 :Bool = true; - - yawRateDEPRECATED @7 :Float32; - filterStateDEPRECATED @15 :LiveLocationKalman.Measurement; - - struct FilterState { - value @0 : List(Float64); - std @1 : List(Float64); - } -} - -struct LiveTorqueParametersData { - liveValid @0 :Bool; - latAccelFactorRaw @1 :Float32; - latAccelOffsetRaw @2 :Float32; - frictionCoefficientRaw @3 :Float32; - latAccelFactorFiltered @4 :Float32; - latAccelOffsetFiltered @5 :Float32; - frictionCoefficientFiltered @6 :Float32; - totalBucketPoints @7 :Float32; - decay @8 :Float32; - maxResets @9 :Float32; - points @10 :List(List(Float32)); - version @11 :Int32; - useParams @12 :Bool; - calPerc @13 :Int8; -} - -struct LiveDelayData { - lateralDelay @0 :Float32; - validBlocks @1 :Int32; - status @2 :Status; - - lateralDelayEstimate @3 :Float32; - lateralDelayEstimateStd @5 :Float32; - points @4 :List(Float32); - calPerc @6 :Int8; - - enum Status { - unestimated @0; - estimated @1; - invalid @2; - } -} - -struct LiveMapDataDEPRECATED { - speedLimitValid @0 :Bool; - speedLimit @1 :Float32; - speedAdvisoryValid @12 :Bool; - speedAdvisory @13 :Float32; - speedLimitAheadValid @14 :Bool; - speedLimitAhead @15 :Float32; - speedLimitAheadDistance @16 :Float32; - curvatureValid @2 :Bool; - curvature @3 :Float32; - wayId @4 :UInt64; - roadX @5 :List(Float32); - roadY @6 :List(Float32); - lastGps @7: GpsLocationData; - roadCurvatureX @8 :List(Float32); - roadCurvature @9 :List(Float32); - distToTurn @10 :Float32; - mapValid @11 :Bool; -} - -struct CameraOdometry { - frameId @4 :UInt32; - timestampEof @5 :UInt64; - trans @0 :List(Float32); # m/s in device frame - rot @1 :List(Float32); # rad/s in device frame - transStd @2 :List(Float32); # std m/s in device frame - rotStd @3 :List(Float32); # std rad/s in device frame - wideFromDeviceEuler @6 :List(Float32); - wideFromDeviceEulerStd @7 :List(Float32); - roadTransformTrans @8 :List(Float32); - roadTransformTransStd @9 :List(Float32); -} - -struct Sentinel { - enum SentinelType { - endOfSegment @0; - endOfRoute @1; - startOfSegment @2; - startOfRoute @3; - } - type @0 :SentinelType; - signal @1 :Int32; -} - -struct UIDebug { - drawTimeMillis @0 :Float32; -} - -struct ManagerState { - processes @0 :List(ProcessState); - - struct ProcessState { - name @0 :Text; - pid @1 :Int32; - running @2 :Bool; - shouldBeRunning @4 :Bool; - exitCode @3 :Int32; - } -} - -struct UploaderState { - immediateQueueSize @0 :UInt32; - immediateQueueCount @1 :UInt32; - rawQueueSize @2 :UInt32; - rawQueueCount @3 :UInt32; - - # stats for last successfully uploaded file - lastTime @4 :Float32; # s - lastSpeed @5 :Float32; # MB/s - lastFilename @6 :Text; -} - -struct NavInstruction { - maneuverPrimaryText @0 :Text; - maneuverSecondaryText @1 :Text; - maneuverDistance @2 :Float32; # m - maneuverType @3 :Text; # TODO: Make Enum - maneuverModifier @4 :Text; # TODO: Make Enum - - distanceRemaining @5 :Float32; # m - timeRemaining @6 :Float32; # s - timeRemainingTypical @7 :Float32; # s - - lanes @8 :List(Lane); - showFull @9 :Bool; - - speedLimit @10 :Float32; # m/s - speedLimitSign @11 :SpeedLimitSign; - - allManeuvers @12 :List(Maneuver); - - struct Lane { - directions @0 :List(Direction); - active @1 :Bool; - activeDirection @2 :Direction; - } - - enum Direction { - none @0; - left @1; - right @2; - straight @3; - slightLeft @4; - slightRight @5; - } - - enum SpeedLimitSign { - mutcd @0; # US Style - vienna @1; # EU Style - } - - struct Maneuver { - distance @0 :Float32; - type @1 :Text; - modifier @2 :Text; - } -} - -struct NavRoute { - coordinates @0 :List(Coordinate); - - struct Coordinate { - latitude @0 :Float32; - longitude @1 :Float32; - } -} - -struct MapRenderState { - locationMonoTime @0 :UInt64; - renderTime @1 :Float32; - frameId @2: UInt32; -} - -struct NavModelData { - frameId @0 :UInt32; - locationMonoTime @6 :UInt64; - modelExecutionTime @1 :Float32; - dspExecutionTime @2 :Float32; - features @3 :List(Float32); - # predicted future position - position @4 :XYData; - desirePrediction @5 :List(Float32); - - # All SI units and in device frame - struct XYData { - x @0 :List(Float32); - y @1 :List(Float32); - xStd @2 :List(Float32); - yStd @3 :List(Float32); - } -} - -struct EncodeData { - idx @0 :EncodeIndex; - data @1 :Data; - header @2 :Data; - unixTimestampNanos @3 :UInt64; - width @4 :UInt32; - height @5 :UInt32; -} - -struct DebugAlert { - alertText1 @0 :Text; - alertText2 @1 :Text; -} - -struct UserBookmark @0xfe346a9de48d9b50 { -} - -struct SoundPressure @0xdc24138990726023 { - soundPressure @0 :Float32; - - # uncalibrated, A-weighted - soundPressureWeighted @3 :Float32; - soundPressureWeightedDb @1 :Float32; - - filteredSoundPressureWeightedDbDEPRECATED @2 :Float32; -} - -struct AudioData { - data @0 :Data; - sampleRate @1 :UInt32; -} - -struct AudioFeedback { - audio @0 :AudioData; - blockNum @1 :UInt16; -} - -struct Touch { - sec @0 :Int64; - usec @1 :Int64; - type @2 :UInt8; - code @3 :Int32; - value @4 :Int32; -} - -struct Event { - logMonoTime @0 :UInt64; # nanoseconds - valid @67 :Bool = true; - - union { - # *********** log metadata *********** - initData @1 :InitData; - sentinel @73 :Sentinel; - - # *********** bootlog *********** - boot @60 :Boot; - - # ********** openpilot daemon msgs ********** - gpsNMEA @3 :GPSNMEAData; - can @5 :List(CanData); - controlsState @7 :ControlsState; - selfdriveState @130 :SelfdriveState; - gyroscope @99 :SensorEventData; - accelerometer @98 :SensorEventData; - magnetometer @95 :SensorEventData; - lightSensor @96 :SensorEventData; - temperatureSensor @97 :SensorEventData; - pandaStates @81 :List(PandaState); - peripheralState @80 :PeripheralState; - radarState @13 :RadarState; - liveTracks @131 :Car.RadarData; - sendcan @17 :List(CanData); - liveCalibration @19 :LiveCalibrationData; - carState @22 :Car.CarState; - carControl @23 :Car.CarControl; - carOutput @127 :Car.CarOutput; - longitudinalPlan @24 :LongitudinalPlan; - driverAssistance @132 :DriverAssistance; - ubloxGnss @34 :UbloxGnss; - ubloxRaw @39 :Data; - qcomGnss @31 :QcomGnss; - gpsLocationExternal @48 :GpsLocationData; - gpsLocation @21 :GpsLocationData; - gnssMeasurements @91 :GnssMeasurements; - liveParameters @61 :LiveParametersData; - liveTorqueParameters @94 :LiveTorqueParametersData; - liveDelay @146 : LiveDelayData; - cameraOdometry @63 :CameraOdometry; - thumbnail @66: Thumbnail; - onroadEvents @134: List(OnroadEvent); - carParams @69: Car.CarParams; - driverMonitoringState @71: DriverMonitoringState; - livePose @129 :LivePose; - modelV2 @75 :ModelDataV2; - drivingModelData @128 :DrivingModelData; - driverStateV2 @92 :DriverStateV2; - - # camera stuff, each camera state has a matching encode idx - roadCameraState @2 :FrameData; - driverCameraState @70: FrameData; - wideRoadCameraState @74: FrameData; - roadEncodeIdx @15 :EncodeIndex; - driverEncodeIdx @76 :EncodeIndex; - wideRoadEncodeIdx @77 :EncodeIndex; - qRoadEncodeIdx @90 :EncodeIndex; - - livestreamRoadEncodeIdx @117 :EncodeIndex; - livestreamWideRoadEncodeIdx @118 :EncodeIndex; - livestreamDriverEncodeIdx @119 :EncodeIndex; - - # microphone data - soundPressure @103 :SoundPressure; - rawAudioData @147 :AudioData; - - # systems stuff - androidLog @20 :AndroidLogEntry; - managerState @78 :ManagerState; - uploaderState @79 :UploaderState; - procLog @33 :ProcLog; - clocks @35 :Clocks; - deviceState @6 :DeviceState; - logMessage @18 :Text; - errorLogMessage @85 :Text; - - # touch frame - touch @135 :List(Touch); - - # navigation - navInstruction @82 :NavInstruction; - navRoute @83 :NavRoute; - navThumbnail @84: Thumbnail; - mapRenderState @105: MapRenderState; - - # UI services - uiDebug @102 :UIDebug; - - # driving feedback - userBookmark @93 :UserBookmark; - bookmarkButton @148 :UserBookmark; - audioFeedback @149 :AudioFeedback; - - # *********** debug *********** - testJoystick @52 :Joystick; - roadEncodeData @86 :EncodeData; - driverEncodeData @87 :EncodeData; - wideRoadEncodeData @88 :EncodeData; - qRoadEncodeData @89 :EncodeData; - alertDebug @133 :DebugAlert; - - livestreamRoadEncodeData @120 :EncodeData; - livestreamWideRoadEncodeData @121 :EncodeData; - livestreamDriverEncodeData @122 :EncodeData; - - # *********** Custom: reserved for forks *********** - - # DO change the name of the field - # DON'T change anything after the "@" - customReservedRawData0 @124 :Data; - customReservedRawData1 @125 :Data; - customReservedRawData2 @126 :Data; - - # DO change the name of the field and struct - # DON'T change the ID (e.g. @107) - # DON'T change which struct it points to - customReserved0 @107 :Custom.CustomReserved0; - customReserved1 @108 :Custom.CustomReserved1; - customReserved2 @109 :Custom.CustomReserved2; - customReserved3 @110 :Custom.CustomReserved3; - customReserved4 @111 :Custom.CustomReserved4; - customReserved5 @112 :Custom.CustomReserved5; - customReserved6 @113 :Custom.CustomReserved6; - customReserved7 @114 :Custom.CustomReserved7; - customReserved8 @115 :Custom.CustomReserved8; - customReserved9 @116 :Custom.CustomReserved9; - customReserved10 @136 :Custom.CustomReserved10; - customReserved11 @137 :Custom.CustomReserved11; - customReserved12 @138 :Custom.CustomReserved12; - customReserved13 @139 :Custom.CustomReserved13; - customReserved14 @140 :Custom.CustomReserved14; - customReserved15 @141 :Custom.CustomReserved15; - customReserved16 @142 :Custom.CustomReserved16; - customReserved17 @143 :Custom.CustomReserved17; - customReserved18 @144 :Custom.CustomReserved18; - customReserved19 @145 :Custom.CustomReserved19; - - # *********** legacy + deprecated *********** - model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated - liveMpcDEPRECATED @36 :LiveMpcData; - liveLongitudinalMpcDEPRECATED @37 :LiveLongitudinalMpcData; - liveLocationKalmanLegacyDEPRECATED @51 :Legacy.LiveLocationData; - orbslamCorrectionDEPRECATED @45 :Legacy.OrbslamCorrection; - liveUIDEPRECATED @14 :Legacy.LiveUI; - sensorEventDEPRECATED @4 :SensorEventData; - liveEventDEPRECATED @8 :List(Legacy.LiveEventData); - liveLocationDEPRECATED @25 :Legacy.LiveLocationData; - ethernetDataDEPRECATED @26 :List(Legacy.EthernetPacket); - cellInfoDEPRECATED @28 :List(Legacy.CellInfo); - wifiScanDEPRECATED @29 :List(Legacy.WifiScan); - uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent; - liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED; - gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints; - gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan; - applanixRawDEPRECATED @42 :Data; - androidGnssDEPRECATED @30 :Legacy.AndroidGnss; - lidarPtsDEPRECATED @32 :Legacy.LidarPts; - navStatusDEPRECATED @38 :Legacy.NavStatus; - trafficEventsDEPRECATED @43 :List(Legacy.TrafficEvent); - liveLocationTimingDEPRECATED @44 :Legacy.LiveLocationData; - liveLocationCorrectedDEPRECATED @46 :Legacy.LiveLocationData; - navUpdateDEPRECATED @27 :Legacy.NavUpdate; - orbObservationDEPRECATED @47 :List(Legacy.OrbObservation); - locationDEPRECATED @49 :Legacy.LiveLocationData; - orbOdometryDEPRECATED @53 :Legacy.OrbOdometry; - orbFeaturesDEPRECATED @54 :Legacy.OrbFeatures; - applanixLocationDEPRECATED @55 :Legacy.LiveLocationData; - orbKeyFrameDEPRECATED @56 :Legacy.OrbKeyFrame; - orbFeaturesSummaryDEPRECATED @58 :Legacy.OrbFeaturesSummary; - featuresDEPRECATED @10 :Legacy.CalibrationFeatures; - kalmanOdometryDEPRECATED @65 :Legacy.KalmanOdometry; - uiLayoutStateDEPRECATED @57 :Legacy.UiLayoutState; - pandaStateDEPRECATED @12 :PandaState; - driverStateDEPRECATED @59 :DriverStateDEPRECATED; - sensorEventsDEPRECATED @11 :List(SensorEventData); - lateralPlanDEPRECATED @64 :LateralPlan; - navModelDEPRECATED @104 :NavModelData; - uiPlanDEPRECATED @106 :UiPlan; - liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; - liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); - onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED); - gyroscope2DEPRECATED @100 :SensorEventData; - accelerometer2DEPRECATED @101 :SensorEventData; - temperatureSensor2DEPRECATED @123 :SensorEventData; - } -} diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py deleted file mode 100644 index 2c925b4cc40b1d..00000000000000 --- a/cereal/messaging/__init__.py +++ /dev/null @@ -1,269 +0,0 @@ -# must be built with scons -from msgq import fake_event_handle, drain_sock_raw, MultiplePublishersError, IpcError, \ - Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \ - set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event -import msgq -import os -import capnp -import time - -from typing import Optional, List, Union, Dict - -from cereal import log -from cereal.services import SERVICE_LIST -from openpilot.common.utils import MovingAverage - -NO_TRAVERSAL_LIMIT = 2**64-1 - - -def pub_sock(endpoint: str) -> PubSocket: - service = SERVICE_LIST.get(endpoint) - segment_size = service.queue_size if service else 0 - return msgq.pub_sock(endpoint, segment_size) - - -def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1", - conflate: bool = False, timeout: Optional[int] = None) -> SubSocket: - service = SERVICE_LIST.get(endpoint) - segment_size = service.queue_size if service else 0 - return msgq.sub_sock(endpoint, poller=poller, addr=addr, conflate=conflate, - timeout=timeout, segment_size=segment_size) - - -def reset_context(): - msgq.context = Context() - - -def log_from_bytes(dat: bytes, struct: capnp.lib.capnp._StructModule = log.Event) -> capnp.lib.capnp._DynamicStructReader: - with struct.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg: - return msg - - -def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> capnp.lib.capnp._DynamicStructBuilder: - args = { - 'valid': False, - 'logMonoTime': int(time.monotonic() * 1e9), - **kwargs - } - dat = log.Event.new_message(**args) - if service is not None: - if size is None: - dat.init(service) - else: - dat.init(service, size) - return dat - - -def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]: - """Receive all message currently available on the queue""" - msgs = drain_sock_raw(sock, wait_for_one=wait_for_one) - return [log_from_bytes(m) for m in msgs] - - -# TODO: print when we drop packets? -def recv_sock(sock: SubSocket, wait: bool = False) -> Optional[capnp.lib.capnp._DynamicStructReader]: - """Same as drain sock, but only returns latest message. Consider using conflate instead.""" - dat = None - - while 1: - if wait and dat is None: - recv = sock.receive() - else: - recv = sock.receive(non_blocking=True) - - if recv is None: # Timeout hit - break - - dat = recv - - if dat is not None: - dat = log_from_bytes(dat) - - return dat - - -def recv_one(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]: - dat = sock.receive() - if dat is not None: - dat = log_from_bytes(dat) - return dat - - -def recv_one_or_none(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]: - dat = sock.receive(non_blocking=True) - if dat is not None: - dat = log_from_bytes(dat) - return dat - - -def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader: - """Keep receiving until we get a message""" - while True: - dat = sock.receive() - if dat is not None: - return log_from_bytes(dat) - - -class FrequencyTracker: - def __init__(self, service_freq: float, update_freq: float, is_poll: bool): - freq = max(min(service_freq, update_freq), 1.) - if is_poll: - min_freq = max_freq = freq - else: - max_freq = min(freq, update_freq) - if service_freq >= 2 * update_freq: - min_freq = update_freq - elif update_freq >= 2* service_freq: - min_freq = freq - else: - min_freq = min(freq, freq / 2.) - - self.min_freq = min_freq * 0.8 - self.max_freq = max_freq * 1.2 - self.avg_dt = MovingAverage(int(10 * freq)) - self.recent_avg_dt = MovingAverage(int(freq)) - self.prev_time = 0.0 - - def record_recv_time(self, cur_time: float) -> None: - # TODO: Handle case where cur_time is less than prev_time - if self.prev_time > 1e-5: - dt = cur_time - self.prev_time - - self.avg_dt.add_value(dt) - self.recent_avg_dt.add_value(dt) - - self.prev_time = cur_time - - @property - def valid(self) -> bool: - if self.avg_dt.count == 0: - return False - - avg_freq = 1.0 / self.avg_dt.get_average() - if self.min_freq <= avg_freq <= self.max_freq: - return True - - avg_freq_recent = 1.0 / self.recent_avg_dt.get_average() - return self.min_freq <= avg_freq_recent <= self.max_freq - - -class SubMaster: - def __init__(self, services: List[str], poll: Optional[str] = None, - ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None, - ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None): - self.frame = -1 - self.services = services - self.seen = {s: False for s in services} - self.updated = {s: False for s in services} - self.recv_time = {s: 0. for s in services} - self.recv_frame = {s: 0 for s in services} - self.sock = {} - self.data = {} - self.logMonoTime = {s: 0 for s in services} - - # zero-frequency / on-demand services are always alive and presumed valid; all others must pass checks - on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in services} - self.static_freq_services = set(s for s in services if not on_demand[s]) - self.alive = {s: on_demand[s] for s in services} - self.freq_ok = {s: on_demand[s] for s in services} - self.valid = {s: on_demand[s] for s in services} - - self.freq_tracker: Dict[str, FrequencyTracker] = {} - self.poller = Poller() - polled_services = set([poll, ] if poll is not None else services) - self.non_polled_services = set(services) - polled_services - - self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq - self.ignore_alive = [] if ignore_alive is None else ignore_alive - self.ignore_valid = [] if ignore_valid is None else ignore_valid - - self.simulation = bool(int(os.getenv("SIMULATION", "0"))) - - # if freq and poll aren't specified, assume the max to be conservative - assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used." - self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services]) - - for s in services: - p = self.poller if s not in self.non_polled_services else None - self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True) - - try: - data = new_message(s) - except capnp.lib.capnp.KjException: - data = new_message(s, 0) # lists - - self.data[s] = getattr(data.as_reader(), s) - self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll) - - def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader: - return self.data[s] - - def _check_avg_freq(self, s: str) -> bool: - return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive) - - def update(self, timeout: int = 100) -> None: - msgs = [] - for sock in self.poller.poll(timeout): - msgs.append(recv_one_or_none(sock)) - - # non-blocking receive for non-polled sockets - for s in self.non_polled_services: - msgs.append(recv_one_or_none(self.sock[s])) - self.update_msgs(time.monotonic(), msgs) - - def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None: - self.frame += 1 - self.updated = dict.fromkeys(self.services, False) - for msg in msgs: - if msg is None: - continue - - s = msg.which() - self.seen[s] = True - self.updated[s] = True - - self.freq_tracker[s].record_recv_time(cur_time) - self.recv_time[s] = cur_time - self.recv_frame[s] = self.frame - self.data[s] = getattr(msg, s) - self.logMonoTime[s] = msg.logMonoTime - self.valid[s] = msg.valid - - for s in self.static_freq_services: - # alive if delay is within 10x the expected frequency; checks relaxed in simulator - self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) or (self.seen[s] and self.simulation) - self.freq_ok[s] = self.freq_tracker[s].valid or self.simulation - - def all_alive(self, service_list: Optional[List[str]] = None) -> bool: - return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive) - - def all_freq_ok(self, service_list: Optional[List[str]] = None) -> bool: - return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s)) - - def all_valid(self, service_list: Optional[List[str]] = None) -> bool: - return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid) - - def all_checks(self, service_list: Optional[List[str]] = None) -> bool: - return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list) - - -class PubMaster: - def __init__(self, services: List[str]): - self.sock = {} - for s in services: - self.sock[s] = pub_sock(s) - - def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None: - if not isinstance(dat, bytes): - dat = dat.to_bytes() - self.sock[s].send(dat) - - def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool: - for _ in range(int(timeout*(1./dt))): - if self.sock[s].all_readers_updated(): - return True - time.sleep(dt) - return False - - def all_readers_updated(self, s: str) -> bool: - return self.sock[s].all_readers_updated() # type: ignore diff --git a/cereal/messaging/bridge.cc b/cereal/messaging/bridge.cc deleted file mode 100644 index fb92c575c94c57..00000000000000 --- a/cereal/messaging/bridge.cc +++ /dev/null @@ -1,72 +0,0 @@ -#include - -#include "cereal/messaging/msgq_to_zmq.h" -#include "cereal/services.h" -#include "common/util.h" - -ExitHandler do_exit; - -static std::vector get_services(const std::string &whitelist_str, bool zmq_to_msgq) { - std::vector service_list; - for (const auto& it : services) { - std::string name = it.second.name; - bool in_whitelist = whitelist_str.find(name) != std::string::npos; - if (zmq_to_msgq && !in_whitelist) { - continue; - } - service_list.push_back(name); - } - return service_list; -} - -void msgq_to_zmq(const std::vector &endpoints, const std::string &ip) { - MsgqToZmq bridge; - bridge.run(endpoints, ip); -} - -void zmq_to_msgq(const std::vector &endpoints, const std::string &ip) { - auto poller = std::make_unique(); - auto pub_context = std::make_unique(); - auto sub_context = std::make_unique(); - std::map sub2pub; - - for (auto endpoint : endpoints) { - auto pub_sock = new MSGQPubSocket(); - auto sub_sock = new ZMQSubSocket(); - size_t queue_size = services.at(endpoint).queue_size; - pub_sock->connect(pub_context.get(), endpoint, true, queue_size); - sub_sock->connect(sub_context.get(), endpoint, ip, false); - - poller->registerSocket(sub_sock); - sub2pub[sub_sock] = pub_sock; - } - - while (!do_exit) { - for (auto sub_sock : poller->poll(100)) { - std::unique_ptr msg(sub_sock->receive(true)); - if (msg) { - sub2pub[sub_sock]->sendMessage(msg.get()); - } - } - } - - // Clean up allocated sockets - for (auto &[sub_sock, pub_sock] : sub2pub) { - delete sub_sock; - delete pub_sock; - } -} - -int main(int argc, char **argv) { - bool is_zmq_to_msgq = argc > 2; - std::string ip = is_zmq_to_msgq ? argv[1] : "127.0.0.1"; - std::string whitelist_str = is_zmq_to_msgq ? std::string(argv[2]) : ""; - std::vector endpoints = get_services(whitelist_str, is_zmq_to_msgq); - - if (is_zmq_to_msgq) { - zmq_to_msgq(endpoints, ip); - } else { - msgq_to_zmq(endpoints, ip); - } - return 0; -} diff --git a/cereal/messaging/messaging.h b/cereal/messaging/messaging.h deleted file mode 100644 index fb9c261f2b57ea..00000000000000 --- a/cereal/messaging/messaging.h +++ /dev/null @@ -1,102 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "cereal/gen/cpp/log.capnp.h" -#include "common/timing.h" -#include "msgq/ipc.h" - -class SubMaster { -public: - SubMaster(const std::vector &service_list, const std::vector &poll = {}, - const char *address = nullptr, const std::vector &ignore_alive = {}); - void update(int timeout = 1000); - void update_msgs(uint64_t current_time, const std::vector> &messages); - inline bool allAlive(const std::vector &service_list = {}) { return all_(service_list, false, true); } - inline bool allValid(const std::vector &service_list = {}) { return all_(service_list, true, false); } - inline bool allAliveAndValid(const std::vector &service_list = {}) { return all_(service_list, true, true); } - void drain(); - ~SubMaster(); - - uint64_t frame = 0; - bool updated(const char *name) const; - bool alive(const char *name) const; - bool valid(const char *name) const; - uint64_t rcv_frame(const char *name) const; - uint64_t rcv_time(const char *name) const; - cereal::Event::Reader &operator[](const char *name) const; - -private: - bool all_(const std::vector &service_list, bool valid, bool alive); - Poller *poller_ = nullptr; - struct SubMessage; - std::map messages_; - std::map services_; -}; - -class MessageBuilder : public capnp::MallocMessageBuilder { -public: - MessageBuilder() = default; - - cereal::Event::Builder initEvent(bool valid = true) { - cereal::Event::Builder event = initRoot(); - event.setLogMonoTime(nanos_since_boot()); - event.setValid(valid); - return event; - } - - kj::ArrayPtr toBytes() { - heapArray_ = capnp::messageToFlatArray(*this); - return heapArray_.asBytes(); - } - - size_t getSerializedSize() { - return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word); - } - - int serializeToBuffer(unsigned char *buffer, size_t buffer_size) { - size_t serialized_size = getSerializedSize(); - if (serialized_size > buffer_size) { return -1; } - kj::ArrayOutputStream out(kj::ArrayPtr(buffer, buffer_size)); - capnp::writeMessage(out, *this); - return serialized_size; - } - -private: - kj::Array heapArray_; -}; - -class PubMaster { -public: - PubMaster(const std::vector &service_list); - inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); } - int send(const char *name, MessageBuilder &msg); - ~PubMaster(); - -private: - std::map sockets_; -}; - -class AlignedBuffer { -public: - kj::ArrayPtr align(const char *data, const size_t size) { - words_size = size / sizeof(capnp::word) + 1; - if (aligned_buf.size() < words_size) { - aligned_buf = kj::heapArray(words_size < 512 ? 512 : words_size); - } - memcpy(aligned_buf.begin(), data, size); - return aligned_buf.slice(0, words_size); - } - inline kj::ArrayPtr align(Message *m) { - return align(m->getData(), m->getSize()); - } -private: - kj::Array aligned_buf; - size_t words_size; -}; diff --git a/cereal/messaging/msgq_to_zmq.cc b/cereal/messaging/msgq_to_zmq.cc deleted file mode 100644 index 7f8c738d4d2432..00000000000000 --- a/cereal/messaging/msgq_to_zmq.cc +++ /dev/null @@ -1,146 +0,0 @@ -#include "cereal/messaging/msgq_to_zmq.h" - -#include - -#include "cereal/services.h" -#include "common/util.h" - -extern ExitHandler do_exit; - -// Max messages to process per socket per poll -constexpr int MAX_MESSAGES_PER_SOCKET = 50; - -static std::string recv_zmq_msg(void *sock) { - zmq_msg_t msg; - zmq_msg_init(&msg); - std::string ret; - if (zmq_msg_recv(&msg, sock, 0) > 0) { - ret.assign((char *)zmq_msg_data(&msg), zmq_msg_size(&msg)); - } - zmq_msg_close(&msg); - return ret; -} - -void MsgqToZmq::run(const std::vector &endpoints, const std::string &ip) { - zmq_context = std::make_unique(); - msgq_context = std::make_unique(); - - // Create ZMQPubSockets for each endpoint - for (const auto &endpoint : endpoints) { - auto &socket_pair = socket_pairs.emplace_back(); - socket_pair.endpoint = endpoint; - socket_pair.pub_sock = std::make_unique(); - int ret = socket_pair.pub_sock->connect(zmq_context.get(), endpoint); - if (ret != 0) { - printf("Failed to create ZMQ publisher for [%s]: %s\n", endpoint.c_str(), zmq_strerror(zmq_errno())); - return; - } - } - - // Start ZMQ monitoring thread to monitor socket events - std::thread thread(&MsgqToZmq::zmqMonitorThread, this); - - // Main loop for processing messages - while (!do_exit) { - { - std::unique_lock lk(mutex); - cv.wait(lk, [this]() { return do_exit || !sub2pub.empty(); }); - if (do_exit) break; - - for (auto sub_sock : msgq_poller->poll(100)) { - // Process messages for each socket - ZMQPubSocket *pub_sock = sub2pub.at(sub_sock); - for (int i = 0; i < MAX_MESSAGES_PER_SOCKET; ++i) { - auto msg = std::unique_ptr(sub_sock->receive(true)); - if (!msg) break; - - while (pub_sock->sendMessage(msg.get()) == -1) { - if (errno != EINTR) break; - } - } - } - } - util::sleep_for(1); // Give zmqMonitorThread a chance to acquire the mutex - } - - thread.join(); -} - -void MsgqToZmq::zmqMonitorThread() { - std::vector pollitems; - - // Set up ZMQ monitor for each pub socket - for (int i = 0; i < socket_pairs.size(); ++i) { - std::string addr = "inproc://op-bridge-monitor-" + std::to_string(i); - zmq_socket_monitor(socket_pairs[i].pub_sock->sock, addr.c_str(), ZMQ_EVENT_ACCEPTED | ZMQ_EVENT_DISCONNECTED); - - void *monitor_socket = zmq_socket(zmq_context->getRawContext(), ZMQ_PAIR); - zmq_connect(monitor_socket, addr.c_str()); - pollitems.emplace_back(zmq_pollitem_t{.socket = monitor_socket, .events = ZMQ_POLLIN}); - } - - while (!do_exit) { - int ret = zmq_poll(pollitems.data(), pollitems.size(), 1000); - if (ret < 0) { - if (errno == EINTR) { - // Due to frequent EINTR signals from msgq, introduce a brief delay (200 ms) - // to reduce CPU usage during retry attempts. - util::sleep_for(200); - } - continue; - } - - for (int i = 0; i < pollitems.size(); ++i) { - if (pollitems[i].revents & ZMQ_POLLIN) { - // First frame in message contains event number and value - std::string frame = recv_zmq_msg(pollitems[i].socket); - if (frame.empty()) continue; - - uint16_t event_type = *(uint16_t *)(frame.data()); - - // Second frame in message contains event address - frame = recv_zmq_msg(pollitems[i].socket); - if (frame.empty()) continue; - - std::unique_lock lk(mutex); - auto &pair = socket_pairs[i]; - if (event_type & ZMQ_EVENT_ACCEPTED) { - printf("socket [%s] connected\n", pair.endpoint.c_str()); - if (++pair.connected_clients == 1) { - // Create new MSGQ subscriber socket and map to ZMQ publisher - pair.sub_sock = std::make_unique(); - size_t queue_size = services.at(pair.endpoint).queue_size; - pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1", false, true, queue_size); - sub2pub[pair.sub_sock.get()] = pair.pub_sock.get(); - registerSockets(); - } - } else if (event_type & ZMQ_EVENT_DISCONNECTED) { - printf("socket [%s] disconnected\n", pair.endpoint.c_str()); - if (pair.connected_clients == 0 || --pair.connected_clients == 0) { - // Remove MSGQ subscriber socket from mapping and reset it - sub2pub.erase(pair.sub_sock.get()); - pair.sub_sock.reset(nullptr); - registerSockets(); - } - } - cv.notify_one(); - } - } - } - - // Clean up monitor sockets - for (int i = 0; i < pollitems.size(); ++i) { - zmq_socket_monitor(socket_pairs[i].pub_sock->sock, nullptr, 0); - zmq_close(pollitems[i].socket); - } - cv.notify_one(); -} - -void MsgqToZmq::registerSockets() { - msgq_poller = std::make_unique(); - for (const auto &socket_pair : socket_pairs) { - if (socket_pair.sub_sock) { - msgq_poller->registerSocket(socket_pair.sub_sock.get()); - } - } -} diff --git a/cereal/messaging/msgq_to_zmq.h b/cereal/messaging/msgq_to_zmq.h deleted file mode 100644 index ebdbe5df690e98..00000000000000 --- a/cereal/messaging/msgq_to_zmq.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#define private public -#include "msgq/impl_msgq.h" -#include "msgq/impl_zmq.h" - -class MsgqToZmq { -public: - MsgqToZmq() {} - void run(const std::vector &endpoints, const std::string &ip); - -protected: - void registerSockets(); - void zmqMonitorThread(); - - struct SocketPair { - std::string endpoint; - std::unique_ptr pub_sock; - std::unique_ptr sub_sock; - int connected_clients = 0; - }; - - std::unique_ptr msgq_context; - std::unique_ptr zmq_context; - std::mutex mutex; - std::condition_variable cv; - std::unique_ptr msgq_poller; - std::map sub2pub; - std::vector socket_pairs; -}; diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc deleted file mode 100644 index dfeeb807ee596f..00000000000000 --- a/cereal/messaging/socketmaster.cc +++ /dev/null @@ -1,204 +0,0 @@ -#include -#include -#include -#include - -#include "cereal/services.h" -#include "cereal/messaging/messaging.h" - -const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1"); - -static inline bool inList(const std::vector &list, const char *value) { - for (auto &v : list) { - if (strcmp(value, v) == 0) return true; - } - return false; -} - -class MessageContext { -public: - MessageContext() : ctx_(nullptr) {} - ~MessageContext() { delete ctx_; } - inline Context *context() { - std::call_once(init_flag, [=]() { ctx_ = Context::create(); }); - return ctx_; - } -private: - Context *ctx_; - std::once_flag init_flag; -}; - -MessageContext message_context; - -struct SubMaster::SubMessage { - std::string name; - SubSocket *socket = nullptr; - 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; - bool is_polled = false; - capnp::FlatArrayMessageReader *msg_reader = nullptr; - AlignedBuffer aligned_buf; - cereal::Event::Reader event; -}; - -SubMaster::SubMaster(const std::vector &service_list, const std::vector &poll, - const char *address, const std::vector &ignore_alive) { - poller_ = Poller::create(); - for (auto name : service_list) { - assert(services.count(std::string(name)) > 0); - - service serv = services.at(std::string(name)); - SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true, true, serv.queue_size); - assert(socket != 0); - bool is_polled = inList(poll, name) || poll.empty(); - if (is_polled) poller_->registerSocket(socket); - SubMessage *m = new SubMessage{ - .name = name, - .socket = socket, - .freq = serv.frequency, - .ignore_alive = inList(ignore_alive, name), - .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)), - .is_polled = is_polled}; - m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader({}); - messages_[socket] = m; - services_[name] = m; - } -} - -void SubMaster::update(int timeout) { - for (auto &kv : messages_) kv.second->updated = false; - - auto sockets = poller_->poll(timeout); - - // add non-polled sockets for non-blocking receive - for (auto &kv : messages_) { - SubMessage *m = kv.second; - SubSocket *s = kv.first; - if (!m->is_polled) sockets.push_back(s); - } - - uint64_t current_time = nanos_since_boot(); - - std::vector> messages; - - for (auto s : sockets) { - Message *msg = s->receive(true); - if (msg == nullptr) continue; - - SubMessage *m = messages_.at(s); - - m->msg_reader->~FlatArrayMessageReader(); - capnp::ReaderOptions options; - options.traversalLimitInWords = kj::maxValue; // Don't limit - m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg), options); - delete msg; - messages.push_back({m->name, m->msg_reader->getRoot()}); - } - - update_msgs(current_time, messages); -} - -void SubMaster::update_msgs(uint64_t current_time, const std::vector> &messages){ - if (++frame == UINT64_MAX) frame = 1; - - for (auto &kv : messages) { - auto m_find = services_.find(kv.first); - if (m_find == services_.end()){ - continue; - } - SubMessage *m = m_find->second; - m->event = kv.second; - m->updated = true; - m->rcv_time = current_time; - m->rcv_frame = frame; - m->valid = m->event.getValid(); - if (SIMULATION) m->alive = true; - } - - if (!SIMULATION) { - for (auto &kv : messages_) { - SubMessage *m = kv.second; - m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); - } - } -} - -bool SubMaster::all_(const std::vector &service_list, bool valid, bool alive) { - int found = 0; - for (auto &kv : messages_) { - SubMessage *m = kv.second; - if (service_list.size() == 0 || inList(service_list, m->name.c_str())) { - found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive)); - } - } - return service_list.size() == 0 ? found == messages_.size() : found == service_list.size(); -} - -void SubMaster::drain() { - while (true) { - auto polls = poller_->poll(0); - if (polls.size() == 0) - break; - - for (auto sock : polls) { - Message *msg = sock->receive(true); - delete msg; - } - } -} - -bool SubMaster::updated(const char *name) const { - return services_.at(name)->updated; -} - -bool SubMaster::alive(const char *name) const { - return services_.at(name)->alive; -} - -bool SubMaster::valid(const char *name) const { - return services_.at(name)->valid; -} - -uint64_t SubMaster::rcv_frame(const char *name) const { - return services_.at(name)->rcv_frame; -} - -uint64_t SubMaster::rcv_time(const char *name) const { - return services_.at(name)->rcv_time; -} - -cereal::Event::Reader &SubMaster::operator[](const char *name) const { - return services_.at(name)->event; -} - -SubMaster::~SubMaster() { - delete poller_; - for (auto &kv : messages_) { - SubMessage *m = kv.second; - m->msg_reader->~FlatArrayMessageReader(); - free(m->allocated_msg_reader); - delete m->socket; - delete m; - } -} - -PubMaster::PubMaster(const std::vector &service_list) { - for (auto name : service_list) { - assert(services.count(name) > 0); - service serv = services.at(std::string(name)); - PubSocket *socket = PubSocket::create(message_context.context(), name, true, serv.queue_size); - assert(socket); - sockets_[name] = socket; - } -} - -int PubMaster::send(const char *name, MessageBuilder &msg) { - auto bytes = msg.toBytes(); - return send(name, bytes.begin(), bytes.size()); -} - -PubMaster::~PubMaster() { - for (auto s : sockets_) delete s.second; -} diff --git a/cereal/messaging/tests/test_messaging.py b/cereal/messaging/tests/test_messaging.py deleted file mode 100644 index 583eb8b0d86115..00000000000000 --- a/cereal/messaging/tests/test_messaging.py +++ /dev/null @@ -1,185 +0,0 @@ -import os -import capnp -import multiprocessing -import numbers -import random -import threading -import time -from parameterized import parameterized -import pytest - -from cereal import log, car -import cereal.messaging as messaging -from cereal.services import SERVICE_LIST - -events = [evt for evt in log.Event.schema.union_fields if evt in SERVICE_LIST.keys()] - -def random_sock(): - return random.choice(events) - -def random_socks(num_socks=10): - return list({random_sock() for _ in range(num_socks)}) - -def random_bytes(length=1000): - return bytes([random.randrange(0xFF) for _ in range(length)]) - -def zmq_sleep(t=1): - if "ZMQ" in os.environ: - time.sleep(t) - - -# TODO: this should take any capnp struct and returrn a msg with random populated data -def random_carstate(): - fields = ["vEgo", "aEgo", "brake", "steeringAngleDeg"] - msg = messaging.new_message("carState") - cs = msg.carState - for f in fields: - setattr(cs, f, random.random() * 10) - return msg - -# TODO: this should compare any capnp structs -def assert_carstate(cs1, cs2): - for f in car.CarState.schema.non_union_fields: - # TODO: check all types - val1, val2 = getattr(cs1, f), getattr(cs2, f) - if isinstance(val1, numbers.Number): - assert val1 == val2, f"{f}: sent '{val1}' vs recvd '{val2}'" - -def delayed_send(delay, sock, dat): - def send_func(): - sock.send(dat) - threading.Timer(delay, send_func).start() - - -class TestMessaging: - def setUp(self): - # TODO: ZMQ tests are too slow; all sleeps will need to be - # replaced with logic to block on the necessary condition - if "ZMQ" in os.environ: - pytest.skip() - - # ZMQ pub socket takes too long to die - # sleep to prevent multiple publishers error between tests - zmq_sleep() - - @parameterized.expand(events) - def test_new_message(self, evt): - try: - msg = messaging.new_message(evt) - except capnp.lib.capnp.KjException: - msg = messaging.new_message(evt, random.randrange(200)) - assert (time.monotonic() - msg.logMonoTime) < 0.1 - assert not msg.valid - assert evt == msg.which() - - @parameterized.expand(events) - def test_pub_sock(self, evt): - messaging.pub_sock(evt) - - @parameterized.expand(events) - def test_sub_sock(self, evt): - messaging.sub_sock(evt) - - @parameterized.expand([ - (messaging.drain_sock, capnp._DynamicStructReader), - (messaging.drain_sock_raw, bytes), - ]) - def test_drain_sock(self, func, expected_type): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sub_sock = messaging.sub_sock(sock, timeout=1000) - zmq_sleep() - - # no wait and no msgs in queue - msgs = func(sub_sock) - assert isinstance(msgs, list) - assert len(msgs) == 0 - - # no wait but msgs are queued up - num_msgs = random.randrange(3, 10) - for _ in range(num_msgs): - pub_sock.send(messaging.new_message(sock).to_bytes()) - time.sleep(0.1) - msgs = func(sub_sock) - assert isinstance(msgs, list) - assert all(isinstance(msg, expected_type) for msg in msgs) - assert len(msgs) == num_msgs - - def test_recv_sock(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sub_sock = messaging.sub_sock(sock, timeout=100) - zmq_sleep() - - # no wait and no msg in queue, socket should timeout - recvd = messaging.recv_sock(sub_sock) - assert recvd is None - - # no wait and one msg in queue - msg = random_carstate() - pub_sock.send(msg.to_bytes()) - time.sleep(0.01) - recvd = messaging.recv_sock(sub_sock) - assert isinstance(recvd, capnp._DynamicStructReader) - # https://github.com/python/mypy/issues/13038 - assert_carstate(msg.carState, recvd.carState) - - def test_recv_one(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sub_sock = messaging.sub_sock(sock, timeout=1000) - zmq_sleep() - - # no msg in queue, socket should timeout - recvd = messaging.recv_one(sub_sock) - assert recvd is None - - # one msg in queue - msg = random_carstate() - pub_sock.send(msg.to_bytes()) - recvd = messaging.recv_one(sub_sock) - assert isinstance(recvd, capnp._DynamicStructReader) - assert_carstate(msg.carState, recvd.carState) - - @pytest.mark.xfail(condition="ZMQ" in os.environ, reason='ZMQ detected') - def test_recv_one_or_none(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sub_sock = messaging.sub_sock(sock) - zmq_sleep() - - # no msg in queue, socket shouldn't block - recvd = messaging.recv_one_or_none(sub_sock) - assert recvd is None - - # one msg in queue - msg = random_carstate() - pub_sock.send(msg.to_bytes()) - recvd = messaging.recv_one_or_none(sub_sock) - assert isinstance(recvd, capnp._DynamicStructReader) - assert_carstate(msg.carState, recvd.carState) - - def test_recv_one_retry(self): - sock = "carState" - sock_timeout = 0.1 - pub_sock = messaging.pub_sock(sock) - sub_sock = messaging.sub_sock(sock, timeout=round(sock_timeout*1000)) - zmq_sleep() - - # this test doesn't work with ZMQ since multiprocessing interrupts it - if "ZMQ" not in os.environ: - # wait 5 socket timeouts and make sure it's still retrying - p = multiprocessing.Process(target=messaging.recv_one_retry, args=(sub_sock,)) - p.start() - time.sleep(sock_timeout*5) - assert p.is_alive() - p.terminate() - - # wait 5 socket timeouts before sending - msg = random_carstate() - start_time = time.monotonic() - delayed_send(sock_timeout*5, pub_sock, msg.to_bytes()) - recvd = messaging.recv_one_retry(sub_sock) - assert (time.monotonic() - start_time) >= sock_timeout*5 - assert isinstance(recvd, capnp._DynamicStructReader) - assert_carstate(msg.carState, recvd.carState) diff --git a/cereal/messaging/tests/test_pub_sub_master.py b/cereal/messaging/tests/test_pub_sub_master.py deleted file mode 100644 index 5e26b49701e87a..00000000000000 --- a/cereal/messaging/tests/test_pub_sub_master.py +++ /dev/null @@ -1,160 +0,0 @@ -import random -import time -from typing import Sized, cast - -import cereal.messaging as messaging -from cereal.messaging.tests.test_messaging import events, random_sock, random_socks, \ - random_bytes, random_carstate, assert_carstate, \ - zmq_sleep -from cereal.services import SERVICE_LIST - - -class TestSubMaster: - - def setup_method(self): - # ZMQ pub socket takes too long to die - # sleep to prevent multiple publishers error between tests - zmq_sleep(3) - - def test_init(self): - sm = messaging.SubMaster(events) - for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive, - sm.sock, sm.data, sm.logMonoTime, sm.valid]: - assert len(cast(Sized, p)) == len(events) - - def test_init_state(self): - socks = random_socks() - sm = messaging.SubMaster(socks) - assert sm.frame == -1 - assert not any(sm.updated.values()) - assert not any(sm.seen.values()) - on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in sm.services} - assert all(sm.alive[s] == sm.valid[s] == sm.freq_ok[s] == on_demand[s] for s in sm.services) - assert all(t == 0. for t in sm.recv_time.values()) - assert all(f == 0 for f in sm.recv_frame.values()) - assert all(t == 0 for t in sm.logMonoTime.values()) - - for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive, - sm.sock, sm.data, sm.logMonoTime, sm.valid]: - assert len(cast(Sized, p)) == len(socks) - - def test_getitem(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sm = messaging.SubMaster([sock,]) - zmq_sleep() - - msg = random_carstate() - pub_sock.send(msg.to_bytes()) - sm.update(1000) - assert_carstate(msg.carState, sm[sock]) - - # TODO: break this test up to individually test SubMaster.update and SubMaster.update_msgs - def test_update(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sm = messaging.SubMaster([sock,]) - zmq_sleep() - - for i in range(10): - msg = messaging.new_message(sock) - pub_sock.send(msg.to_bytes()) - sm.update(1000) - assert sm.frame == i - assert all(sm.updated.values()) - - def test_update_timeout(self): - sock = random_sock() - sm = messaging.SubMaster([sock,]) - timeout = random.randrange(1000, 3000) - start_time = time.monotonic() - sm.update(timeout) - t = time.monotonic() - start_time - assert t >= timeout/1000. - assert t < 3 - assert not any(sm.updated.values()) - - def test_avg_frequency_checks(self): - for poll in (True, False): - sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"], - poll=("modelV2" if poll else None), - frequency=(20. if not poll else None)) - - checks = { - "carState": (20, 20), - "modelV2": (20, 20 if poll else 10), - "cameraOdometry": (20, 10), - "liveCalibration": (4, 4), - "carParams": (None, None), - "userBookmark": (None, None), - } - - for service, (max_freq, min_freq) in checks.items(): - if max_freq is not None: - assert sm._check_avg_freq(service) - assert sm.freq_tracker[service].max_freq == max_freq*1.2 - assert sm.freq_tracker[service].min_freq == min_freq*0.8 - else: - assert not sm._check_avg_freq(service) - - def test_alive(self): - pass - - def test_ignore_alive(self): - pass - - def test_valid(self): - pass - - # SubMaster should always conflate - def test_conflate(self): - sock = "carState" - pub_sock = messaging.pub_sock(sock) - sm = messaging.SubMaster([sock,]) - - n = 10 - for i in range(n+1): - msg = messaging.new_message(sock) - msg.carState.vEgo = i - pub_sock.send(msg.to_bytes()) - time.sleep(0.01) - sm.update(1000) - assert sm[sock].vEgo == n - - -class TestPubMaster: - - def setup_method(self): - # ZMQ pub socket takes too long to die - # sleep to prevent multiple publishers error between tests - zmq_sleep(3) - - def test_init(self): - messaging.PubMaster(events) - - def test_send(self): - socks = random_socks() - pm = messaging.PubMaster(socks) - sub_socks = {s: messaging.sub_sock(s, conflate=True, timeout=1000) for s in socks} - zmq_sleep() - - # PubMaster accepts either a capnp msg builder or bytes - for capnp in [True, False]: - for i in range(100): - sock = socks[i % len(socks)] - - if capnp: - try: - msg = messaging.new_message(sock) - except Exception: - msg = messaging.new_message(sock, random.randrange(50)) - else: - msg = random_bytes() - - pm.send(sock, msg) - recvd = sub_socks[sock].receive() - - if capnp: - msg.clear_write_flag() - msg = msg.to_bytes() - assert msg == recvd, i diff --git a/cereal/messaging/tests/test_services.py b/cereal/messaging/tests/test_services.py deleted file mode 100644 index 8bfd2ea978ae03..00000000000000 --- a/cereal/messaging/tests/test_services.py +++ /dev/null @@ -1,21 +0,0 @@ -import os -import tempfile -from typing import Dict -from parameterized import parameterized - -import cereal.services as services -from cereal.services import SERVICE_LIST - - -class TestServices: - - @parameterized.expand(SERVICE_LIST.keys()) - def test_services(self, s): - service = SERVICE_LIST[s] - assert service.frequency <= 104 - assert service.decimation != 0 - - def test_generated_header(self): - with tempfile.NamedTemporaryFile(suffix=".h") as f: - ret = os.system(f"python3 {services.__file__} > {f.name} && clang++ {f.name} -std=c++11") - assert ret == 0, "generated services header is not valid C" diff --git a/cereal/services.py b/cereal/services.py deleted file mode 100755 index e7350aceac06fa..00000000000000 --- a/cereal/services.py +++ /dev/null @@ -1,133 +0,0 @@ -#!/usr/bin/env python3 -from enum import IntEnum -from typing import Optional - - -# TODO: this should be automatically determined using the capnp schema -class QueueSize(IntEnum): - BIG = 10 * 1024 * 1024 # 10MB - video frames, large AI outputs - MEDIUM = 2 * 1024 * 1024 # 2MB - high freq (CAN), livestream - SMALL = 250 * 1024 # 250KB - most services - - -class Service: - def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] = None, - queue_size: QueueSize = QueueSize.SMALL): - self.should_log = should_log - self.frequency = frequency - self.decimation = decimation - self.queue_size = queue_size - - -_services: dict[str, tuple] = { - # service: (should_log, frequency, qlog decimation (optional)) - # note: the "EncodeIdx" packets will still be in the log - "gyroscope": (True, 104., 104), - "accelerometer": (True, 104., 104), - "magnetometer": (True, 25.), - "lightSensor": (True, 100., 100), - "temperatureSensor": (True, 2., 200), - "gpsNMEA": (True, 9.), - "deviceState": (True, 2., 1), - "touch": (True, 20., 1), - "can": (True, 100., 2053, QueueSize.BIG), # decimation gives ~3 msgs in a full segment - "controlsState": (True, 100., 10, QueueSize.MEDIUM), - "selfdriveState": (True, 100., 10), - "pandaStates": (True, 10., 1), - "peripheralState": (True, 2., 1), - "radarState": (True, 20., 5), - "roadEncodeIdx": (False, 20., 1), - "liveTracks": (True, 20.), - "sendcan": (True, 100., 139, QueueSize.MEDIUM), - "logMessage": (True, 0.), - "errorLogMessage": (True, 0., 1), - "liveCalibration": (True, 4., 4), - "liveTorqueParameters": (True, 4., 1), - "liveDelay": (True, 4., 1), - "androidLog": (True, 0.), - "carState": (True, 100., 10), - "carControl": (True, 100., 10), - "carOutput": (True, 100., 10), - "longitudinalPlan": (True, 20., 10), - "driverAssistance": (True, 20., 20), - "procLog": (True, 0.5, 15, QueueSize.BIG), - "gpsLocationExternal": (True, 10., 10), - "gpsLocation": (True, 1., 1), - "ubloxGnss": (True, 10.), - "qcomGnss": (True, 2.), - "gnssMeasurements": (True, 10., 10), - "clocks": (True, 0.1, 1), - "ubloxRaw": (True, 20.), - "livePose": (True, 20., 4), - "liveParameters": (True, 20., 5), - "cameraOdometry": (True, 20., 10), - "thumbnail": (True, 1 / 60., 1), - "onroadEvents": (True, 1., 1), - "carParams": (True, 0.02, 1), - "roadCameraState": (True, 20., 20), - "driverCameraState": (True, 20., 20), - "driverEncodeIdx": (False, 20., 1), - "driverStateV2": (True, 20., 10), - "driverMonitoringState": (True, 20., 10), - "wideRoadEncodeIdx": (False, 20., 1), - "wideRoadCameraState": (True, 20., 20), - "drivingModelData": (True, 20., 10), - "modelV2": (True, 20., None, QueueSize.BIG), - "managerState": (True, 2., 1), - "uploaderState": (True, 0., 1), - "navInstruction": (True, 1., 10), - "navRoute": (True, 0.), - "navThumbnail": (True, 0.), - "qRoadEncodeIdx": (False, 20.), - "userBookmark": (True, 0., 1), - "soundPressure": (True, 10., 10), - "rawAudioData": (False, 20.), - "bookmarkButton": (True, 0., 1), - "audioFeedback": (True, 0., 1), - "roadEncodeData": (False, 20., None, QueueSize.BIG), - "driverEncodeData": (False, 20., None, QueueSize.BIG), - "wideRoadEncodeData": (False, 20., None, QueueSize.BIG), - "qRoadEncodeData": (False, 20., None, QueueSize.BIG), - - # debug - "uiDebug": (True, 0., 1), - "testJoystick": (True, 0.), - "alertDebug": (True, 20., 5), - "livestreamWideRoadEncodeIdx": (False, 20.), - "livestreamRoadEncodeIdx": (False, 20.), - "livestreamDriverEncodeIdx": (False, 20.), - "livestreamWideRoadEncodeData": (False, 20., None, QueueSize.MEDIUM), - "livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM), - "livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM), - "customReservedRawData0": (True, 0.), - "customReservedRawData1": (True, 0.), - "customReservedRawData2": (True, 0.), -} -SERVICE_LIST = {name: Service(*vals) for - idx, (name, vals) in enumerate(_services.items())} - - -def build_header(): - h = "" - h += "/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT services.py */\n" - h += "#ifndef __SERVICES_H\n" - h += "#define __SERVICES_H\n" - - h += "#include \n" - h += "#include \n" - - h += "struct service { std::string name; bool should_log; float frequency; int decimation; size_t queue_size; };\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, %f, %d, %d}},\n' % \ - (k, k, should_log, v.frequency, decimation, v.queue_size) - h += "};\n" - - h += "#endif\n" - return h - - -if __name__ == "__main__": - print(build_header()) diff --git a/codecov.yml b/codecov.yml new file mode 100644 index 00000000000000..83427c3ee8b6e5 --- /dev/null +++ b/codecov.yml @@ -0,0 +1,8 @@ +comment: false +coverage: + status: + project: + default: + informational: true + patch: off + diff --git a/common/SConscript b/common/SConscript index 1c68cf05c7aecd..8aee6f42a7f3f4 100644 --- a/common/SConscript +++ b/common/SConscript @@ -1,24 +1,33 @@ -Import('env', 'envCython', 'arch') +Import('env', 'envCython', 'arch', 'SHARED') + +if SHARED: + fxn = env.SharedLibrary +else: + fxn = env.Library common_libs = [ 'params.cc', + 'statlog.cc', 'swaglog.cc', 'util.cc', - 'ratekeeper.cc', - 'clutil.cc', + 'gpio.cc', + 'i2c.cc', + 'watchdog.cc', ] -_common = env.Library('common', common_libs, LIBS="json11") -Export('_common') +_common = fxn('common', common_libs, LIBS="json11") -if GetOption('extras'): - env.Program('tests/test_common', - ['tests/test_runner.cc', 'tests/test_params.cc', 'tests/test_util.cc', 'tests/test_swaglog.cc'], - LIBS=[_common, 'json11', 'zmq', 'pthread']) +files = [ + 'clutil.cc', +] -# Cython bindings -params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11']) +_gpucommon = fxn('gpucommon', files) +Export('_common', '_gpucommon') -common_python = [params_python] +if GetOption('test'): + env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common]) + env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread']) -Export('common_python') +# Cython +envCython.Program('clock.so', 'clock.pyx') +envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11']) diff --git a/common/api.py b/common/api.py deleted file mode 100644 index ebf0290d154de7..00000000000000 --- a/common/api.py +++ /dev/null @@ -1,62 +0,0 @@ -import jwt -import os -import requests -from datetime import datetime, timedelta, UTC -from openpilot.system.hardware.hw import Paths -from openpilot.system.version import get_version - -API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') - -# name: jwt signature algorithm -KEYS = {"id_rsa": "RS256", - "id_ecdsa": "ES256"} - - -class Api: - def __init__(self, dongle_id): - self.dongle_id = dongle_id - self.jwt_algorithm, self.private_key, _ = get_key_pair() - - def get(self, *args, **kwargs): - return self.request('GET', *args, **kwargs) - - def post(self, *args, **kwargs): - return self.request('POST', *args, **kwargs) - - def request(self, method, endpoint, timeout=None, access_token=None, **params): - return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) - - def get_token(self, payload_extra=None, expiry_hours=1): - now = datetime.now(UTC).replace(tzinfo=None) - payload = { - 'identity': self.dongle_id, - 'nbf': now, - 'iat': now, - 'exp': now + timedelta(hours=expiry_hours) - } - if payload_extra is not None: - payload.update(payload_extra) - token = jwt.encode(payload, self.private_key, algorithm=self.jwt_algorithm) - if isinstance(token, bytes): - token = token.decode('utf8') - return token - - -def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params): - headers = {} - if access_token is not None: - headers['Authorization'] = "JWT " + access_token - - headers['User-Agent'] = "openpilot-" + get_version() - - # TODO: add session to Api - req = requests if session is None else session - return req.request(method, API_HOST + "/" + endpoint, timeout=timeout, headers=headers, params=params) - - -def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]: - for key in KEYS: - if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'): - with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public: - return KEYS[key], private.read(), public.read() - return None, None, None diff --git a/common/api/__init__.py b/common/api/__init__.py new file mode 100644 index 00000000000000..c1fa635bd6808a --- /dev/null +++ b/common/api/__init__.py @@ -0,0 +1,46 @@ +import jwt +import os +import requests +from datetime import datetime, timedelta +from common.basedir import PERSIST +from system.version import get_version + +API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') + +class Api(): + def __init__(self, dongle_id): + self.dongle_id = dongle_id + with open(PERSIST+'/comma/id_rsa') as f: + self.private_key = f.read() + + def get(self, *args, **kwargs): + return self.request('GET', *args, **kwargs) + + def post(self, *args, **kwargs): + return self.request('POST', *args, **kwargs) + + def request(self, method, endpoint, timeout=None, access_token=None, **params): + return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) + + def get_token(self, expiry_hours=1): + now = datetime.utcnow() + payload = { + 'identity': self.dongle_id, + 'nbf': now, + 'iat': now, + 'exp': now + timedelta(hours=expiry_hours) + } + token = jwt.encode(payload, self.private_key, algorithm='RS256') + if isinstance(token, bytes): + token = token.decode('utf8') + return token + + +def api_get(endpoint, method='GET', timeout=None, access_token=None, **params): + headers = {} + if access_token is not None: + headers['Authorization'] = "JWT " + access_token + + headers['User-Agent'] = "openpilot-" + get_version() + + return requests.request(method, API_HOST + "/" + endpoint, timeout=timeout, headers=headers, params=params) diff --git a/common/basedir.py b/common/basedir.py index 6b4811e53c3f89..371b54d3ef34e4 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,4 +1,11 @@ import os +from pathlib import Path +from system.hardware import PC BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) + +if PC: + PERSIST = os.path.join(str(Path.home()), ".comma", "persist") +else: + PERSIST = "/persist" diff --git a/common/clock.pyx b/common/clock.pyx new file mode 100644 index 00000000000000..81333565c58089 --- /dev/null +++ b/common/clock.pyx @@ -0,0 +1,24 @@ +# distutils: language = c++ +# cython: language_level = 3 +from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t + +IF UNAME_SYSNAME == "Darwin": + # Darwin doesn't have a CLOCK_BOOTTIME + CLOCK_BOOTTIME = CLOCK_MONOTONIC_RAW +ELSE: + from posix.time cimport CLOCK_BOOTTIME + +cdef double readclock(clockid_t clock_id): + cdef timespec ts + cdef double current + + clock_gettime(clock_id, &ts) + current = ts.tv_sec + (ts.tv_nsec / 1000000000.) + return current + +def monotonic_time(): + return readclock(CLOCK_MONOTONIC_RAW) + +def sec_since_boot(): + return readclock(CLOCK_BOOTTIME) + diff --git a/common/clutil.cc b/common/clutil.cc index f8381a7e092f8f..9d3447d8074727 100644 --- a/common/clutil.cc +++ b/common/clutil.cc @@ -5,7 +5,6 @@ #include #include "common/util.h" -#include "common/swaglog.h" namespace { // helper functions @@ -32,14 +31,14 @@ void cl_print_info(cl_platform_id platform, cl_device_id device) { case CL_DEVICE_TYPE_ACCELERATOR: type_str = "CL_DEVICE_TYPE_ACCELERATOR"; break; } - LOGD("vendor: %s", get_platform_info(platform, CL_PLATFORM_VENDOR).c_str()); - LOGD("platform version: %s", get_platform_info(platform, CL_PLATFORM_VERSION).c_str()); - LOGD("profile: %s", get_platform_info(platform, CL_PLATFORM_PROFILE).c_str()); - LOGD("extensions: %s", get_platform_info(platform, CL_PLATFORM_EXTENSIONS).c_str()); - LOGD("name: %s", get_device_info(device, CL_DEVICE_NAME).c_str()); - LOGD("device version: %s", get_device_info(device, CL_DEVICE_VERSION).c_str()); - LOGD("max work group size: %zu", work_group_size); - LOGD("type = %d, %s", (int)device_type, type_str); + std::cout << "vendor: " << get_platform_info(platform, CL_PLATFORM_VENDOR) << std::endl + << "platform version: " << get_platform_info(platform, CL_PLATFORM_VERSION) << std::endl + << "profile: " << get_platform_info(platform, CL_PLATFORM_PROFILE) << std::endl + << "extensions: " << get_platform_info(platform, CL_PLATFORM_EXTENSIONS) << std::endl + << "name :" << get_device_info(device, CL_DEVICE_NAME) << std::endl + << "device version :" << get_device_info(device, CL_DEVICE_VERSION) << std::endl + << "max work group size :" << work_group_size << std::endl + << "type = " << device_type << " = " << type_str << std::endl; } void cl_print_build_errors(cl_program program, cl_device_id device) { @@ -50,7 +49,7 @@ void cl_print_build_errors(cl_program program, cl_device_id device) { std::string log(log_size, '\0'); clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL); - LOGE("build failed; status=%d, log: %s", status, log.c_str()); + std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; } } // namespace @@ -62,27 +61,18 @@ cl_device_id cl_get_device_id(cl_device_type device_type) { CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL)); for (size_t i = 0; i < num_platforms; ++i) { - LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str()); - + std::cout << "platform[" << i << "] CL_PLATFORM_NAME: " << get_platform_info(platform_ids[i], CL_PLATFORM_NAME) << std::endl; // Get first device if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) { cl_print_info(platform_ids[i], device_id); return device_id; } } - LOGE("No valid openCL platform found"); + std::cout << "No valid openCL platform found" << std::endl; assert(0); return nullptr; } -cl_context cl_create_context(cl_device_id device_id) { - return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); -} - -void cl_release_context(cl_context context) { - clReleaseContext(context); -} - cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) { return cl_program_from_source(ctx, device_id, util::read_file(path), args); } @@ -96,3 +86,109 @@ cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const } return prg; } + +cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args) { + cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, &binary, NULL, &err)); + if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) { + cl_print_build_errors(prg, device_id); + assert(0); + } + return prg; +} + +// Given a cl code and return a string representation +#define CL_ERR_TO_STR(err) case err: return #err +const char* cl_get_error_string(int err) { + switch (err) { + CL_ERR_TO_STR(CL_SUCCESS); + CL_ERR_TO_STR(CL_DEVICE_NOT_FOUND); + CL_ERR_TO_STR(CL_DEVICE_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_COMPILER_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_MEM_OBJECT_ALLOCATION_FAILURE); + CL_ERR_TO_STR(CL_OUT_OF_RESOURCES); + CL_ERR_TO_STR(CL_OUT_OF_HOST_MEMORY); + CL_ERR_TO_STR(CL_PROFILING_INFO_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_MEM_COPY_OVERLAP); + CL_ERR_TO_STR(CL_IMAGE_FORMAT_MISMATCH); + CL_ERR_TO_STR(CL_IMAGE_FORMAT_NOT_SUPPORTED); + CL_ERR_TO_STR(CL_MAP_FAILURE); + CL_ERR_TO_STR(CL_MISALIGNED_SUB_BUFFER_OFFSET); + CL_ERR_TO_STR(CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST); + CL_ERR_TO_STR(CL_COMPILE_PROGRAM_FAILURE); + CL_ERR_TO_STR(CL_LINKER_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_LINK_PROGRAM_FAILURE); + CL_ERR_TO_STR(CL_DEVICE_PARTITION_FAILED); + CL_ERR_TO_STR(CL_KERNEL_ARG_INFO_NOT_AVAILABLE); + CL_ERR_TO_STR(CL_INVALID_VALUE); + CL_ERR_TO_STR(CL_INVALID_DEVICE_TYPE); + CL_ERR_TO_STR(CL_INVALID_PLATFORM); + CL_ERR_TO_STR(CL_INVALID_DEVICE); + CL_ERR_TO_STR(CL_INVALID_CONTEXT); + CL_ERR_TO_STR(CL_INVALID_QUEUE_PROPERTIES); + CL_ERR_TO_STR(CL_INVALID_COMMAND_QUEUE); + CL_ERR_TO_STR(CL_INVALID_HOST_PTR); + CL_ERR_TO_STR(CL_INVALID_MEM_OBJECT); + CL_ERR_TO_STR(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR); + CL_ERR_TO_STR(CL_INVALID_IMAGE_SIZE); + CL_ERR_TO_STR(CL_INVALID_SAMPLER); + CL_ERR_TO_STR(CL_INVALID_BINARY); + CL_ERR_TO_STR(CL_INVALID_BUILD_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_PROGRAM); + CL_ERR_TO_STR(CL_INVALID_PROGRAM_EXECUTABLE); + CL_ERR_TO_STR(CL_INVALID_KERNEL_NAME); + CL_ERR_TO_STR(CL_INVALID_KERNEL_DEFINITION); + CL_ERR_TO_STR(CL_INVALID_KERNEL); + CL_ERR_TO_STR(CL_INVALID_ARG_INDEX); + CL_ERR_TO_STR(CL_INVALID_ARG_VALUE); + CL_ERR_TO_STR(CL_INVALID_ARG_SIZE); + CL_ERR_TO_STR(CL_INVALID_KERNEL_ARGS); + CL_ERR_TO_STR(CL_INVALID_WORK_DIMENSION); + CL_ERR_TO_STR(CL_INVALID_WORK_GROUP_SIZE); + CL_ERR_TO_STR(CL_INVALID_WORK_ITEM_SIZE); + CL_ERR_TO_STR(CL_INVALID_GLOBAL_OFFSET); + CL_ERR_TO_STR(CL_INVALID_EVENT_WAIT_LIST); + CL_ERR_TO_STR(CL_INVALID_EVENT); + CL_ERR_TO_STR(CL_INVALID_OPERATION); + CL_ERR_TO_STR(CL_INVALID_GL_OBJECT); + CL_ERR_TO_STR(CL_INVALID_BUFFER_SIZE); + CL_ERR_TO_STR(CL_INVALID_MIP_LEVEL); + CL_ERR_TO_STR(CL_INVALID_GLOBAL_WORK_SIZE); + CL_ERR_TO_STR(CL_INVALID_PROPERTY); + CL_ERR_TO_STR(CL_INVALID_IMAGE_DESCRIPTOR); + CL_ERR_TO_STR(CL_INVALID_COMPILER_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_LINKER_OPTIONS); + CL_ERR_TO_STR(CL_INVALID_DEVICE_PARTITION_COUNT); + case -69: return "CL_INVALID_PIPE_SIZE"; + case -70: return "CL_INVALID_DEVICE_QUEUE"; + case -71: return "CL_INVALID_SPEC_ID"; + case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED"; + case -1002: return "CL_INVALID_D3D10_DEVICE_KHR"; + case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR"; + case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR"; + case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR"; + case -1006: return "CL_INVALID_D3D11_DEVICE_KHR"; + case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR"; + case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR"; + case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR"; + case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR"; + case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR"; + case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR"; + case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR"; + case -1093: return "CL_INVALID_EGL_OBJECT_KHR"; + case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR"; + case -1001: return "CL_PLATFORM_NOT_FOUND_KHR"; + case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT"; + case -1058: return "CL_INVALID_PARTITION_COUNT_EXT"; + case -1059: return "CL_INVALID_PARTITION_NAME_EXT"; + case -1094: return "CL_INVALID_ACCELERATOR_INTEL"; + case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL"; + case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL"; + case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL"; + case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR"; + case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL"; + case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL"; + case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL"; + case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL"; + default: return "CL_UNKNOWN_ERROR"; + } +} diff --git a/common/clutil.h b/common/clutil.h index b364e79d45b6fc..be1a07c332a68a 100644 --- a/common/clutil.h +++ b/common/clutil.h @@ -22,7 +22,7 @@ }) cl_device_id cl_get_device_id(cl_device_type device_type); -cl_context cl_create_context(cl_device_id device_id); -void cl_release_context(cl_context context); cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr); +cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr); cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args); +const char* cl_get_error_string(int err); diff --git a/common/constants.py b/common/constants.py deleted file mode 100644 index 7ca425c4b2430a..00000000000000 --- a/common/constants.py +++ /dev/null @@ -1,23 +0,0 @@ -import numpy as np - -# conversions -class CV: - # Speed - MPH_TO_KPH = 1.609344 - KPH_TO_MPH = 1. / MPH_TO_KPH - MS_TO_KPH = 3.6 - KPH_TO_MS = 1. / MS_TO_KPH - MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH - MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS - MS_TO_KNOTS = 1.9438 - KNOTS_TO_MS = 1. / MS_TO_KNOTS - - # Angle - DEG_TO_RAD = np.pi / 180. - RAD_TO_DEG = 1. / DEG_TO_RAD - - # Mass - LB_TO_KG = 0.453592 - - -ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 diff --git a/common/conversions.py b/common/conversions.py new file mode 100644 index 00000000000000..b02b33c625a73f --- /dev/null +++ b/common/conversions.py @@ -0,0 +1,19 @@ +import numpy as np + +class Conversions: + # Speed + MPH_TO_KPH = 1.609344 + KPH_TO_MPH = 1. / MPH_TO_KPH + MS_TO_KPH = 3.6 + KPH_TO_MS = 1. / MS_TO_KPH + MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH + MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS + MS_TO_KNOTS = 1.9438 + KNOTS_TO_MS = 1. / MS_TO_KNOTS + + # Angle + DEG_TO_RAD = np.pi / 180. + RAD_TO_DEG = 1. / DEG_TO_RAD + + # Mass + LB_TO_KG = 0.453592 diff --git a/common/dict_helpers.py b/common/dict_helpers.py new file mode 100644 index 00000000000000..62cff63b58ebb7 --- /dev/null +++ b/common/dict_helpers.py @@ -0,0 +1,9 @@ +# remove all keys that end in DEPRECATED +def strip_deprecated_keys(d): + for k in list(d.keys()): + if isinstance(k, str): + if k.endswith('DEPRECATED'): + d.pop(k) + elif isinstance(d[k], dict): + strip_deprecated_keys(d[k]) + return d diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py new file mode 100644 index 00000000000000..a228b40256abf1 --- /dev/null +++ b/common/ffi_wrapper.py @@ -0,0 +1,55 @@ +import os +import sys +import fcntl +import hashlib +import platform +from cffi import FFI + +def suffix(): + if platform.system() == "Darwin": + return ".dylib" + else: + return ".so" + +def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries=None): + if libraries is None: + libraries = [] + + cache = name + "_" + hashlib.sha1(c_code.encode('utf-8')).hexdigest() + try: + os.mkdir(tmpdir) + except OSError: + pass + + fd = os.open(tmpdir, 0) + fcntl.flock(fd, fcntl.LOCK_EX) + try: + sys.path.append(tmpdir) + try: + mod = __import__(cache) + except Exception: + print(f"cache miss {cache}") + compile_code(cache, c_code, c_header, tmpdir, cflags, libraries) + mod = __import__(cache) + finally: + os.close(fd) + + return mod.ffi, mod.lib + + +def compile_code(name, c_code, c_header, directory, cflags="", libraries=None): + if libraries is None: + libraries = [] + + ffibuilder = FFI() + ffibuilder.set_source(name, c_code, source_extension='.cpp', libraries=libraries) + ffibuilder.cdef(c_header) + os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++1z" + os.environ['CFLAGS'] = cflags + ffibuilder.compile(verbose=True, debug=False, tmpdir=directory) + + +def wrap_compiled(name, directory): + sys.path.append(directory) + mod = __import__(name) + return mod.ffi, mod.lib diff --git a/common/file_helpers.py b/common/file_helpers.py new file mode 100644 index 00000000000000..8a45fa313c455b --- /dev/null +++ b/common/file_helpers.py @@ -0,0 +1,99 @@ +import os +import shutil +import tempfile +from atomicwrites import AtomicWriter + + +def mkdirs_exists_ok(path): + if path.startswith('http://') or path.startswith('https://'): + raise ValueError('URL path') + try: + os.makedirs(path) + except OSError: + if not os.path.isdir(path): + raise + + +def rm_not_exists_ok(path): + try: + os.remove(path) + except OSError: + if os.path.exists(path): + raise + + +def rm_tree_or_link(path): + if os.path.islink(path): + os.unlink(path) + elif os.path.isdir(path): + shutil.rmtree(path) + + +def get_tmpdir_on_same_filesystem(path): + normpath = os.path.normpath(path) + parts = normpath.split("/") + if len(parts) > 1 and parts[1] == "scratch": + return "/scratch/tmp" + elif len(parts) > 2 and parts[2] == "runner": + return f"/{parts[1]}/runner/tmp" + return "/tmp" + + +class NamedTemporaryDir(): + def __init__(self, temp_dir=None): + self._path = tempfile.mkdtemp(dir=temp_dir) + + @property + def name(self): + return self._path + + def close(self): + shutil.rmtree(self._path) + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + self.close() + + +class CallbackReader: + """Wraps a file, but overrides the read method to also + call a callback function with the number of bytes read so far.""" + def __init__(self, f, callback, *args): + self.f = f + self.callback = callback + self.cb_args = args + self.total_read = 0 + + def __getattr__(self, attr): + return getattr(self.f, attr) + + def read(self, *args, **kwargs): + chunk = self.f.read(*args, **kwargs) + self.total_read += len(chunk) + self.callback(*self.cb_args, self.total_read) + return chunk + + +def _get_fileobject_func(writer, temp_dir): + def _get_fileobject(): + return writer.get_fileobject(dir=temp_dir) + return _get_fileobject + +def atomic_write_on_fs_tmp(path, **kwargs): + """Creates an atomic writer using a temporary file in a temporary directory + on the same filesystem as path. + """ + # TODO(mgraczyk): This use of AtomicWriter relies on implementation details to set the temp + # directory. + writer = AtomicWriter(path, **kwargs) + return writer._open(_get_fileobject_func(writer, get_tmpdir_on_same_filesystem(path))) + + +def atomic_write_in_dir(path, **kwargs): + """Creates an atomic writer using a temporary file in the same directory + as the destination file. + """ + writer = AtomicWriter(path, **kwargs) + return writer._open(_get_fileobject_func(writer, os.path.dirname(path))) diff --git a/common/filter_simple.py b/common/filter_simple.py index 212e1a8f409248..0ec7a515621afb 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -1,4 +1,5 @@ class FirstOrderFilter: + # first order filter def __init__(self, x0, rc, dt, initialized=True): self.x = x0 self.dt = dt @@ -15,20 +16,3 @@ def update(self, x): self.initialized = True self.x = x return self.x - - -class BounceFilter(FirstOrderFilter): - def __init__(self, x0, rc, dt, initialized=True, bounce=2): - self.velocity = FirstOrderFilter(0.0, 0.15, dt) - self.bounce = bounce - super().__init__(x0, rc, dt, initialized) - - def update(self, x): - super().update(x) - scale = self.dt / (1.0 / 60.0) # tuned at 60 fps - self.velocity.x += (x - self.x) * self.bounce * scale * self.dt - self.velocity.update(0.0) - if abs(self.velocity.x) < 1e-5: - self.velocity.x = 0.0 - self.x += self.velocity.x - return self.x diff --git a/common/git.py b/common/git.py deleted file mode 100644 index 6b662e57191d03..00000000000000 --- a/common/git.py +++ /dev/null @@ -1,42 +0,0 @@ -from functools import cache -import subprocess -from openpilot.common.utils import run_cmd, run_cmd_default - - -@cache -def get_commit(cwd: str | None = None, branch: str = "HEAD") -> str: - return run_cmd_default(["git", "rev-parse", branch], cwd=cwd) - - -@cache -def get_commit_date(cwd: str | None = None, commit: str = "HEAD") -> str: - return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd) - - -@cache -def get_short_branch(cwd: str | None = None) -> str: - return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd) - - -@cache -def get_branch(cwd: str | None = None) -> str: - return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd) - - -@cache -def get_origin(cwd: str | None = None) -> str: - try: - local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd) - tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd) - return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd) - except subprocess.CalledProcessError: # Not on a branch, fallback - return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd) - - -@cache -def get_normalized_origin(cwd: str | None = None) -> str: - return get_origin(cwd) \ - .replace("git@", "", 1) \ - .replace(".git", "", 1) \ - .replace("https://", "", 1) \ - .replace(":", "/", 1) diff --git a/common/gpio.cc b/common/gpio.cc new file mode 100644 index 00000000000000..73ff1b3f52c204 --- /dev/null +++ b/common/gpio.cc @@ -0,0 +1,32 @@ +#include "common/gpio.h" + +#include +#include + +#include + +#include "common/util.h" + +// We assume that all pins have already been exported on boot, +// and that we have permission to write to them. + +int gpio_init(int pin_nr, bool output) { + char pin_dir_path[50]; + int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path), + "/sys/class/gpio/gpio%d/direction", pin_nr); + if(pin_dir_path_len <= 0) { + return -1; + } + const char *value = output ? "out" : "in"; + return util::write_file(pin_dir_path, (void*)value, strlen(value)); +} + +int gpio_set(int pin_nr, bool high) { + char pin_val_path[50]; + int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path), + "/sys/class/gpio/gpio%d/value", pin_nr); + if(pin_val_path_len <= 0) { + return -1; + } + return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); +} diff --git a/common/gpio.h b/common/gpio.h new file mode 100644 index 00000000000000..e030019875fb4a --- /dev/null +++ b/common/gpio.h @@ -0,0 +1,21 @@ +#pragma once + +// Pin definitions +#ifdef QCOM2 + #define GPIO_HUB_RST_N 30 + #define GPIO_UBLOX_RST_N 32 + #define GPIO_UBLOX_SAFEBOOT_N 33 + #define GPIO_UBLOX_PWR_EN 34 + #define GPIO_STM_RST_N 124 + #define GPIO_STM_BOOT0 134 +#else + #define GPIO_HUB_RST_N 0 + #define GPIO_UBLOX_RST_N 0 + #define GPIO_UBLOX_SAFEBOOT_N 0 + #define GPIO_UBLOX_PWR_EN 0 + #define GPIO_STM_RST_N 0 + #define GPIO_STM_BOOT0 0 +#endif + +int gpio_init(int pin_nr, bool output); +int gpio_set(int pin_nr, bool high); diff --git a/common/gpio.py b/common/gpio.py index 8f025a2daf726e..260f8898a11000 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,7 +1,4 @@ -import os -import fcntl -import ctypes -from functools import cache +from typing import Optional def gpio_init(pin: int, output: bool) -> None: try: @@ -17,7 +14,7 @@ def gpio_set(pin: int, high: bool) -> None: except Exception as e: print(f"Failed to set gpio {pin} value: {e}") -def gpio_read(pin: int) -> bool | None: +def gpio_read(pin: int) -> Optional[bool]: val = None try: with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f: @@ -26,64 +23,3 @@ def gpio_read(pin: int) -> bool | None: print(f"Failed to set gpio {pin} value: {e}") return val - -def gpio_export(pin: int) -> None: - if os.path.isdir(f"/sys/class/gpio/gpio{pin}"): - return - - try: - with open("/sys/class/gpio/export", 'w') as f: - f.write(str(pin)) - except Exception: - print(f"Failed to export gpio {pin}") - -@cache -def get_irq_action(irq: int) -> list[str]: - try: - with open(f"/sys/kernel/irq/{irq}/actions") as f: - actions = f.read().strip().split(',') - return actions - except FileNotFoundError: - return [] - -def get_irqs_for_action(action: str) -> list[str]: - ret = [] - with open("/proc/interrupts") as f: - for l in f.readlines(): - irq = l.split(':')[0].strip() - if irq.isdigit() and action in get_irq_action(irq): - ret.append(irq) - return ret - -# *** gpiochip *** - -class gpioevent_data(ctypes.Structure): - _fields_ = [ - ("timestamp", ctypes.c_uint64), - ("id", ctypes.c_uint32), - ] - -class gpioevent_request(ctypes.Structure): - _fields_ = [ - ("lineoffset", ctypes.c_uint32), - ("handleflags", ctypes.c_uint32), - ("eventflags", ctypes.c_uint32), - ("label", ctypes.c_char * 32), - ("fd", ctypes.c_int) - ] - -def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int: - GPIOEVENT_REQUEST_BOTH_EDGES = 0x3 - GPIOHANDLE_REQUEST_INPUT = 0x1 - GPIO_GET_LINEEVENT_IOCTL = 0xc030b404 - - rq = gpioevent_request() - rq.lineoffset = pin - rq.handleflags = GPIOHANDLE_REQUEST_INPUT - rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES - rq.label = label.encode('utf-8')[:31] + b'\0' - - fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY) - fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq) - os.close(fd) - return int(rq.fd) diff --git a/common/gps.py b/common/gps.py deleted file mode 100644 index 6f96d72e99aa95..00000000000000 --- a/common/gps.py +++ /dev/null @@ -1,8 +0,0 @@ -from openpilot.common.params import Params - - -def get_gps_location_service(params: Params) -> str: - if params.get_bool("UbloxAvailable"): - return "gpsLocationExternal" - else: - return "gpsLocation" diff --git a/common/i2c.cc b/common/i2c.cc new file mode 100644 index 00000000000000..eb10cd64bb8ae5 --- /dev/null +++ b/common/i2c.cc @@ -0,0 +1,87 @@ +#include "common/i2c.h" + +#include +#include +#include + +#include +#include +#include + +#include "common/util.h" +#include "common/swaglog.h" +#include "common/util.h" + +#define UNUSED(x) (void)(x) + +#ifdef QCOM2 +// TODO: decide if we want to isntall libi2c-dev everywhere +extern "C" { + #include + #include +} + +I2CBus::I2CBus(uint8_t bus_id) { + char bus_name[20]; + snprintf(bus_name, 20, "/dev/i2c-%d", bus_id); + + i2c_fd = HANDLE_EINTR(open(bus_name, O_RDWR)); + if(i2c_fd < 0) { + throw std::runtime_error("Failed to open I2C bus"); + } +} + +I2CBus::~I2CBus() { + if(i2c_fd >= 0) { close(i2c_fd); } +} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { + int ret = 0; + + ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer); + if((ret < 0) || (ret != len)) { goto fail; } + +fail: + return ret; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { + int ret = 0; + + ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data); + if(ret < 0) { goto fail; } + +fail: + return ret; +} + +#else + +I2CBus::I2CBus(uint8_t bus_id) { + UNUSED(bus_id); + i2c_fd = -1; +} + +I2CBus::~I2CBus() {} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { + UNUSED(device_address); + UNUSED(register_address); + UNUSED(buffer); + UNUSED(len); + return -1; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { + UNUSED(device_address); + UNUSED(register_address); + UNUSED(data); + return -1; +} +#endif diff --git a/common/i2c.h b/common/i2c.h new file mode 100644 index 00000000000000..0669116bb86733 --- /dev/null +++ b/common/i2c.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include + +class I2CBus { + private: + int i2c_fd; + + public: + I2CBus(uint8_t bus_id); + ~I2CBus(); + + int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint8_t device_address, uint register_address, uint8_t data); +}; diff --git a/common/i2c.py b/common/i2c.py deleted file mode 100644 index 1dfaa659ad302e..00000000000000 --- a/common/i2c.py +++ /dev/null @@ -1,81 +0,0 @@ -import os -import fcntl -import ctypes - -# I2C constants from /usr/include/linux/i2c-dev.h -I2C_SLAVE = 0x0703 -I2C_SLAVE_FORCE = 0x0706 -I2C_SMBUS = 0x0720 - -# SMBus transfer types -I2C_SMBUS_READ = 1 -I2C_SMBUS_WRITE = 0 -I2C_SMBUS_BYTE_DATA = 2 -I2C_SMBUS_I2C_BLOCK_DATA = 8 - -I2C_SMBUS_BLOCK_MAX = 32 - - -class _I2cSmbusData(ctypes.Union): - _fields_ = [ - ("byte", ctypes.c_uint8), - ("word", ctypes.c_uint16), - ("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)), - ] - - -class _I2cSmbusIoctlData(ctypes.Structure): - _fields_ = [ - ("read_write", ctypes.c_uint8), - ("command", ctypes.c_uint8), - ("size", ctypes.c_uint32), - ("data", ctypes.POINTER(_I2cSmbusData)), - ] - - -class SMBus: - def __init__(self, bus: int): - self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR) - - def __enter__(self) -> 'SMBus': - return self - - def __exit__(self, *args) -> None: - self.close() - - def close(self) -> None: - if hasattr(self, '_fd') and self._fd >= 0: - os.close(self._fd) - self._fd = -1 - - def _set_address(self, addr: int, force: bool = False) -> None: - ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE - fcntl.ioctl(self._fd, ioctl_arg, addr) - - def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None: - ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data)) - fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data) - - def read_byte_data(self, addr: int, register: int, force: bool = False) -> int: - self._set_address(addr, force) - data = _I2cSmbusData() - self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data) - return int(data.byte) - - def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None: - self._set_address(addr, force) - data = _I2cSmbusData() - data.byte = value & 0xFF - self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data) - - def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]: - self._set_address(addr, force) - if not (0 <= length <= I2C_SMBUS_BLOCK_MAX): - raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}") - - data = _I2cSmbusData() - data.block[0] = length - self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data) - read_len = int(data.block[0]) or length - read_len = min(read_len, length) - return [int(b) for b in data.block[1 : read_len + 1]] diff --git a/common/kalman/.gitignore b/common/kalman/.gitignore new file mode 100644 index 00000000000000..d86912e7d04c50 --- /dev/null +++ b/common/kalman/.gitignore @@ -0,0 +1 @@ +simple_kalman_impl.c diff --git a/common/kalman/SConscript b/common/kalman/SConscript new file mode 100644 index 00000000000000..d60354c987ab60 --- /dev/null +++ b/common/kalman/SConscript @@ -0,0 +1,3 @@ +Import('envCython') + +envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx') diff --git a/cereal/messaging/tests/__init__.py b/common/kalman/__init__.py similarity index 100% rename from cereal/messaging/tests/__init__.py rename to common/kalman/__init__.py diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py new file mode 100644 index 00000000000000..33289e4f509fbc --- /dev/null +++ b/common/kalman/simple_kalman.py @@ -0,0 +1,3 @@ +# pylint: skip-file +from common.kalman.simple_kalman_impl import KF1D as KF1D +assert KF1D diff --git a/common/kalman/simple_kalman_impl.pxd b/common/kalman/simple_kalman_impl.pxd new file mode 100644 index 00000000000000..cb39a45bca2c36 --- /dev/null +++ b/common/kalman/simple_kalman_impl.pxd @@ -0,0 +1,18 @@ +# cython: language_level = 3 + +cdef class KF1D: + cdef public: + double x0_0 + double x1_0 + double K0_0 + double K1_0 + double A0_0 + double A0_1 + double A1_0 + double A1_1 + double C0_0 + double C0_1 + double A_K_0 + double A_K_1 + double A_K_2 + double A_K_3 diff --git a/common/kalman/simple_kalman_impl.pyx b/common/kalman/simple_kalman_impl.pyx new file mode 100644 index 00000000000000..16aefba2e5b796 --- /dev/null +++ b/common/kalman/simple_kalman_impl.pyx @@ -0,0 +1,37 @@ +# distutils: language = c++ +# cython: language_level=3 + +cdef class KF1D: + def __init__(self, x0, A, C, K): + self.x0_0 = x0[0][0] + self.x1_0 = x0[1][0] + self.A0_0 = A[0][0] + self.A0_1 = A[0][1] + self.A1_0 = A[1][0] + self.A1_1 = A[1][1] + self.C0_0 = C[0] + self.C0_1 = C[1] + self.K0_0 = K[0][0] + self.K1_0 = K[1][0] + + self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 + self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 + self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 + self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 + + def update(self, meas): + cdef double x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas + cdef double x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas + self.x0_0 = x0_0 + self.x1_0 = x1_0 + + return [self.x0_0, self.x1_0] + + @property + def x(self): + return [[self.x0_0], [self.x1_0]] + + @x.setter + def x(self, x): + self.x0_0 = x[0][0] + self.x1_0 = x[1][0] diff --git a/common/kalman/simple_kalman_old.py b/common/kalman/simple_kalman_old.py new file mode 100644 index 00000000000000..d11770faf6cc74 --- /dev/null +++ b/common/kalman/simple_kalman_old.py @@ -0,0 +1,23 @@ +import numpy as np + + +class KF1D: + # this EKF assumes constant covariance matrix, so calculations are much simpler + # the Kalman gain also needs to be precomputed using the control module + + def __init__(self, x0, A, C, K): + self.x = x0 + self.A = A + self.C = np.atleast_2d(C) + self.K = K + + self.A_K = self.A - np.dot(self.K, self.C) + + # K matrix needs to be pre-computed as follow: + # import control + # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) + # self.K = np.transpose(K) + + def update(self, meas): + self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) + return self.x diff --git a/openpilot/__init__.py b/common/kalman/tests/__init__.py similarity index 100% rename from openpilot/__init__.py rename to common/kalman/tests/__init__.py diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py new file mode 100644 index 00000000000000..96b252765526d7 --- /dev/null +++ b/common/kalman/tests/test_simple_kalman.py @@ -0,0 +1,87 @@ +import unittest +import random +import timeit +import numpy as np + +from common.kalman.simple_kalman import KF1D +from common.kalman.simple_kalman_old import KF1D as KF1D_old + + +class TestSimpleKalman(unittest.TestCase): + def setUp(self): + dt = 0.01 + x0_0 = 0.0 + x1_0 = 0.0 + A0_0 = 1.0 + A0_1 = dt + A1_0 = 0.0 + A1_1 = 1.0 + C0_0 = 1.0 + C0_1 = 0.0 + K0_0 = 0.12287673 + K1_0 = 0.29666309 + + self.kf_old = KF1D_old(x0=np.array([[x0_0], [x1_0]]), + A=np.array([[A0_0, A0_1], [A1_0, A1_1]]), + C=np.array([C0_0, C0_1]), + K=np.array([[K0_0], [K1_0]])) + + self.kf = KF1D(x0=[[x0_0], [x1_0]], + A=[[A0_0, A0_1], [A1_0, A1_1]], + C=[C0_0, C0_1], + K=[[K0_0], [K1_0]]) + + def test_getter_setter(self): + self.kf.x = [[1.0], [1.0]] + self.assertEqual(self.kf.x, [[1.0], [1.0]]) + + def update_returns_state(self): + x = self.kf.update(100) + self.assertEqual(x, self.kf.x) + + def test_old_equal_new(self): + for _ in range(1000): + v_wheel = random.uniform(0, 200) + + x_old = self.kf_old.update(v_wheel) + x = self.kf.update(v_wheel) + + # Compare the output x, verify that the error is less than 1e-4 + np.testing.assert_almost_equal(x_old[0], x[0]) + np.testing.assert_almost_equal(x_old[1], x[1]) + + def test_new_is_faster(self): + setup = """ +import numpy as np + +from common.kalman.simple_kalman import KF1D +from common.kalman.simple_kalman_old import KF1D as KF1D_old + +dt = 0.01 +x0_0 = 0.0 +x1_0 = 0.0 +A0_0 = 1.0 +A0_1 = dt +A1_0 = 0.0 +A1_1 = 1.0 +C0_0 = 1.0 +C0_1 = 0.0 +K0_0 = 0.12287673 +K1_0 = 0.29666309 + +kf_old = KF1D_old(x0=np.array([[x0_0], [x1_0]]), + A=np.array([[A0_0, A0_1], [A1_0, A1_1]]), + C=np.array([C0_0, C0_1]), + K=np.array([[K0_0], [K1_0]])) + +kf = KF1D(x0=[[x0_0], [x1_0]], + A=[[A0_0, A0_1], [A1_0, A1_1]], + C=[C0_0, C0_1], + K=[[K0_0], [K1_0]]) + """ + kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000) + kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000) + self.assertTrue(kf_speed < kf_old_speed / 4) + +if __name__ == "__main__": + unittest.main() diff --git a/common/lazy_property.py b/common/lazy_property.py new file mode 100644 index 00000000000000..919dd9e87e8ec4 --- /dev/null +++ b/common/lazy_property.py @@ -0,0 +1,12 @@ +class lazy_property(): + """Defines a property whose value will be computed only once and as needed. + + This can only be used on instance methods. + """ + def __init__(self, func): + self._func = func + + def __get__(self, obj_self, cls): + value = self._func(obj_self) + setattr(obj_self, self._func.__name__, value) + return value diff --git a/common/logging_extra.py b/common/logging_extra.py index ceaf083cd5c442..5baaac1f9050b8 100644 --- a/common/logging_extra.py +++ b/common/logging_extra.py @@ -8,7 +8,6 @@ import socket import logging import traceback -import numpy as np from threading import local from collections import OrderedDict from contextlib import contextmanager @@ -16,8 +15,6 @@ LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ def json_handler(obj): - if isinstance(obj, np.bool_): - return bool(obj) # if isinstance(obj, (datetime.date, datetime.time)): # return obj.isoformat() return repr(obj) @@ -156,9 +153,9 @@ def bind(self, **kwargs): def bind_global(self, **kwargs): self.global_ctx.update(kwargs) - def event(self, event, *args, **kwargs): + def event(self, event_name, *args, **kwargs): evt = NiceOrderedDict() - evt['event'] = event + evt['event'] = event_name if args: evt['args'] = args evt.update(kwargs) @@ -199,7 +196,8 @@ def findCaller(self, stack_info=False, stacklevel=1): co = f.f_code filename = os.path.normcase(co.co_filename) - if filename == _srcfile: + # TODO: is this pylint exception correct? + if filename == _srcfile: # pylint: disable=comparison-with-callable f = f.f_back continue sinfo = None diff --git a/common/markdown.py b/common/markdown.py deleted file mode 100644 index f0f056d9632ce0..00000000000000 --- a/common/markdown.py +++ /dev/null @@ -1,45 +0,0 @@ -HTML_REPLACEMENTS = [ - (r'&', r'&'), - (r'"', r'"'), -] - -def parse_markdown(text: str, tab_length: int = 2) -> str: - lines = text.split("\n") - output: list[str] = [] - list_level = 0 - - def end_outstanding_lists(level: int, end_level: int) -> int: - while level > end_level: - level -= 1 - output.append("") - if level > 0: - output.append("") - return end_level - - for i, line in enumerate(lines): - if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading - output.append(f"

{line}

") - elif line.startswith("==="): - pass - elif line.lstrip().startswith("* "): # list - line_level = 1 + line.count(" " * tab_length, 0, line.index("*")) - if list_level >= line_level: - list_level = end_outstanding_lists(list_level, line_level) - else: - list_level += 1 - if list_level > 1: - output[-1] = output[-1].replace("", "") - output.append("
    ") - output.append(f"
  • {line.replace('*', '', 1).lstrip()}
    • ") - else: - list_level = end_outstanding_lists(list_level, 0) - if len(line) > 0: - output.append(line) - - end_outstanding_lists(list_level, 0) - output_str = "\n".join(output) + "\n" - - for (fr, to) in HTML_REPLACEMENTS: - output_str = output_str.replace(fr, to) - - return output_str diff --git a/common/mat.h b/common/mat.h index 8e10d619717ba3..626f3404feab49 100644 --- a/common/mat.h +++ b/common/mat.h @@ -1,7 +1,7 @@ #pragma once typedef struct vec3 { - float v[3]; + float v[3]; } vec3; typedef struct vec4 { @@ -9,7 +9,7 @@ typedef struct vec4 { } vec4; typedef struct mat3 { - float v[3*3]; + float v[3*3]; } mat3; typedef struct mat4 { diff --git a/common/mock/__init__.py b/common/mock/__init__.py deleted file mode 100644 index 4b01dfe8411b11..00000000000000 --- a/common/mock/__init__.py +++ /dev/null @@ -1,50 +0,0 @@ -""" -Utilities for generating mock messages for testing. -example in common/tests/test_mock.py -""" - - -import functools -import threading -from cereal.messaging import PubMaster -from cereal.services import SERVICE_LIST -from openpilot.common.mock.generators import generate_livePose -from openpilot.common.realtime import Ratekeeper - - -MOCK_GENERATOR = { - "livePose": generate_livePose -} - - -def generate_messages_loop(services: list[str], done: threading.Event): - pm = PubMaster(services) - rk = Ratekeeper(100) - i = 0 - while not done.is_set(): - for s in services: - should_send = i % (100/SERVICE_LIST[s].frequency) == 0 - if should_send: - message = MOCK_GENERATOR[s]() - pm.send(s, message) - i += 1 - rk.keep_time() - - -def mock_messages(services: list[str] | str): - if isinstance(services, str): - services = [services] - - def decorator(func): - @functools.wraps(func) - def wrapper(*args, **kwargs): - done = threading.Event() - t = threading.Thread(target=generate_messages_loop, args=(services, done)) - t.start() - try: - return func(*args, **kwargs) - finally: - done.set() - t.join() - return wrapper - return decorator diff --git a/common/mock/generators.py b/common/mock/generators.py deleted file mode 100644 index 5cd9c88a56fc33..00000000000000 --- a/common/mock/generators.py +++ /dev/null @@ -1,14 +0,0 @@ -from cereal import messaging - - -def generate_livePose(): - msg = messaging.new_message('livePose') - meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True} - msg.livePose.orientationNED = meas - msg.livePose.velocityDevice = meas - msg.livePose.angularVelocityDevice = meas - msg.livePose.accelerationDevice = meas - msg.livePose.inputsOK = True - msg.livePose.posenetOK = True - msg.livePose.sensorsOK = True - return msg diff --git a/common/modeldata.h b/common/modeldata.h new file mode 100644 index 00000000000000..e13840d53eb07d --- /dev/null +++ b/common/modeldata.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include "common/mat.h" +#include "system/hardware/hw.h" + +const int TRAJECTORY_SIZE = 33; +const int LAT_MPC_N = 16; +const int LON_MPC_N = 32; +const float MIN_DRAW_DISTANCE = 10.0; +const float MAX_DRAW_DISTANCE = 100.0; + +template +constexpr std::array build_idxs(float max_val) { + std::array result{}; + for (int i = 0; i < size; ++i) { + result[i] = max_val * ((i / (double)(size - 1)) * (i / (double)(size - 1))); + } + return result; +} + +constexpr auto T_IDXS = build_idxs(10.0); +constexpr auto T_IDXS_FLOAT = build_idxs(10.0); +constexpr auto X_IDXS = build_idxs(192.0); +constexpr auto X_IDXS_FLOAT = build_idxs(192.0); + +const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2, + 0.0, 2648.0, 1208.0 / 2, + 0.0, 0.0, 1.0}}; + +// tici ecam focal probably wrong? magnification is not consistent across frame +// Need to retrain model before this can be changed +const mat3 ecam_intrinsic_matrix = (mat3){{567.0, 0.0, 1928.0 / 2, + 0.0, 567.0, 1208.0 / 2, + 0.0, 0.0, 1.0}}; + +static inline mat3 get_model_yuv_transform() { + float db_s = 1.0; + const mat3 transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + }}; + // Can this be removed since scale is 1? + return transform_scale_buffer(transform, db_s); +} diff --git a/common/numpy_fast.py b/common/numpy_fast.py new file mode 100644 index 00000000000000..878c0005c8e811 --- /dev/null +++ b/common/numpy_fast.py @@ -0,0 +1,19 @@ +def clip(x, lo, hi): + return max(lo, min(hi, x)) + +def interp(x, xp, fp): + N = len(xp) + + def get_interp(xv): + hi = 0 + while hi < N and xv > xp[hi]: + hi += 1 + low = hi - 1 + return fp[-1] if hi == N and xv > xp[low] else ( + fp[0] if hi == 0 else + (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) + + return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x) + +def mean(x): + return sum(x) / len(x) diff --git a/common/numpy_helpers.py b/common/numpy_helpers.py new file mode 100644 index 00000000000000..7b1efe89761d1f --- /dev/null +++ b/common/numpy_helpers.py @@ -0,0 +1,22 @@ +import numpy as np + + +def deep_interp_np(x, xp, fp, axis=None): + if axis is not None: + fp = fp.swapaxes(0,axis) + x = np.atleast_1d(x) + xp = np.array(xp) + if len(xp) < 2: + return np.repeat(fp, len(x), axis=0) + if min(np.diff(xp)) < 0: + raise RuntimeError('Bad x array for interpolation') + j = np.searchsorted(xp, x) - 1 + j = np.clip(j, 0, len(xp)-2) + d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0) + vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T + if axis is not None: + vals_interp = vals_interp.swapaxes(0,axis) + if len(vals_interp) == 1: + return vals_interp[0] + else: + return vals_interp diff --git a/common/params.cc b/common/params.cc index 6af00fe95c0109..5c8c94be5393ae 100644 --- a/common/params.cc +++ b/common/params.cc @@ -3,13 +3,9 @@ #include #include -#include -#include #include #include -#include "common/params_keys.h" -#include "common/queue.h" #include "common/swaglog.h" #include "common/util.h" #include "system/hardware/hw.h" @@ -25,8 +21,8 @@ int fsync_dir(const std::string &path) { int result = -1; int fd = HANDLE_EINTR(open(path.c_str(), O_RDONLY, 0755)); if (fd >= 0) { - result = HANDLE_EINTR(fsync(fd)); - HANDLE_EINTR(close(fd)); + result = fsync(fd); + close(fd); } return result; } @@ -67,9 +63,7 @@ bool create_params_path(const std::string ¶m_path, const std::string &key_pa std::string ensure_params_path(const std::string &prefix, const std::string &path = {}) { std::string params_path = path.empty() ? Path::params() : path; if (!create_params_path(params_path, params_path + prefix)) { - throw std::runtime_error(util::string_format( - "Failed to ensure params path, errno=%d, path=%s, param_prefix=%s", - errno, params_path.c_str(), prefix.c_str())); + throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno)); } return params_path; } @@ -88,43 +82,127 @@ class FileLock { int fd_ = -1; }; +std::unordered_map keys = { + {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, + {"AssistNowToken", PERSISTENT}, + {"AthenadPid", PERSISTENT}, + {"AthenadUploadQueue", PERSISTENT}, + {"CalibrationParams", PERSISTENT}, + {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, + {"CameraDebugExpTime", CLEAR_ON_MANAGER_START}, + {"CarBatteryCapacity", PERSISTENT}, + {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CarParamsCache", CLEAR_ON_MANAGER_START}, + {"CarParamsPersistent", PERSISTENT}, + {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CompletedTrainingVersion", PERSISTENT}, + {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"DashcamOverride", PERSISTENT}, + {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"DisablePowerDown", PERSISTENT}, + {"EndToEndLong", PERSISTENT}, + {"ExperimentalLongitudinalEnabled", PERSISTENT}, // WARNING: THIS MAY DISABLE AEB + {"DisableUpdates", PERSISTENT}, + {"DisengageOnAccelerator", PERSISTENT}, + {"DongleId", PERSISTENT}, + {"DoReboot", CLEAR_ON_MANAGER_START}, + {"DoShutdown", CLEAR_ON_MANAGER_START}, + {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"ForcePowerDown", CLEAR_ON_MANAGER_START}, + {"GitBranch", PERSISTENT}, + {"GitCommit", PERSISTENT}, + {"GitDiff", PERSISTENT}, + {"GithubSshKeys", PERSISTENT}, + {"GithubUsername", PERSISTENT}, + {"GitRemote", PERSISTENT}, + {"GsmApn", PERSISTENT}, + {"GsmRoaming", PERSISTENT}, + {"HardwareSerial", PERSISTENT}, + {"HasAcceptedTerms", PERSISTENT}, + {"IMEI", PERSISTENT}, + {"InstallDate", PERSISTENT}, + {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, + {"IsEngaged", PERSISTENT}, + {"IsLdwEnabled", PERSISTENT}, + {"IsMetric", PERSISTENT}, + {"IsOffroad", CLEAR_ON_MANAGER_START}, + {"IsOnroad", PERSISTENT}, + {"IsRhdDetected", PERSISTENT}, + {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, + {"IsTestedBranch", CLEAR_ON_MANAGER_START}, + {"IsUpdateAvailable", CLEAR_ON_MANAGER_START}, + {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"LaikadEphemeris", PERSISTENT | DONT_LOG}, + {"LanguageSetting", PERSISTENT}, + {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, + {"LastGPSPosition", PERSISTENT}, + {"LastManagerExitReason", CLEAR_ON_MANAGER_START}, + {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, + {"LastSystemShutdown", CLEAR_ON_MANAGER_START}, + {"LastUpdateException", CLEAR_ON_MANAGER_START}, + {"LastUpdateTime", PERSISTENT}, + {"LiveParameters", PERSISTENT}, + {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"NavSettingTime24h", PERSISTENT}, + {"NavSettingLeftSide", PERSISTENT}, + {"NavdRender", PERSISTENT}, + {"OpenpilotEnabledToggle", PERSISTENT}, + {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"PandaSignatures", CLEAR_ON_MANAGER_START}, + {"Passive", PERSISTENT}, + {"PrimeType", PERSISTENT}, + {"RecordFront", PERSISTENT}, + {"RecordFrontLock", PERSISTENT}, // for the internal fleet + {"ReleaseNotes", PERSISTENT}, + {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, + {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"SshEnabled", PERSISTENT}, + {"SubscriberInfo", PERSISTENT}, + {"SwitchToBranch", CLEAR_ON_MANAGER_START}, + {"TermsVersion", PERSISTENT}, + {"Timezone", PERSISTENT}, + {"TrainingVersion", PERSISTENT}, + {"UpdateAvailable", CLEAR_ON_MANAGER_START}, + {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, + {"Version", PERSISTENT}, + {"VisionRadarToggle", PERSISTENT}, + {"WideCameraOnly", PERSISTENT}, + {"ApiCache_Device", PERSISTENT}, + {"ApiCache_DriveStats", PERSISTENT}, + {"ApiCache_NavDestinations", PERSISTENT}, + {"ApiCache_Owner", PERSISTENT}, + {"Offroad_BadNvme", CLEAR_ON_MANAGER_START}, + {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, + {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, + {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, + {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, + {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, + {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START}, + {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, + {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, + {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, +}; + } // namespace Params::Params(const std::string &path) { - params_prefix = "/" + util::getenv("OPENPILOT_PREFIX", "d"); - params_path = ensure_params_path(params_prefix, path); -} - -Params::~Params() { - if (future.valid()) { - future.wait(); - } - assert(queue.empty()); -} - -std::vector Params::allKeys() const { - std::vector ret; - for (auto &p : keys) { - ret.push_back(p.first); - } - return ret; + const char* env = std::getenv("OPENPILOT_PREFIX"); + prefix = env ? "/" + std::string(env) : "/d"; + std::string default_param_path = ensure_params_path(prefix); + params_path = path.empty() ? default_param_path : ensure_params_path(prefix, path); } bool Params::checkKey(const std::string &key) { return keys.find(key) != keys.end(); } -ParamKeyFlag Params::getKeyFlag(const std::string &key) { - return static_cast(keys[key].flags); -} - ParamKeyType Params::getKeyType(const std::string &key) { - return keys[key].type; -} - -std::optional Params::getKeyDefaultValue(const std::string &key) { - return keys[key].default_value; + return static_cast(keys[key]); } int Params::put(const char* key, const char* value, size_t value_size) { @@ -148,7 +226,7 @@ int Params::put(const char* key, const char* value, size_t value_size) { } // fsync to force persist the changes. - if ((result = HANDLE_EINTR(fsync(tmp_fd))) < 0) break; + if ((result = fsync(tmp_fd)) < 0) break; FileLock file_lock(params_path + "/.lock"); @@ -160,9 +238,7 @@ int Params::put(const char* key, const char* value, size_t value_size) { } while (false); close(tmp_fd); - if (result != 0) { - ::unlink(tmp_path.c_str()); - } + ::unlink(tmp_path.c_str()); return result; } @@ -203,40 +279,15 @@ std::map Params::readAll() { return util::read_files_in_dir(getParamPath()); } -void Params::clearAll(ParamKeyFlag key_flag) { +void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - // 1) delete params of key_flag - // 2) delete files that are not defined in the keys. - if (DIR *d = opendir(getParamPath().c_str())) { - struct dirent *de = NULL; - while ((de = readdir(d))) { - if (de->d_type != DT_DIR) { - auto it = keys.find(de->d_name); - if (it == keys.end() || (it->second.flags & key_flag)) { - unlink(getParamPath(de->d_name).c_str()); - } - } + std::string path; + for (auto &[key, type] : keys) { + if (type & key_type) { + unlink(getParamPath(key).c_str()); } - closedir(d); } fsync_dir(getParamPath()); } - -void Params::putNonBlocking(const std::string &key, const std::string &val) { - queue.push(std::make_pair(key, val)); - // start thread on demand - if (!future.valid() || future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { - future = std::async(std::launch::async, &Params::asyncWriteThread, this); - } -} - -void Params::asyncWriteThread() { - // TODO: write the latest one if a key has multiple values in the queue. - std::pair p; - while (queue.try_pop(p, 0)) { - // Params::put is Thread-Safe - put(p.first, p.second); - } -} diff --git a/common/params.h b/common/params.h index 8169063ac0d932..aa4b1d7af35d8d 100644 --- a/common/params.h +++ b/common/params.h @@ -1,67 +1,34 @@ #pragma once -#include #include -#include #include -#include -#include -#include -#include "common/queue.h" - -enum ParamKeyFlag { +enum ParamKeyType { PERSISTENT = 0x02, CLEAR_ON_MANAGER_START = 0x04, - CLEAR_ON_ONROAD_TRANSITION = 0x08, - CLEAR_ON_OFFROAD_TRANSITION = 0x10, + CLEAR_ON_IGNITION_ON = 0x08, + CLEAR_ON_IGNITION_OFF = 0x10, DONT_LOG = 0x20, - DEVELOPMENT_ONLY = 0x40, - CLEAR_ON_IGNITION_ON = 0x80, ALL = 0xFFFFFFFF }; -enum ParamKeyType { - STRING = 0, // must be utf-8 decodable - BOOL = 1, - INT = 2, - FLOAT = 3, - TIME = 4, // ISO 8601 - JSON = 5, - BYTES = 6 -}; - -struct ParamKeyAttributes { - uint32_t flags; - ParamKeyType type; - std::optional default_value = std::nullopt; -}; - class Params { public: - explicit Params(const std::string &path = {}); - ~Params(); - // Not copyable. - Params(const Params&) = delete; - Params& operator=(const Params&) = delete; - - std::vector allKeys() const; + Params(const std::string &path = {}); bool checkKey(const std::string &key); - ParamKeyFlag getKeyFlag(const std::string &key); ParamKeyType getKeyType(const std::string &key); - std::optional getKeyDefaultValue(const std::string &key); inline std::string getParamPath(const std::string &key = {}) { - return params_path + params_prefix + (key.empty() ? "" : "/" + key); + return params_path + prefix + (key.empty() ? "" : "/" + key); } // Delete a value int remove(const std::string &key); - void clearAll(ParamKeyFlag flag); + void clearAll(ParamKeyType type); // helpers for reading values std::string get(const std::string &key, bool block = false); - inline bool getBool(const std::string &key, bool block = false) { - return get(key, block) == "1"; + inline bool getBool(const std::string &key) { + return get(key) == "1"; } std::map readAll(); @@ -73,18 +40,8 @@ class Params { inline int putBool(const std::string &key, bool val) { return put(key.c_str(), val ? "1" : "0", 1); } - void putNonBlocking(const std::string &key, const std::string &val); - inline void putBoolNonBlocking(const std::string &key, bool val) { - putNonBlocking(key, val ? "1" : "0"); - } private: - void asyncWriteThread(); - std::string params_path; - std::string params_prefix; - - // for nonblocking write - std::future future; - SafeQueue> queue; + std::string prefix; }; diff --git a/common/params.py b/common/params.py index 494617200f2cfc..b6be424d417c81 100644 --- a/common/params.py +++ b/common/params.py @@ -1,8 +1,9 @@ -from openpilot.common.params_pyx import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName +from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking # pylint: disable=no-name-in-module, import-error assert Params -assert ParamKeyFlag assert ParamKeyType assert UnknownKeyName +assert put_nonblocking +assert put_bool_nonblocking if __name__ == "__main__": import sys diff --git a/common/params_keys.h b/common/params_keys.h deleted file mode 100644 index d6104e749773dc..00000000000000 --- a/common/params_keys.h +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once - -#include -#include - -#include "cereal/gen/cpp/log.capnp.h" - -inline static std::unordered_map keys = { - {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}}, - {"AdbEnabled", {PERSISTENT, BOOL}}, - {"AlwaysOnDM", {PERSISTENT, BOOL}}, - {"ApiCache_Device", {PERSISTENT, STRING}}, - {"ApiCache_FirehoseStats", {PERSISTENT, JSON}}, - {"AssistNowToken", {PERSISTENT, STRING}}, - {"AthenadPid", {PERSISTENT, INT}}, - {"AthenadUploadQueue", {PERSISTENT, JSON}}, - {"AthenadRecentlyViewedRoutes", {PERSISTENT, STRING}}, - {"BootCount", {PERSISTENT, INT}}, - {"CalibrationParams", {PERSISTENT, BYTES}}, - {"CameraDebugExpGain", {CLEAR_ON_MANAGER_START, STRING}}, - {"CameraDebugExpTime", {CLEAR_ON_MANAGER_START, STRING}}, - {"CarBatteryCapacity", {PERSISTENT, INT}}, - {"CarParams", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BYTES}}, - {"CarParamsCache", {CLEAR_ON_MANAGER_START, BYTES}}, - {"CarParamsPersistent", {PERSISTENT, BYTES}}, - {"CarParamsPrevRoute", {PERSISTENT, BYTES}}, - {"CompletedTrainingVersion", {PERSISTENT, STRING, "0"}}, - {"ControlsReady", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"CurrentBootlog", {PERSISTENT, STRING}}, - {"CurrentRoute", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, STRING}}, - {"DisableLogging", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"DisablePowerDown", {PERSISTENT, BOOL}}, - {"DisableUpdates", {PERSISTENT, BOOL}}, - {"DisengageOnAccelerator", {PERSISTENT, BOOL, "0"}}, - {"DongleId", {PERSISTENT, STRING}}, - {"DoReboot", {CLEAR_ON_MANAGER_START, BOOL}}, - {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, - {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, - {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, - {"AlphaLongitudinalEnabled", {PERSISTENT | DEVELOPMENT_ONLY, BOOL}}, - {"ExperimentalMode", {PERSISTENT, BOOL}}, - {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, - {"FirmwareQueryDone", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"ForcePowerDown", {PERSISTENT, BOOL}}, - {"GitBranch", {PERSISTENT, STRING}}, - {"GitCommit", {PERSISTENT, STRING}}, - {"GitCommitDate", {PERSISTENT, STRING}}, - {"GitDiff", {PERSISTENT, STRING}}, - {"GithubSshKeys", {PERSISTENT, STRING}}, - {"GithubUsername", {PERSISTENT, STRING}}, - {"GitRemote", {PERSISTENT, STRING}}, - {"GsmApn", {PERSISTENT, STRING}}, - {"GsmMetered", {PERSISTENT, BOOL, "1"}}, - {"GsmRoaming", {PERSISTENT, BOOL}}, - {"HardwareSerial", {PERSISTENT, STRING}}, - {"HasAcceptedTerms", {PERSISTENT, STRING, "0"}}, - {"InstallDate", {PERSISTENT, TIME}}, - {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}}, - {"IsEngaged", {PERSISTENT, BOOL}}, - {"IsLdwEnabled", {PERSISTENT, BOOL}}, - {"IsMetric", {PERSISTENT, BOOL}}, - {"IsOffroad", {CLEAR_ON_MANAGER_START, BOOL}}, - {"IsOnroad", {PERSISTENT, BOOL}}, - {"IsRhdDetected", {PERSISTENT, BOOL}}, - {"IsReleaseBranch", {CLEAR_ON_MANAGER_START, BOOL}}, - {"IsTakingSnapshot", {CLEAR_ON_MANAGER_START, BOOL}}, - {"IsTestedBranch", {CLEAR_ON_MANAGER_START, BOOL}}, - {"JoystickDebugMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, - {"LanguageSetting", {PERSISTENT, STRING, "en"}}, - {"LastAthenaPingTime", {CLEAR_ON_MANAGER_START, INT}}, - {"LastGPSPosition", {PERSISTENT, STRING}}, - {"LastManagerExitReason", {CLEAR_ON_MANAGER_START, STRING}}, - {"LastOffroadStatusPacket", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}}, - {"LastAgnosPowerMonitorShutdown", {CLEAR_ON_MANAGER_START, STRING}}, - {"LastPowerDropDetected", {CLEAR_ON_MANAGER_START, STRING}}, - {"LastUpdateException", {CLEAR_ON_MANAGER_START, STRING}}, - {"LastUpdateRouteCount", {PERSISTENT, INT, "0"}}, - {"LastUpdateTime", {PERSISTENT, TIME}}, - {"LastUpdateUptimeOnroad", {PERSISTENT, FLOAT, "0.0"}}, - {"LiveDelay", {PERSISTENT, BYTES}}, - {"LiveParameters", {PERSISTENT, JSON}}, - {"LiveParametersV2", {PERSISTENT, BYTES}}, - {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}}, - {"LocationFilterInitialState", {PERSISTENT, BYTES}}, - {"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, - {"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast(cereal::LongitudinalPersonality::STANDARD))}}, - {"NetworkMetered", {PERSISTENT, BOOL}}, - {"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, - {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_ExcessiveActuation", {PERSISTENT, JSON}}, - {"Offroad_IsTakingSnapshot", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_NeosUpdate", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_NoFirmware", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, - {"Offroad_Recalibration", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, - {"Offroad_TemperatureTooHigh", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_UnregisteredHardware", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_UpdateFailed", {CLEAR_ON_MANAGER_START, JSON}}, - {"Offroad_DriverMonitoringUncertain", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, - {"OnroadCycleRequested", {CLEAR_ON_MANAGER_START, BOOL}}, - {"OpenpilotEnabledToggle", {PERSISTENT, BOOL, "1"}}, - {"PandaHeartbeatLost", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, - {"PandaSomResetTriggered", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, - {"PandaSignatures", {CLEAR_ON_MANAGER_START, BYTES}}, - {"PrimeType", {PERSISTENT, INT}}, - {"RecordAudio", {PERSISTENT, BOOL}}, - {"RecordAudioFeedback", {PERSISTENT, BOOL, "0"}}, - {"RecordFront", {PERSISTENT, BOOL}}, - {"RecordFrontLock", {PERSISTENT, BOOL}}, // for the internal fleet - {"SecOCKey", {PERSISTENT | DONT_LOG, STRING}}, - {"ShowDebugInfo", {PERSISTENT, BOOL}}, - {"RouteCount", {PERSISTENT, INT, "0"}}, - {"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, - {"SshEnabled", {PERSISTENT, BOOL}}, - {"UbloxAvailable", {PERSISTENT, BOOL}}, - {"UpdateAvailable", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, - {"UpdateFailedCount", {CLEAR_ON_MANAGER_START, INT}}, - {"UpdaterAvailableBranches", {PERSISTENT, STRING}}, - {"UpdaterCurrentDescription", {CLEAR_ON_MANAGER_START, STRING}}, - {"UpdaterCurrentReleaseNotes", {CLEAR_ON_MANAGER_START, BYTES}}, - {"UpdaterFetchAvailable", {CLEAR_ON_MANAGER_START, BOOL}}, - {"UpdaterNewDescription", {CLEAR_ON_MANAGER_START, STRING}}, - {"UpdaterNewReleaseNotes", {CLEAR_ON_MANAGER_START, BYTES}}, - {"UpdaterState", {CLEAR_ON_MANAGER_START, STRING}}, - {"UpdaterTargetBranch", {CLEAR_ON_MANAGER_START, STRING}}, - {"UpdaterLastFetchTime", {PERSISTENT, TIME}}, - {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}}, - {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}}, - {"Version", {PERSISTENT, STRING}}, -}; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx old mode 100644 new mode 100755 index 93c550f22a2c8a..8d52b8d3f6208f --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -1,94 +1,48 @@ # distutils: language = c++ # cython: language_level = 3 -import builtins -import datetime -import json from libcpp cimport bool from libcpp.string cimport string -from libcpp.vector cimport vector -from libcpp.optional cimport optional - -from openpilot.common.swaglog import cloudlog +import threading cdef extern from "common/params.h": - cpdef enum ParamKeyFlag: + cpdef enum ParamKeyType: PERSISTENT CLEAR_ON_MANAGER_START - CLEAR_ON_ONROAD_TRANSITION - CLEAR_ON_OFFROAD_TRANSITION - DEVELOPMENT_ONLY CLEAR_ON_IGNITION_ON + CLEAR_ON_IGNITION_OFF ALL - cpdef enum ParamKeyType: - STRING - BOOL - INT - FLOAT - TIME - JSON - BYTES - cdef cppclass c_Params "Params": - c_Params(string) except + nogil + c_Params(string) nogil string get(string, bool) nogil - bool getBool(string, bool) nogil + bool getBool(string) nogil int remove(string) nogil int put(string, string) nogil - void putNonBlocking(string, string) nogil - void putBoolNonBlocking(string, bool) nogil int putBool(string, bool) nogil bool checkKey(string) nogil - ParamKeyType getKeyType(string) nogil - optional[string] getKeyDefaultValue(string) nogil string getParamPath(string) nogil - void clearAll(ParamKeyFlag) - vector[string] allKeys() - -PYTHON_2_CPP = { - (str, STRING): lambda v: v, - (builtins.bool, BOOL): lambda v: "1" if v else "0", - (int, INT): str, - (float, FLOAT): str, - (datetime.datetime, TIME): lambda v: v.isoformat(), - (dict, JSON): json.dumps, - (list, JSON): json.dumps, - (bytes, BYTES): lambda v: v, -} -CPP_2_PYTHON = { - STRING: lambda v: v.decode("utf-8"), - BOOL: lambda v: v == b"1", - INT: int, - FLOAT: float, - TIME: lambda v: datetime.datetime.fromisoformat(v.decode("utf-8")), - JSON: json.loads, - BYTES: lambda v: v, -} + void clearAll(ParamKeyType) + def ensure_bytes(v): - return v.encode() if isinstance(v, str) else v + return v.encode() if isinstance(v, str) else v; class UnknownKeyName(Exception): pass cdef class Params: cdef c_Params* p - cdef str d def __cinit__(self, d=""): cdef string path = d.encode() with nogil: self.p = new c_Params(path) - self.d = d - - def __reduce__(self): - return (type(self), (self.d,)) def __dealloc__(self): del self.p - def clear_all(self, tx_flag=ParamKeyFlag.ALL): - self.p.clearAll(tx_flag) + def clear_all(self, tx_type=ParamKeyType.ALL): + self.p.clearAll(tx_type) def check_key(self, key): key = ensure_bytes(key) @@ -96,60 +50,38 @@ cdef class Params: raise UnknownKeyName(key) return key - def python2cpp(self, proposed_type, expected_type, value, key): - cast = PYTHON_2_CPP.get((proposed_type, expected_type)) - if cast: - return cast(value) - raise TypeError(f"Type mismatch while writing param {key}: {proposed_type=} {expected_type=} {value=}") - - def _cpp2python(self, t, value, default, key): - if value is None: - return None - try: - return CPP_2_PYTHON[t](value) - except (KeyError, TypeError, ValueError): - cloudlog.warning(f"Failed to cast param {key} with {value=} from type {t=}") - return self._cpp2python(t, default, None, key) - - def get(self, key, bool block=False, bool return_default=False): + def get(self, key, bool block=False, encoding=None): cdef string k = self.check_key(key) - cdef ParamKeyType t = self.p.getKeyType(k) - cdef optional[string] default = self.p.getKeyDefaultValue(k) cdef string val with nogil: val = self.p.get(k, block) - default_val = (default.value() if default.has_value() else None) if return_default else None if val == b"": if block: # If we got no value while running in blocked mode # it means we got an interrupt while waiting raise KeyboardInterrupt else: - return self._cpp2python(t, default_val, None, key) - return self._cpp2python(t, val, default_val, key) + return None + + return val if encoding is None else val.decode(encoding) - def get_bool(self, key, bool block=False): + def get_bool(self, key): cdef string k = self.check_key(key) cdef bool r with nogil: - r = self.p.getBool(k, block) + r = self.p.getBool(k) return r - def _put_cast(self, key, dat): - cdef string k = self.check_key(key) - cdef ParamKeyType t = self.p.getKeyType(k) - return ensure_bytes(self.python2cpp(type(dat), t, dat, key)) - def put(self, key, dat): """ Warning: This function blocks until the param is written to disk! In very rare cases this can take over a second, and your code will hang. - Use the put_nonblocking, put_bool_nonblocking in time sensitive code, but + Use the put_nonblocking helper function in time sensitive code, but in general try to avoid writing params as much as possible. """ cdef string k = self.check_key(key) - cdef string dat_bytes = self._put_cast(key, dat) + cdef string dat_bytes = ensure_bytes(dat) with nogil: self.p.put(k, dat_bytes) @@ -158,17 +90,6 @@ cdef class Params: with nogil: self.p.putBool(k, val) - def put_nonblocking(self, key, dat): - cdef string k = self.check_key(key) - cdef string dat_bytes = self._put_cast(key, dat) - with nogil: - self.p.putNonBlocking(k, dat_bytes) - - def put_bool_nonblocking(self, key, bool val): - cdef string k = self.check_key(key) - with nogil: - self.p.putBoolNonBlocking(k, val) - def remove(self, key): cdef string k = self.check_key(key) with nogil: @@ -178,19 +99,8 @@ cdef class Params: cdef string key_bytes = ensure_bytes(key) return self.p.getParamPath(key_bytes).decode("utf-8") - def get_type(self, key): - return self.p.getKeyType(self.check_key(key)) +def put_nonblocking(key, val, d=""): + threading.Thread(target=lambda: Params(d).put(key, val)).start() - def all_keys(self): - return self.p.allKeys() - - def get_default_value(self, key): - cdef string k = self.check_key(key) - cdef ParamKeyType t = self.p.getKeyType(k) - cdef optional[string] default = self.p.getKeyDefaultValue(k) - return self._cpp2python(t, default.value(), None, key) if default.has_value() else None - - def cpp2python(self, key, value): - cdef string k = self.check_key(key) - cdef ParamKeyType t = self.p.getKeyType(k) - return self._cpp2python(t, value, None, key) +def put_bool_nonblocking(key, bool val, d=""): + threading.Thread(target=lambda: Params(d).put_bool(key, val)).start() diff --git a/common/pid.py b/common/pid.py deleted file mode 100644 index b3d64d6fcdc2d6..00000000000000 --- a/common/pid.py +++ /dev/null @@ -1,57 +0,0 @@ -import numpy as np -from numbers import Number - -class PIDController: - def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): - self._k_p: list[list[float]] = [[0], [k_p]] if isinstance(k_p, Number) else k_p - self._k_i: list[list[float]] = [[0], [k_i]] if isinstance(k_i, Number) else k_i - self._k_d: list[list[float]] = [[0], [k_d]] if isinstance(k_d, Number) else k_d - - self.set_limits(pos_limit, neg_limit) - - self.i_dt = 1.0 / rate - self.speed = 0.0 - - self.reset() - - @property - def k_p(self): - return np.interp(self.speed, self._k_p[0], self._k_p[1]) - - @property - def k_i(self): - return np.interp(self.speed, self._k_i[0], self._k_i[1]) - - @property - def k_d(self): - return np.interp(self.speed, self._k_d[0], self._k_d[1]) - - def reset(self): - self.p = 0.0 - self.i = 0.0 - self.d = 0.0 - self.f = 0.0 - self.control = 0 - - def set_limits(self, pos_limit, neg_limit): - self.pos_limit = pos_limit - self.neg_limit = neg_limit - - def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): - self.speed = speed - self.p = self.k_p * float(error) - self.d = self.k_d * error_rate - self.f = feedforward - - if not freeze_integrator: - i = self.i + self.k_i * self.i_dt * error - - # Don't allow windup if already clipping - test_control = self.p + i + self.d + self.f - i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit - i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit - self.i = np.clip(i, i_lowerbound, i_upperbound) - - control = self.p + self.i + self.d + self.f - self.control = np.clip(control, self.neg_limit, self.pos_limit) - return self.control diff --git a/common/prefix.h b/common/prefix.h deleted file mode 100644 index 30ee18f6375a7e..00000000000000 --- a/common/prefix.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include - -#include "common/params.h" -#include "common/util.h" -#include "system/hardware/hw.h" - -class OpenpilotPrefix { -public: - OpenpilotPrefix(std::string prefix = {}) { - if (prefix.empty()) { - prefix = util::random_string(15); - } -#ifdef __APPLE__ - msgq_path = "/tmp/msgq_" + prefix; -#else - msgq_path = "/dev/shm/msgq_" + prefix; -#endif - bool ret = util::create_directories(msgq_path, 0777); - assert(ret); - setenv("OPENPILOT_PREFIX", prefix.c_str(), 1); - } - - ~OpenpilotPrefix() { - auto param_path = Params().getParamPath(); - if (util::file_exists(param_path)) { - std::string real_path = util::readlink(param_path); - system(util::string_format("rm %s -rf", real_path.c_str()).c_str()); - unlink(param_path.c_str()); - } - if (getenv("COMMA_CACHE") == nullptr) { - system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()).c_str()); - } - system(util::string_format("rm %s -rf", Path::comma_home().c_str()).c_str()); - system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str()); - unsetenv("OPENPILOT_PREFIX"); - } - -private: - std::string msgq_path; -}; diff --git a/common/prefix.py b/common/prefix.py deleted file mode 100644 index d0a5f926286935..00000000000000 --- a/common/prefix.py +++ /dev/null @@ -1,61 +0,0 @@ -import os -import platform -import shutil -import uuid - - -from openpilot.common.params import Params -from openpilot.system.hardware import PC -from openpilot.system.hardware.hw import Paths -from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT - -class OpenpilotPrefix: - def __init__(self, prefix: str | None = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False): - self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15]) - shm_path = "/tmp" if platform.system() == "Darwin" else "/dev/shm" - self.msgq_path = os.path.join(shm_path, "msgq_" + self.prefix) - self.create_dirs_on_enter = create_dirs_on_enter - self.clean_dirs_on_exit = clean_dirs_on_exit - self.shared_download_cache = shared_download_cache - - def __enter__(self): - self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None) - os.environ['OPENPILOT_PREFIX'] = self.prefix - - if self.create_dirs_on_enter: - self.create_dirs() - - if self.shared_download_cache: - os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT - - return self - - def __exit__(self, exc_type, exc_obj, exc_tb): - if self.clean_dirs_on_exit: - self.clean_dirs() - try: - del os.environ['OPENPILOT_PREFIX'] - if self.original_prefix is not None: - os.environ['OPENPILOT_PREFIX'] = self.original_prefix - except KeyError: - pass - return False - - def create_dirs(self): - try: - os.mkdir(self.msgq_path) - except FileExistsError: - pass - os.makedirs(Paths.log_root(), exist_ok=True) - - def clean_dirs(self): - symlink_path = Params().get_param_path() - if os.path.exists(symlink_path): - shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True) - os.remove(symlink_path) - shutil.rmtree(self.msgq_path, ignore_errors=True) - if PC: - shutil.rmtree(Paths.log_root(), ignore_errors=True) - if not os.environ.get("COMMA_CACHE", False): - shutil.rmtree(Paths.download_cache_root(), ignore_errors=True) - shutil.rmtree(Paths.comma_home(), ignore_errors=True) diff --git a/common/profiler.py b/common/profiler.py new file mode 100644 index 00000000000000..8b1a7a8cfa53dd --- /dev/null +++ b/common/profiler.py @@ -0,0 +1,45 @@ +import time + +class Profiler(): + def __init__(self, enabled=False): + self.enabled = enabled + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.time() + self.last_time = self.start_time + self.tot = 0. + + def reset(self, enabled=False): + self.enabled = enabled + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.time() + self.last_time = self.start_time + + def checkpoint(self, name, ignore=False): + # ignore flag needed when benchmarking threads with ratekeeper + if not self.enabled: + return + tt = time.time() + if name not in self.cp: + self.cp[name] = 0. + if ignore: + self.cp_ignored.append(name) + self.cp[name] += tt - self.last_time + if not ignore: + self.tot += tt - self.last_time + self.last_time = tt + + def display(self): + if not self.enabled: + return + self.iter += 1 + print("******* Profiling %d *******" % self.iter) + for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]): + if n in self.cp_ignored: + print("%30s: %9.2f avg: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) + else: + print("%30s: %9.2f avg: %7.2f percent: %3.0f" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100)) + print(f"Iter clock: {self.tot / self.iter:2.6f} TOTAL: {self.tot:2.2f}") diff --git a/common/ratekeeper.cc b/common/ratekeeper.cc deleted file mode 100644 index 7e63815168d1e8..00000000000000 --- a/common/ratekeeper.cc +++ /dev/null @@ -1,40 +0,0 @@ -#include "common/ratekeeper.h" - -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold) - : name(name), - print_delay_threshold(std::max(0.f, print_delay_threshold)) { - interval = 1 / rate; - last_monitor_time = seconds_since_boot(); - next_frame_time = last_monitor_time + interval; -} - -bool RateKeeper::keepTime() { - bool lagged = monitorTime(); - if (remaining_ > 0) { - util::sleep_for(remaining_ * 1000); - } - return lagged; -} - -bool RateKeeper::monitorTime() { - ++frame_; - last_monitor_time = seconds_since_boot(); - remaining_ = next_frame_time - last_monitor_time; - - bool lagged = remaining_ < 0; - if (lagged) { - if (print_delay_threshold > 0 && remaining_ < -print_delay_threshold) { - LOGW("%s lagging by %.2f ms", name.c_str(), -remaining_ * 1000); - } - next_frame_time = last_monitor_time + interval; - } else { - next_frame_time += interval; - } - return lagged; -} diff --git a/common/ratekeeper.h b/common/ratekeeper.h deleted file mode 100644 index e7323c6ec3428e..00000000000000 --- a/common/ratekeeper.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include -#include - -class RateKeeper { -public: - RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0); - ~RateKeeper() {} - bool keepTime(); - bool monitorTime(); - inline uint64_t frame() const { return frame_; } - inline double remaining() const { return remaining_; } - -private: - double interval; - double next_frame_time; - double last_monitor_time; - double remaining_ = 0; - float print_delay_threshold = 0; - uint64_t frame_ = 0; - std::string name; -}; diff --git a/common/realtime.py b/common/realtime.py index 0b14681021c838..8a79d8d39fb099 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -1,19 +1,20 @@ """Utilities for reading real time clocks and keeping soft real time constraints.""" import gc import os -import sys import time +from collections import deque +from typing import Optional, List, Union -from setproctitle import getproctitle +from setproctitle import getproctitle # pylint: disable=no-name-in-module -from openpilot.common.utils import MovingAverage -from openpilot.system.hardware import PC +from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error +from system.hardware import PC # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model -DT_HW = 0.5 # hardwared and manager +DT_TRML = 0.5 # thermald and manager DT_DMON = 0.05 # driver monitoring @@ -24,36 +25,38 @@ class Priority: CTRL_LOW = 51 # plannerd & radard # CORE 3 - # - pandad = 55 + # - boardd = 55 CTRL_HIGH = 53 -def set_core_affinity(cores: list[int]) -> None: - if sys.platform == 'linux' and not PC: - os.sched_setaffinity(0, cores) +def set_realtime_priority(level: int) -> None: + if not PC: + os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # type: ignore[attr-defined] # pylint: disable=no-member -def config_realtime_process(cores: int | list[int], priority: int) -> None: +def set_core_affinity(cores: List[int]) -> None: + if not PC: + os.sched_setaffinity(0, cores) # pylint: disable=no-member + + +def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None: gc.disable() - if sys.platform == 'linux' and not PC: - os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority)) + set_realtime_priority(priority) c = cores if isinstance(cores, list) else [cores, ] set_core_affinity(c) class Ratekeeper: - def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None: + def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None: """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" self._interval = 1. / rate + self._next_frame_time = sec_since_boot() + self._interval self._print_delay_threshold = print_delay_threshold self._frame = 0 self._remaining = 0.0 self._process_name = getproctitle() - self._last_monitor_time = -1. - self._next_frame_time = -1. - - self.avg_dt = MovingAverage(100) - self.avg_dt.add_value(self._interval) + self._dts = deque([self._interval], maxlen=100) + self._last_monitor_time = sec_since_boot() @property def frame(self) -> int: @@ -65,8 +68,9 @@ def remaining(self) -> float: @property def lagging(self) -> bool: + avg_dt = sum(self._dts) / len(self._dts) expected_dt = self._interval * (1 / 0.9) - return self.avg_dt.get_average() > expected_dt + return avg_dt > expected_dt # Maintain loop rate by calling this at the end of each loop def keep_time(self) -> bool: @@ -75,18 +79,14 @@ def keep_time(self) -> bool: time.sleep(self._remaining) return lagged - # Monitors the cumulative lag, but does not enforce a rate + # this only monitor the cumulative lag, but does not enforce a rate def monitor_time(self) -> bool: - if self._last_monitor_time < 0: - self._next_frame_time = time.monotonic() + self._interval - self._last_monitor_time = time.monotonic() - prev = self._last_monitor_time - self._last_monitor_time = time.monotonic() - self.avg_dt.add_value(self._last_monitor_time - prev) + self._last_monitor_time = sec_since_boot() + self._dts.append(self._last_monitor_time - prev) lagged = False - remaining = self._next_frame_time - time.monotonic() + remaining = self._next_frame_time - sec_since_boot() self._next_frame_time += self._interval if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold: print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms") diff --git a/common/simple_kalman.py b/common/simple_kalman.py deleted file mode 100644 index 194b27204bd511..00000000000000 --- a/common/simple_kalman.py +++ /dev/null @@ -1,54 +0,0 @@ -import numpy as np - - -def get_kalman_gain(dt, A, C, Q, R, iterations=100): - P = np.zeros_like(Q) - for _ in range(iterations): - P = A.dot(P).dot(A.T) + dt * Q - S = C.dot(P).dot(C.T) + R - K = P.dot(C.T).dot(np.linalg.inv(S)) - P = (np.eye(len(P)) - K.dot(C)).dot(P) - return K - - -class KF1D: - # this EKF assumes constant covariance matrix, so calculations are much simpler - # the Kalman gain also needs to be precomputed using the control module - - def __init__(self, x0, A, C, K): - self.x0_0 = x0[0][0] - self.x1_0 = x0[1][0] - self.A0_0 = A[0][0] - self.A0_1 = A[0][1] - self.A1_0 = A[1][0] - self.A1_1 = A[1][1] - self.C0_0 = C[0] - self.C0_1 = C[1] - self.K0_0 = K[0][0] - self.K1_0 = K[1][0] - - self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 - self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 - self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 - self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 - - # K matrix needs to be pre-computed as follow: - # import control - # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) - # self.K = np.transpose(K) - - def update(self, meas): - #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) - x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas - x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas - self.x0_0 = x0_0 - self.x1_0 = x1_0 - return [self.x0_0, self.x1_0] - - @property - def x(self): - return [[self.x0_0], [self.x1_0]] - - def set_x(self, x): - self.x0_0 = x[0][0] - self.x1_0 = x[1][0] diff --git a/common/spinner.py b/common/spinner.py old mode 100755 new mode 100644 index 12a816eaf8d729..57242d644dd6a3 --- a/common/spinner.py +++ b/common/spinner.py @@ -1,14 +1,14 @@ import os import subprocess -from openpilot.common.basedir import BASEDIR +from common.basedir import BASEDIR -class Spinner: +class Spinner(): def __init__(self): try: - self.spinner_proc = subprocess.Popen(["./spinner.py"], + self.spinner_proc = subprocess.Popen(["./spinner"], stdin=subprocess.PIPE, - cwd=os.path.join(BASEDIR, "system", "ui"), + cwd=os.path.join(BASEDIR, "selfdrive", "ui"), close_fds=True) except OSError: self.spinner_proc = None @@ -29,11 +29,11 @@ def update_progress(self, cur: float, total: float): def close(self): if self.spinner_proc is not None: - self.spinner_proc.kill() try: - self.spinner_proc.communicate(timeout=2.) - except subprocess.TimeoutExpired: - print("WARNING: failed to kill spinner") + self.spinner_proc.stdin.close() + except BrokenPipeError: + pass + self.spinner_proc.terminate() self.spinner_proc = None def __del__(self): diff --git a/common/stat_live.py b/common/stat_live.py index 3901c448d8273d..a91c1819bb9149 100644 --- a/common/stat_live.py +++ b/common/stat_live.py @@ -1,6 +1,6 @@ import numpy as np -class RunningStat: +class RunningStat(): # tracks realtime mean and standard deviation without storing any data def __init__(self, priors=None, max_trackable=-1): self.max_trackable = max_trackable @@ -51,7 +51,7 @@ def std(self): def params_to_save(self): return [self.M, self.S, self.n] -class RunningStatFilter: +class RunningStatFilter(): def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1): self.raw_stat = RunningStat(raw_priors, -1) self.filtered_stat = RunningStat(filtered_priors, max_trackable) diff --git a/common/statlog.cc b/common/statlog.cc new file mode 100644 index 00000000000000..26945882d9d1a9 --- /dev/null +++ b/common/statlog.cc @@ -0,0 +1,46 @@ +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include "common/statlog.h" +#include "common/util.h" + +#include +#include +#include + +class StatlogState : public LogState { + public: + StatlogState() : LogState("ipc:///tmp/stats") {} +}; + +static StatlogState s = {}; + +static void log(const char* metric_type, const char* metric, const char* fmt, ...) { + std::lock_guard lk(s.lock); + if (!s.initialized) s.initialize(); + + char* value_buf = nullptr; + va_list args; + va_start(args, fmt); + int ret = vasprintf(&value_buf, fmt, args); + va_end(args); + + if (ret > 0 && value_buf) { + char* line_buf = nullptr; + ret = asprintf(&line_buf, "%s:%s|%s", metric, value_buf, metric_type); + if (ret > 0 && line_buf) { + zmq_send(s.sock, line_buf, ret, ZMQ_NOBLOCK); + free(line_buf); + } + free(value_buf); + } +} + +void statlog_log(const char* metric_type, const char* metric, int value) { + log(metric_type, metric, "%d", value); +} + +void statlog_log(const char* metric_type, const char* metric, float value) { + log(metric_type, metric, "%f", value); +} diff --git a/common/statlog.h b/common/statlog.h new file mode 100644 index 00000000000000..5d223bb666dc75 --- /dev/null +++ b/common/statlog.h @@ -0,0 +1,10 @@ +#pragma once + +#define STATLOG_GAUGE "g" +#define STATLOG_SAMPLE "sa" + +void statlog_log(const char* metric_type, const char* metric, int value); +void statlog_log(const char* metric_type, const char* metric, float value); + +#define statlog_gauge(metric, value) statlog_log(STATLOG_GAUGE, metric, value) +#define statlog_sample(metric, value) statlog_log(STATLOG_SAMPLE, metric, value) diff --git a/common/swaglog.cc b/common/swaglog.cc index 62a405a2b6e244..22682dc54c57b6 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -5,32 +5,29 @@ #include "common/swaglog.h" #include +#include #include #include #include #include -#include -#include "third_party/json11/json11.hpp" +#include "json11.hpp" + +#include "common/util.h" #include "common/version.h" #include "system/hardware/hw.h" -class SwaglogState { -public: - SwaglogState() { - zctx = zmq_ctx_new(); - sock = zmq_socket(zctx, ZMQ_PUSH); - - // Timeout on shutdown for messages to be received by the logging process - int timeout = 100; - zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout)); - zmq_connect(sock, Path::swaglog_ipc().c_str()); +class SwaglogState : public LogState { + public: + SwaglogState() : LogState("ipc:///tmp/logmessage") {} - // workaround for https://github.com/dropbox/json11/issues/38 - setlocale(LC_NUMERIC, "C"); + json11::Json::object ctx_j; + inline void initialize() { + ctx_j = json11::Json::object {}; print_level = CLOUDLOG_WARNING; - if (const char* print_lvl = getenv("LOGPRINT")) { + const char* print_lvl = getenv("LOGPRINT"); + if (print_lvl) { if (strcmp(print_lvl, "debug") == 0) { print_level = CLOUDLOG_DEBUG; } else if (strcmp(print_lvl, "info") == 0) { @@ -40,53 +37,40 @@ class SwaglogState { } } - ctx_j = json11::Json::object{}; - if (char* dongle_id = getenv("DONGLE_ID")) { + // openpilot bindings + char* dongle_id = getenv("DONGLE_ID"); + if (dongle_id) { ctx_j["dongle_id"] = dongle_id; } - if (char* git_origin = getenv("GIT_ORIGIN")) { - ctx_j["origin"] = git_origin; - } - if (char* git_branch = getenv("GIT_BRANCH")) { - ctx_j["branch"] = git_branch; - } - if (char* git_commit = getenv("GIT_COMMIT")) { - ctx_j["commit"] = git_commit; - } - if (char* daemon_name = getenv("MANAGER_DAEMON")) { + char* daemon_name = getenv("MANAGER_DAEMON"); + if (daemon_name) { ctx_j["daemon"] = daemon_name; } ctx_j["version"] = COMMA_VERSION; ctx_j["dirty"] = !getenv("CLEAN"); - ctx_j["device"] = Hardware::get_name(); - } - - ~SwaglogState() { - zmq_close(sock); - zmq_ctx_destroy(zctx); - } - void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { - std::lock_guard lk(lock); - if (levelnum >= print_level) { - printf("%s: %s\n", filename, msg); - } - zmq_send(sock, log_s.data(), log_s.length(), ZMQ_NOBLOCK); + // device type + ctx_j["device"] = Hardware::get_name(); + LogState::initialize(); } - - std::mutex lock; - void* zctx = nullptr; - void* sock = nullptr; - int print_level; - json11::Json::object ctx_j; }; +static SwaglogState s = {}; bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS"); uint32_t NO_FRAME_ID = std::numeric_limits::max(); +static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { + if (levelnum >= s.print_level) { + printf("%s: %s\n", filename, msg); + } + char levelnum_c = levelnum; + zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK); +} + static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func, char* msg_buf, const json11::Json::object &msg_j={}) { - static SwaglogState s; + std::lock_guard lk(s.lock); + if (!s.initialized) s.initialize(); json11::Json::object log_j = json11::Json::object { {"ctx", s.ctx_j}, @@ -102,11 +86,8 @@ static void cloudlog_common(int levelnum, const char* filename, int lineno, cons log_j["msg"] = msg_j; } - std::string log_s; - log_s += (char)levelnum; - ((json11::Json)log_j).dump(log_s); - s.log(levelnum, filename, lineno, func, msg_buf, log_s); - + std::string log_s = ((json11::Json)log_j).dump(); + log(levelnum, filename, lineno, func, msg_buf, log_s); free(msg_buf); } @@ -153,3 +134,4 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args); va_end(args); } + diff --git a/common/swaglog.h b/common/swaglog.h index 06d45b1d981af0..68b05ed2e95946 100644 --- a/common/swaglog.h +++ b/common/swaglog.h @@ -9,29 +9,23 @@ #define CLOUDLOG_CRITICAL 50 -#ifdef __GNUC__ -#define SWAG_LOG_CHECK_FMT(a, b) __attribute__ ((format (printf, a, b))) -#else -#define SWAG_LOG_CHECK_FMT(a, b) -#endif - void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, - const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6); + const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, - const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6); + const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func, - uint32_t frame_id, const char* fmt, ...) SWAG_LOG_CHECK_FMT(6, 7); + uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \ __func__, \ - fmt, ## __VA_ARGS__) - + fmt, ## __VA_ARGS__); + #define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \ __func__, \ - __VA_ARGS__) + __VA_ARGS__); #define cloudlog_rl(burst, millis, lvl, fmt, ...) \ @@ -44,7 +38,7 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun int __millis = (millis); \ uint64_t __ts = nanos_since_boot(); \ \ - if (!__begin) { __begin = __ts; } \ + if (!__begin) __begin = __ts; \ \ if (__begin + __millis*1000000ULL < __ts) { \ if (__missed) { \ diff --git a/common/tests/.gitignore b/common/tests/.gitignore index 6cddfc7bdf0936..1350b3b825c050 100644 --- a/common/tests/.gitignore +++ b/common/tests/.gitignore @@ -1 +1,2 @@ -test_common +test_util +test_swaglog diff --git a/common/tests/test_file_helpers.py b/common/tests/test_file_helpers.py index c2b880f873d1e4..d39e66de135473 100644 --- a/common/tests/test_file_helpers.py +++ b/common/tests/test_file_helpers.py @@ -1,19 +1,27 @@ import os +import unittest from uuid import uuid4 -from openpilot.common.utils import atomic_write +from common.file_helpers import atomic_write_on_fs_tmp +from common.file_helpers import atomic_write_in_dir -class TestFileHelpers: +class TestFileHelpers(unittest.TestCase): def run_atomic_write_func(self, atomic_write_func): path = f"/tmp/tmp{uuid4()}" with atomic_write_func(path) as f: f.write("test") - assert not os.path.exists(path) with open(path) as f: - assert f.read() == "test" + self.assertEqual(f.read(), "test") os.remove(path) - def test_atomic_write(self): - self.run_atomic_write_func(atomic_write) + def test_atomic_write_on_fs_tmp(self): + self.run_atomic_write_func(atomic_write_on_fs_tmp) + + def test_atomic_write_in_dir(self): + self.run_atomic_write_func(atomic_write_in_dir) + + +if __name__ == "__main__": + unittest.main() diff --git a/common/tests/test_markdown.py b/common/tests/test_markdown.py deleted file mode 100644 index d3c7e02c69810e..00000000000000 --- a/common/tests/test_markdown.py +++ /dev/null @@ -1,15 +0,0 @@ -import os - -from openpilot.common.basedir import BASEDIR -from openpilot.common.markdown import parse_markdown - - -class TestMarkdown: - def test_all_release_notes(self): - with open(os.path.join(BASEDIR, "RELEASES.md")) as f: - release_notes = f.read().split("\n\n") - assert len(release_notes) > 10 - - for rn in release_notes: - md = parse_markdown(rn) - assert len(md) > 0 diff --git a/common/tests/test_numpy_fast.py b/common/tests/test_numpy_fast.py new file mode 100644 index 00000000000000..2fb8a1cef34b8b --- /dev/null +++ b/common/tests/test_numpy_fast.py @@ -0,0 +1,26 @@ +import numpy as np +import unittest + +from common.numpy_fast import interp + + +class InterpTest(unittest.TestCase): + def test_correctness_controls(self): + _A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.]) + _A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30]) + v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39, + 39.999999, 40, 41] + + expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + actual = interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + + np.testing.assert_equal(actual, expected) + + for v_ego in v_ego_arr: + expected = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + actual = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + np.testing.assert_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/common/tests/test_params.cc b/common/tests/test_params.cc deleted file mode 100644 index f8d6c79f552a2b..00000000000000 --- a/common/tests/test_params.cc +++ /dev/null @@ -1,27 +0,0 @@ -#include "catch2/catch.hpp" -#define private public -#include "common/params.h" -#include "common/util.h" - -TEST_CASE("params_nonblocking_put") { - char tmp_path[] = "/tmp/asyncWriter_XXXXXX"; - const std::string param_path = mkdtemp(tmp_path); - auto param_names = {"CarParams", "IsMetric"}; - { - Params params(param_path); - for (const auto &name : param_names) { - params.putNonBlocking(name, "1"); - // param is empty - REQUIRE(params.get(name).empty()); - } - - // check if thread is running - REQUIRE(params.future.valid()); - REQUIRE(params.future.wait_for(std::chrono::milliseconds(0)) == std::future_status::timeout); - } - // check results - Params p(param_path); - for (const auto &name : param_names) { - REQUIRE(p.get(name) == "1"); - } -} diff --git a/common/tests/test_params.py b/common/tests/test_params.py index 592bf2c4b24cbc..899a47fe34ed58 100644 --- a/common/tests/test_params.py +++ b/common/tests/test_params.py @@ -1,19 +1,23 @@ -import pytest -import datetime -import os import threading import time -import uuid +import tempfile +import shutil +import unittest -from openpilot.common.params import Params, ParamKeyFlag, UnknownKeyName +from common.params import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking -class TestParams: - def setup_method(self): - self.params = Params() +class TestParams(unittest.TestCase): + def setUp(self): + self.tmpdir = tempfile.mkdtemp() + print("using", self.tmpdir) + self.params = Params(self.tmpdir) + + def tearDown(self): + shutil.rmtree(self.tmpdir) def test_params_put_and_get(self): self.params.put("DongleId", "cb38263377b873ee") - assert self.params.get("DongleId") == "cb38263377b873ee" + assert self.params.get("DongleId") == b"cb38263377b873ee" def test_params_non_ascii(self): st = b"\xe1\x90\xff" @@ -21,45 +25,38 @@ def test_params_non_ascii(self): assert self.params.get("CarParams") == st def test_params_get_cleared_manager_start(self): - self.params.put("CarParams", b"test") + self.params.put("CarParams", "test") self.params.put("DongleId", "cb38263377b873ee") assert self.params.get("CarParams") == b"test" - - undefined_param = self.params.get_param_path(uuid.uuid4().hex) - with open(undefined_param, "w") as f: - f.write("test") - assert os.path.isfile(undefined_param) - - self.params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) + self.params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) assert self.params.get("CarParams") is None assert self.params.get("DongleId") is not None - assert not os.path.isfile(undefined_param) def test_params_two_things(self): self.params.put("DongleId", "bob") - self.params.put("AthenadPid", 123) - assert self.params.get("DongleId") == "bob" - assert self.params.get("AthenadPid") == 123 + self.params.put("AthenadPid", "123") + assert self.params.get("DongleId") == b"bob" + assert self.params.get("AthenadPid") == b"123" def test_params_get_block(self): def _delayed_writer(): time.sleep(0.1) - self.params.put("CarParams", b"test") + self.params.put("CarParams", "test") threading.Thread(target=_delayed_writer).start() assert self.params.get("CarParams") is None - assert self.params.get("CarParams", block=True) == b"test" + assert self.params.get("CarParams", True) == b"test" def test_params_unknown_key_fails(self): - with pytest.raises(UnknownKeyName): + with self.assertRaises(UnknownKeyName): self.params.get("swag") - with pytest.raises(UnknownKeyName): + with self.assertRaises(UnknownKeyName): self.params.get_bool("swag") - with pytest.raises(UnknownKeyName): + with self.assertRaises(UnknownKeyName): self.params.put("swag", "abc") - with pytest.raises(UnknownKeyName): + with self.assertRaises(UnknownKeyName): self.params.put_bool("swag", True) def test_remove_not_there(self): @@ -69,73 +66,38 @@ def test_remove_not_there(self): def test_get_bool(self): self.params.remove("IsMetric") - assert not self.params.get_bool("IsMetric") + self.assertFalse(self.params.get_bool("IsMetric")) self.params.put_bool("IsMetric", True) - assert self.params.get_bool("IsMetric") + self.assertTrue(self.params.get_bool("IsMetric")) self.params.put_bool("IsMetric", False) - assert not self.params.get_bool("IsMetric") + self.assertFalse(self.params.get_bool("IsMetric")) - self.params.put("IsMetric", True) - assert self.params.get_bool("IsMetric") + self.params.put("IsMetric", "1") + self.assertTrue(self.params.get_bool("IsMetric")) - self.params.put("IsMetric", False) - assert not self.params.get_bool("IsMetric") + self.params.put("IsMetric", "0") + self.assertFalse(self.params.get_bool("IsMetric")) def test_put_non_blocking_with_get_block(self): - q = Params() + q = Params(self.tmpdir) def _delayed_writer(): time.sleep(0.1) - Params().put_nonblocking("CarParams", b"test") + put_nonblocking("CarParams", "test", self.tmpdir) threading.Thread(target=_delayed_writer).start() assert q.get("CarParams") is None assert q.get("CarParams", True) == b"test" def test_put_bool_non_blocking_with_get_block(self): - q = Params() + q = Params(self.tmpdir) def _delayed_writer(): time.sleep(0.1) - Params().put_bool_nonblocking("CarParams", True) + put_bool_nonblocking("CarParams", True, self.tmpdir) threading.Thread(target=_delayed_writer).start() assert q.get("CarParams") is None assert q.get("CarParams", True) == b"1" - def test_params_all_keys(self): - keys = Params().all_keys() - - # sanity checks - assert len(keys) > 20 - assert len(keys) == len(set(keys)) - assert b"CarParams" in keys - - def test_params_default_value(self): - self.params.remove("LanguageSetting") - self.params.remove("LongitudinalPersonality") - self.params.remove("LiveParameters") - - assert self.params.get("LanguageSetting") is None - assert self.params.get("LanguageSetting", return_default=False) is None - assert isinstance(self.params.get("LanguageSetting", return_default=True), str) - assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int) - assert self.params.get("LiveParameters") is None - assert self.params.get("LiveParameters", return_default=True) is None - - def test_params_get_type(self): - # json - self.params.put("ApiCache_FirehoseStats", {"a": 0}) - assert self.params.get("ApiCache_FirehoseStats") == {"a": 0} - - # int - self.params.put("BootCount", 1441) - assert self.params.get("BootCount") == 1441 - - # bool - self.params.put("AdbEnabled", True) - assert self.params.get("AdbEnabled") - assert isinstance(self.params.get("AdbEnabled"), bool) - - # time - now = datetime.datetime.now(datetime.UTC) - self.params.put("InstallDate", now) - assert self.params.get("InstallDate") == now + +if __name__ == "__main__": + unittest.main() diff --git a/common/tests/test_simple_kalman.py b/common/tests/test_simple_kalman.py deleted file mode 100644 index e44ac2cc57ca87..00000000000000 --- a/common/tests/test_simple_kalman.py +++ /dev/null @@ -1,29 +0,0 @@ -from openpilot.common.simple_kalman import KF1D - - -class TestSimpleKalman: - def setup_method(self): - dt = 0.01 - x0_0 = 0.0 - x1_0 = 0.0 - A0_0 = 1.0 - A0_1 = dt - A1_0 = 0.0 - A1_1 = 1.0 - C0_0 = 1.0 - C0_1 = 0.0 - K0_0 = 0.12287673 - K1_0 = 0.29666309 - - self.kf = KF1D(x0=[[x0_0], [x1_0]], - A=[[A0_0, A0_1], [A1_0, A1_1]], - C=[C0_0, C0_1], - K=[[K0_0], [K1_0]]) - - def test_getter_setter(self): - self.kf.set_x([[1.0], [1.0]]) - assert self.kf.x == [[1.0], [1.0]] - - def test_update_returns_state(self): - x = self.kf.update(100) - assert x == [i[0] for i in self.kf.x] diff --git a/common/tests/test_swaglog.cc b/common/tests/test_swaglog.cc index 09bc4c3795fd5e..20455ec74c7bbb 100644 --- a/common/tests/test_swaglog.cc +++ b/common/tests/test_swaglog.cc @@ -1,14 +1,15 @@ #include - #include - +#define CATCH_CONFIG_MAIN #include "catch2/catch.hpp" + +#include "json11.hpp" #include "common/swaglog.h" #include "common/util.h" #include "common/version.h" #include "system/hardware/hw.h" -#include "third_party/json11/json11.hpp" +const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage"; std::string daemon_name = "testy"; std::string dongle_id = "test_dongle_id"; int LINE_NO = 0; @@ -24,7 +25,7 @@ void log_thread(int thread_id, int msg_cnt) { void recv_log(int thread_cnt, int thread_msg_cnt) { void *zctx = zmq_ctx_new(); void *sock = zmq_socket(zctx, ZMQ_PULL); - zmq_bind(sock, Path::swaglog_ipc().c_str()); + zmq_bind(sock, SWAGLOG_ADDR); std::vector thread_msgs(thread_cnt); int total_count = 0; diff --git a/common/tests/test_util.cc b/common/tests/test_util.cc index de87fa3e0642ed..25ecf09aa9db83 100644 --- a/common/tests/test_util.cc +++ b/common/tests/test_util.cc @@ -5,10 +5,10 @@ #include #include -#include #include #include +#define CATCH_CONFIG_MAIN #include "catch2/catch.hpp" #include "common/util.h" @@ -38,8 +38,7 @@ TEST_CASE("util::read_file") { std::string content = random_bytes(64 * 1024); write(fd, content.c_str(), content.size()); std::string ret = util::read_file(filename); - bool equal = (ret == content); - REQUIRE(equal); + REQUIRE(ret == content); close(fd); } SECTION("read directory") { @@ -109,8 +108,7 @@ TEST_CASE("util::safe_fwrite") { REQUIRE(ret == 0); ret = fclose(f); REQUIRE(ret == 0); - bool equal = (dat == util::read_file(filename)); - REQUIRE(equal); + REQUIRE(dat == util::read_file(filename)); } TEST_CASE("util::create_directories") { diff --git a/common/text_window.py b/common/text_window.py index 358243d1f1fb9e..bea3a149f8d193 100755 --- a/common/text_window.py +++ b/common/text_window.py @@ -2,15 +2,15 @@ import os import time import subprocess -from openpilot.common.basedir import BASEDIR +from common.basedir import BASEDIR class TextWindow: def __init__(self, text): try: - self.text_proc = subprocess.Popen(["./text.py", text], + self.text_proc = subprocess.Popen(["./text", text], stdin=subprocess.PIPE, - cwd=os.path.join(BASEDIR, "system", "ui"), + cwd=os.path.join(BASEDIR, "selfdrive", "ui"), close_fds=True) except OSError: self.text_proc = None diff --git a/common/time_helpers.py b/common/time_helpers.py deleted file mode 100644 index 8564e270c2bcc6..00000000000000 --- a/common/time_helpers.py +++ /dev/null @@ -1,15 +0,0 @@ -import datetime -from pathlib import Path - -MIN_DATE = datetime.datetime(year=2025, month=2, day=21) - -def min_date(): - # on systemd systems, the default time is the systemd build time - systemd_path = Path("/lib/systemd/systemd") - if systemd_path.exists(): - d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime) - return max(MIN_DATE, d + datetime.timedelta(days=1)) - return MIN_DATE - -def system_time_valid(): - return datetime.datetime.now() > min_date() diff --git a/common/transformations/README.md b/common/transformations/README.md index 42a060da80bf32..13878cb99c26c8 100644 --- a/common/transformations/README.md +++ b/common/transformations/README.md @@ -11,7 +11,7 @@ by generating a rotation matrix and multiplying. | :-------------: |:-------------:| :-----:| :----: | | Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. | | ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83.
    This is the global Mesh3D frame. | -| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. | +| NED | [North, East, Down] | meters | Relative to earth's surface, useful for vizualizing. | | Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame.
    Relative to camera, **not imu.**
    ![img](http://upload.wikimedia.org/wikipedia/commons/thumb/2/2f/RPY_angles_of_airplanes.png/440px-RPY_angles_of_airplanes.png)| | Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in.
    More details below.
    | | Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road.
    More details below.
    | diff --git a/common/transformations/SConscript b/common/transformations/SConscript new file mode 100644 index 00000000000000..ee9b9a2b732fef --- /dev/null +++ b/common/transformations/SConscript @@ -0,0 +1,6 @@ +Import('env', 'envCython') + +transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc']) +Export('transformations') + +envCython.Program('transformations.so', 'transformations.pyx') diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 2e68b5e37c56c6..b20ed5c64b5797 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,74 +1,55 @@ -import itertools import numpy as np -from dataclasses import dataclass -import openpilot.common.transformations.orientation as orient +import common.transformations.orientation as orient ## -- hardcoded hardware params -- -@dataclass(frozen=True) -class CameraConfig: - width: int - height: int - focal_length: float - - @property - def size(self): - return (self.width, self.height) - - @property - def intrinsics(self): - # aka 'K' aka camera_frame_from_view_frame - return np.array([ - [self.focal_length, 0.0, float(self.width)/2], - [0.0, self.focal_length, float(self.height)/2], - [0.0, 0.0, 1.0] - ]) - - @property - def intrinsics_inv(self): - # aka 'K_inv' aka view_frame_from_camera_frame - return np.linalg.inv(self.intrinsics) - -@dataclass(frozen=True) -class _NoneCameraConfig(CameraConfig): - width: int = 0 - height: int = 0 - focal_length: float = 0 - -@dataclass(frozen=True) -class DeviceCameraConfig: - fcam: CameraConfig - dcam: CameraConfig - ecam: CameraConfig - - def all_cams(self): - for cam in ['fcam', 'dcam', 'ecam']: - if not isinstance(getattr(self, cam), _NoneCameraConfig): - yield cam, getattr(self, cam) - -_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame -_os_fisheye = CameraConfig(2688 // 2, 1520 // 2, 567.0 / 4 * 3) -_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye) -_os_config = DeviceCameraConfig(CameraConfig(2688 // 2, 1520 // 2, 1522.0 * 3 / 4), _os_fisheye, _os_fisheye) -_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig()) - -DEVICE_CAMERAS = { - # A "device camera" is defined by a device type and sensor - - # sensor type was never set on eon/neo/two - ("neo", "unknown"): _neo_config, - # unknown here is AR0231, field was added with OX03C10 support - ("tici", "unknown"): _ar_ox_config, - - # before deviceState.deviceType was set, assume tici AR config - ("unknown", "ar0231"): _ar_ox_config, - ("unknown", "ox03c10"): _ar_ox_config, - - # simulator (emulates a tici) - ("pc", "unknown"): _ar_ox_config, -} -prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config))) -DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods}) +eon_f_focal_length = 910.0 +eon_d_focal_length = 650.0 +tici_f_focal_length = 2648.0 +tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame + +eon_f_frame_size = (1164, 874) +eon_d_frame_size = (816, 612) +tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208) + +# aka 'K' aka camera_frame_from_view_frame +eon_fcam_intrinsics = np.array([ + [eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2], + [0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) +eon_intrinsics = eon_fcam_intrinsics # xx + +eon_dcam_intrinsics = np.array([ + [eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2], + [0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_fcam_intrinsics = np.array([ + [tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2], + [0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_dcam_intrinsics = np.array([ + [tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2], + [0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_ecam_intrinsics = tici_dcam_intrinsics + +# aka 'K_inv' aka view_frame_from_camera_frame +eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics) +eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx + +tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) +tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) + + +FULL_FRAME_SIZE = tici_f_frame_size +FOCAL = tici_f_focal_length +fcam_intrinsics = tici_fcam_intrinsics + +W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] + # device/mesh : x->forward, y-> right, z->down # view : x->right, y->down, z->forward @@ -80,6 +61,14 @@ def all_cams(self): view_frame_from_device_frame = device_frame_from_view_frame.T +def get_calib_from_vp(vp): + vp_norm = normalize(vp) + yaw_calib = np.arctan(vp_norm[0]) + pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) + roll_calib = 0 + return roll_calib, pitch_calib, yaw_calib + + # aka 'extrinsic_matrix' # road : x->forward, y -> left, z->up def get_view_frame_from_road_frame(roll, pitch, yaw, height): @@ -112,7 +101,7 @@ def roll_from_ke(m): -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) -def normalize(img_pts, intrinsics): +def normalize(img_pts, intrinsics=fcam_intrinsics): # normalizes image coordinates # accepts single pt or array of pts intrinsics_inv = np.linalg.inv(intrinsics) @@ -125,7 +114,7 @@ def normalize(img_pts, intrinsics): return img_pts_normalized[:, :2].reshape(input_shape) -def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf): +def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf): # denormalizes image coordinates # accepts single pt or array of pts img_pts = np.array(img_pts) @@ -142,14 +131,6 @@ def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf): return img_pts_denormalized[:, :2].reshape(input_shape) -def get_calib_from_vp(vp, intrinsics): - vp_norm = normalize(vp, intrinsics) - yaw_calib = np.arctan(vp_norm[0]) - pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) - roll_calib = 0 - return roll_calib, pitch_calib, yaw_calib - - def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef): # device from ecef frame # device frame is x -> forward, y-> right, z -> down diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc new file mode 100644 index 00000000000000..b729ac3d872bd2 --- /dev/null +++ b/common/transformations/coordinates.cc @@ -0,0 +1,102 @@ +#define _USE_MATH_DEFINES + +#include +#include +#include + +#include "coordinates.hpp" + + + +double a = 6378137; // lgtm [cpp/short-global-name] +double b = 6356752.3142; // lgtm [cpp/short-global-name] +double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name] +double e1sq = 6.73949674228 * 0.001; + + +static Geodetic to_degrees(Geodetic geodetic){ + geodetic.lat = RAD2DEG(geodetic.lat); + geodetic.lon = RAD2DEG(geodetic.lon); + return geodetic; +} + +static Geodetic to_radians(Geodetic geodetic){ + geodetic.lat = DEG2RAD(geodetic.lat); + geodetic.lon = DEG2RAD(geodetic.lon); + return geodetic; +} + + +ECEF geodetic2ecef(Geodetic g){ + g = to_radians(g); + double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2)); + double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon); + double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon); + double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat); + return {x, y, z}; +} + +Geodetic ecef2geodetic(ECEF e){ + // Convert from ECEF to geodetic using Ferrari's methods + // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution + double x = e.x; + double y = e.y; + double z = e.z; + + double r = sqrt(x * x + y * y); + double Esq = a * a - b * b; + double F = 54 * b * b * z * z; + double G = r * r + (1 - esq) * z * z - esq * Esq; + double C = (esq * esq * F * r * r) / (pow(G, 3)); + double S = cbrt(1 + C + sqrt(C * C + 2 * C)); + double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); + double Q = sqrt(1 + 2 * esq * esq * P); + double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r); + double U = sqrt(pow((r - esq * r_0), 2) + z * z); + double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z); + double Z_0 = b * b * z / (a * V); + double h = U * (1 - b * b / (a * V)); + + double lat = atan((z + e1sq * Z_0) / r); + double lon = atan2(y, x); + + return to_degrees({lat, lon, h}); +} + +LocalCoord::LocalCoord(Geodetic g, ECEF e){ + init_ecef << e.x, e.y, e.z; + + g = to_radians(g); + + ned2ecef_matrix << + -sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon), + -sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon), + cos(g.lat), 0, -sin(g.lat); + ecef2ned_matrix = ned2ecef_matrix.transpose(); +} + +NED LocalCoord::ecef2ned(ECEF e) { + Eigen::Vector3d ecef; + ecef << e.x, e.y, e.z; + + Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef)); + return {ned[0], ned[1], ned[2]}; +} + +ECEF LocalCoord::ned2ecef(NED n) { + Eigen::Vector3d ned; + ned << n.n, n.e, n.d; + + Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef; + return {ecef[0], ecef[1], ecef[2]}; +} + +NED LocalCoord::geodetic2ned(Geodetic g) { + ECEF e = ::geodetic2ecef(g); + return ecef2ned(e); +} + +Geodetic LocalCoord::ned2geodetic(NED n){ + ECEF e = ned2ecef(n); + return ::ecef2geodetic(e); +} diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp new file mode 100644 index 00000000000000..f5ba0d3fe7ab83 --- /dev/null +++ b/common/transformations/coordinates.hpp @@ -0,0 +1,41 @@ +#pragma once + +#define DEG2RAD(x) ((x) * M_PI / 180.0) +#define RAD2DEG(x) ((x) * 180.0 / M_PI) + +struct ECEF { + double x, y, z; + Eigen::Vector3d to_vector(){ + return Eigen::Vector3d(x, y, z); + } +}; + +struct NED { + double n, e, d; + Eigen::Vector3d to_vector(){ + return Eigen::Vector3d(n, e, d); + } +}; + +struct Geodetic { + double lat, lon, alt; + bool radians=false; +}; + +ECEF geodetic2ecef(Geodetic g); +Geodetic ecef2geodetic(ECEF e); + +class LocalCoord { +public: + Eigen::Matrix3d ned2ecef_matrix; + Eigen::Matrix3d ecef2ned_matrix; + Eigen::Vector3d init_ecef; + LocalCoord(Geodetic g, ECEF e); + LocalCoord(Geodetic g) : LocalCoord(g, ::geodetic2ecef(g)) {} + LocalCoord(ECEF e) : LocalCoord(::ecef2geodetic(e), e) {} + + NED ecef2ned(ECEF e); + ECEF ned2ecef(NED n); + NED geodetic2ned(Geodetic g); + Geodetic ned2geodetic(NED n); +}; diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py index 696e7de2e50dfa..46cc0ded0d91fe 100644 --- a/common/transformations/coordinates.py +++ b/common/transformations/coordinates.py @@ -1,7 +1,8 @@ -from openpilot.common.transformations.orientation import numpy_wrap -from openpilot.common.transformations.transformations import (ecef2geodetic_single, +# pylint: skip-file +from common.transformations.orientation import numpy_wrap +from common.transformations.transformations import (ecef2geodetic_single, geodetic2ecef_single) -from openpilot.common.transformations.transformations import LocalCoord as LocalCoord_single +from common.transformations.transformations import LocalCoord as LocalCoord_single class LocalCoord(LocalCoord_single): diff --git a/common/transformations/model.py b/common/transformations/model.py index ea1dff30e8fc4e..811a17eafe8c4d 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,11 +1,17 @@ import numpy as np -from openpilot.common.transformations.orientation import rot_from_euler -from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye +from common.transformations.camera import (FULL_FRAME_SIZE, + get_view_frame_from_calib_frame) # segnet SEGNET_SIZE = (512, 384) +def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE): + return np.array([[float(segnet_size[0]) / full_frame_size[0], 0.0], + [0.0, float(segnet_size[1]) / full_frame_size[1]]]) +segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx + + # MED model MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) @@ -39,13 +45,6 @@ [0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)], [0.0, 0.0, 1.0]]) -DM_INPUT_SIZE = (1440, 960) -dmonitoringmodel_fl = _ar_ox_fisheye.focal_length -dmonitoringmodel_intrinsics = np.array([ - [dmonitoringmodel_fl, 0.0, DM_INPUT_SIZE[0]/2], - [0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2], - [0.0, 0.0, 1.0]]) - bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics, get_view_frame_from_calib_frame(0, 0, 0, 0)) @@ -58,13 +57,61 @@ medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) -calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3]) -calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3]) -# This function is verified to give similar results to xx.uncommon.utils.transform_img -def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray: - calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel - device_from_calib = rot_from_euler(device_from_calib_euler) - camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib - warp_matrix: np.ndarray = camera_from_calib @ calib_from_model +### This function mimics the update_calibration logic in modeld.cc +### Manually verified to give similar results to xx.uncommon.utils.transform_img +def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True): + from common.transformations.orientation import rot_from_euler + from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics + + if tici and wide_cam: + intrinsics = tici_ecam_intrinsics + elif tici: + intrinsics = tici_fcam_intrinsics + else: + intrinsics = eon_fcam_intrinsics + + if big_model: + sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)] + calib_from_model = np.linalg.inv(sbigmodel_from_calib) + else: + medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0,1,2)] + calib_from_model = np.linalg.inv(medmodel_from_calib) + device_from_calib = rot_from_euler(rpy_calib) + camera_from_calib = intrinsics.dot(view_frame_from_device_frame.dot(device_from_calib)) + warp_matrix = camera_from_calib.dot(calib_from_model) + return warp_matrix + + +### This is old, just for debugging +def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True): + from common.transformations.orientation import rot_from_euler + from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics + + + def get_view_frame_from_road_frame(roll, pitch, yaw, height): + device_from_road = rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) + view_from_road = view_frame_from_device_frame.dot(device_from_road) + return np.hstack((view_from_road, [[0], [height], [0]])) + + if tici and wide_cam: + intrinsics = tici_ecam_intrinsics + elif tici: + intrinsics = tici_fcam_intrinsics + else: + intrinsics = eon_fcam_intrinsics + + model_height = 1.22 + if big_model: + model_from_road = np.dot(sbigmodel_intrinsics, + get_view_frame_from_road_frame(0, 0, 0, model_height)) + else: + model_from_road = np.dot(medmodel_intrinsics, + get_view_frame_from_road_frame(0, 0, 0, model_height)) + ground_from_model = np.linalg.inv(model_from_road[:, (0, 1, 3)]) + + E = get_view_frame_from_road_frame(*rpy_calib, 1.22) + camera_frame_from_road_frame = intrinsics.dot(E) + camera_frame_from_ground = camera_frame_from_road_frame[:,(0,1,3)] + warp_matrix = camera_frame_from_ground .dot(ground_from_model) return warp_matrix diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc new file mode 100644 index 00000000000000..7909c0affba553 --- /dev/null +++ b/common/transformations/orientation.cc @@ -0,0 +1,144 @@ +#define _USE_MATH_DEFINES + +#include +#include +#include + +#include "orientation.hpp" +#include "coordinates.hpp" + +Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){ + if (quat.w() > 0){ + return quat; + } else { + return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z()); + } +} + +Eigen::Quaterniond euler2quat(Eigen::Vector3d euler){ + Eigen::Quaterniond q; + + q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX()); + return ensure_unique(q); +} + + +Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){ + // TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore + // Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); + // return {euler(2), euler(1), euler(0)}; + double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y())); + double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0); + double theta = asin(asin_arg_clipped); + double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z())); + return {gamma, theta, psi}; +} + +Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat){ + return quat.toRotationMatrix(); +} + +Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot){ + return ensure_unique(Eigen::Quaterniond(rot)); +} + +Eigen::Matrix3d euler2rot(Eigen::Vector3d euler){ + return quat2rot(euler2quat(euler)); +} + +Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot){ + return quat2euler(rot2quat(rot)); +} + +Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw){ + return euler2rot({roll, pitch, yaw}); +} + +Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle){ + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(angle, axis); + return q.toRotationMatrix(); +} + + +Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose) { + /* + Using Rotations to Build Aerospace Coordinate Systems + Don Koks + https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf + */ + LocalCoord converter = LocalCoord(ecef_init); + Eigen::Vector3d zero = ecef_init.to_vector(); + + Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero; + Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero; + Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero; + + Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0; + Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0; + Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0; + + Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1; + Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1; + Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1; + + Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2; + Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2; + + + x0 = Eigen::Vector3d(1, 0, 0); + y0 = Eigen::Vector3d(0, 1, 0); + z0 = Eigen::Vector3d(0, 0, 1); + + double psi = atan2(x3.dot(y0), x3.dot(x0)); + double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2))); + + y2 = rot(z0, psi) * y0; + z2 = rot(y2, theta) * z0; + + double phi = atan2(y3.dot(z2), y3.dot(y2)); + + return {phi, theta, psi}; +} + +Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){ + /* + Using Rotations to Build Aerospace Coordinate Systems + Don Koks + https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf + */ + LocalCoord converter = LocalCoord(ecef_init); + + Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0); + Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0); + Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1); + + Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0; + Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0; + Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0; + + Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1; + Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1; + Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1; + + Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2; + Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2; + + Eigen::Vector3d zero = ecef_init.to_vector(); + x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero; + y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero; + z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero; + + double psi = atan2(x3.dot(y0), x3.dot(x0)); + double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2))); + + y2 = rot(z0, psi) * y0; + z2 = rot(y2, theta) * z0; + + double phi = atan2(y3.dot(z2), y3.dot(y2)); + + return {phi, theta, psi}; +} + diff --git a/common/transformations/orientation.hpp b/common/transformations/orientation.hpp new file mode 100644 index 00000000000000..ebd7da0aeed2fc --- /dev/null +++ b/common/transformations/orientation.hpp @@ -0,0 +1,17 @@ +#pragma once +#include +#include "coordinates.hpp" + + +Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat); + +Eigen::Quaterniond euler2quat(Eigen::Vector3d euler); +Eigen::Vector3d quat2euler(Eigen::Quaterniond quat); +Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat); +Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot); +Eigen::Matrix3d euler2rot(Eigen::Vector3d euler); +Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot); +Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw); +Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle); +Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose); +Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose); diff --git a/common/transformations/orientation.py b/common/transformations/orientation.py index 86e6a6c34723be..134442b6240117 100644 --- a/common/transformations/orientation.py +++ b/common/transformations/orientation.py @@ -1,7 +1,8 @@ +# pylint: skip-file import numpy as np -from collections.abc import Callable +from typing import Callable -from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single, +from common.transformations.transformations import (ecef_euler_from_ned_single, euler2quat_single, euler2rot_single, ned_euler_from_ecef_single, diff --git a/common/transformations/tests/test_coordinates.py b/common/transformations/tests/test_coordinates.py old mode 100644 new mode 100755 index 0b5d1c36dfb0c2..dc70faed0bd50a --- a/common/transformations/tests/test_coordinates.py +++ b/common/transformations/tests/test_coordinates.py @@ -1,6 +1,9 @@ +#!/usr/bin/env python3 + import numpy as np +import unittest -import openpilot.common.transformations.coordinates as coord +import common.transformations.coordinates as coord geodetic_positions = np.array([[37.7610403, -122.4778699, 115], [27.4840915, -68.5867592, 2380], @@ -41,7 +44,7 @@ [ 78.56272609, 18.53100158, -43.25290759]]) -class TestNED: +class TestNED(unittest.TestCase): def test_small_distances(self): start_geodetic = np.array([33.8042184, -117.888593, 0.0]) local_coord = coord.LocalCoord.from_geodetic(start_geodetic) @@ -51,13 +54,13 @@ def test_small_distances(self): west_geodetic = start_geodetic + [0, -0.0005, 0] west_ned = local_coord.geodetic2ned(west_geodetic) - assert np.abs(west_ned[0]) < 1e-3 - assert west_ned[1] < 0 + self.assertLess(np.abs(west_ned[0]), 1e-3) + self.assertLess(west_ned[1], 0) southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0] southwest_ned = local_coord.geodetic2ned(southwest_geodetic) - assert southwest_ned[0] < 0 - assert southwest_ned[1] < 0 + self.assertLess(southwest_ned[0], 0) + self.assertLess(southwest_ned[1], 0) def test_ecef_geodetic(self): # testing single @@ -102,36 +105,5 @@ def test_ned_batch(self): np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch), ecef_positions_offset_batch, rtol=1e-9, atol=1e-7) - - def test_errors(self): - # Test wrong shape/type for geodetic2ecef - # numpy_wrap raises IndexError for scalar input - with np.testing.assert_raises(IndexError): - coord.geodetic2ecef(1.0) - - with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"): - coord.geodetic2ecef([0, 0]) - - with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"): - coord.geodetic2ecef([0, 0, 0, 0]) - - with np.testing.assert_raises(TypeError): - coord.geodetic2ecef(['a', 'b', 'c']) - - # Test LocalCoord constructor errors - with np.testing.assert_raises(ValueError): - coord.LocalCoord.from_geodetic([0, 0]) - - with np.testing.assert_raises(ValueError): - coord.LocalCoord.from_geodetic(1) - - with np.testing.assert_raises(TypeError): - coord.LocalCoord.from_geodetic(['a', 'b', 'c']) - - # Test wrong shape/type for ecef2geodetic - with np.testing.assert_raises(ValueError): - coord.ecef2geodetic([1, 2]) - with np.testing.assert_raises(ValueError): - coord.ecef2geodetic([1, 2, 3, 4]) - with np.testing.assert_raises(IndexError): - coord.ecef2geodetic(1.0) +if __name__ == "__main__": + unittest.main() diff --git a/common/transformations/tests/test_orientation.py b/common/transformations/tests/test_orientation.py old mode 100644 new mode 100755 index 1bf94115c8320b..50978e1a63df5e --- a/common/transformations/tests/test_orientation.py +++ b/common/transformations/tests/test_orientation.py @@ -1,7 +1,9 @@ +#!/usr/bin/env python3 + import numpy as np -import pytest +import unittest -from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \ +from common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \ rot2quat, quat2rot, \ ned_euler_from_ecef @@ -30,7 +32,7 @@ [ 2.50450101, 0.36304151, 0.33136365]]) -class TestOrientation: +class TestOrientation(unittest.TestCase): def test_quat_euler(self): for i, eul in enumerate(eulers): np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7) @@ -61,31 +63,6 @@ def test_euler_ned(self): #np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7) # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7) - def test_inputs(self): - with pytest.raises(ValueError): - euler2quat([1, 2]) - - with pytest.raises(ValueError): - quat2rot([1, 2, 3]) - - with pytest.raises(IndexError): - rot2quat(np.zeros((2, 2))) - - def test_euler_rot_consistency(self): - rpy = [0.1, 0.2, 0.3] - R = euler2rot(rpy) - - # R -> q -> R - q = rot2quat(R) - R_new = quat2rot(q) - np.testing.assert_allclose(R, R_new, atol=1e-15) - - # q -> R -> Euler (quat2euler) -> R - rpy_new = quat2euler(q) - R_new2 = euler2rot(rpy_new) - np.testing.assert_allclose(R, R_new2, atol=1e-15) - # R -> Euler (rot2euler) -> R - rpy_from_rot = rot2euler(R) - R_new3 = euler2rot(rpy_from_rot) - np.testing.assert_allclose(R, R_new3, atol=1e-15) +if __name__ == "__main__": + unittest.main() diff --git a/common/transformations/transformations.pxd b/common/transformations/transformations.pxd new file mode 100644 index 00000000000000..7af009870198ad --- /dev/null +++ b/common/transformations/transformations.pxd @@ -0,0 +1,72 @@ +#cython: language_level=3 +from libcpp cimport bool + +cdef extern from "orientation.cc": + pass + +cdef extern from "orientation.hpp": + cdef cppclass Quaternion "Eigen::Quaterniond": + Quaternion() + Quaternion(double, double, double, double) + double w() + double x() + double y() + double z() + + cdef cppclass Vector3 "Eigen::Vector3d": + Vector3() + Vector3(double, double, double) + double operator()(int) + + cdef cppclass Matrix3 "Eigen::Matrix3d": + Matrix3() + Matrix3(double*) + + double operator()(int, int) + + Quaternion euler2quat(Vector3) + Vector3 quat2euler(Quaternion) + Matrix3 quat2rot(Quaternion) + Quaternion rot2quat(Matrix3) + Vector3 rot2euler(Matrix3) + Matrix3 euler2rot(Vector3) + Matrix3 rot_matrix(double, double, double) + Vector3 ecef_euler_from_ned(ECEF, Vector3) + Vector3 ned_euler_from_ecef(ECEF, Vector3) + + +cdef extern from "coordinates.cc": + cdef struct ECEF: + double x + double y + double z + + cdef struct NED: + double n + double e + double d + + cdef struct Geodetic: + double lat + double lon + double alt + bool radians + + ECEF geodetic2ecef(Geodetic) + Geodetic ecef2geodetic(ECEF) + + cdef cppclass LocalCoord_c "LocalCoord": + Matrix3 ned2ecef_matrix + Matrix3 ecef2ned_matrix + + LocalCoord_c(Geodetic, ECEF) + LocalCoord_c(Geodetic) + LocalCoord_c(ECEF) + + NED ecef2ned(ECEF) + ECEF ned2ecef(NED) + NED geodetic2ned(Geodetic) + Geodetic ned2geodetic(NED) + +cdef extern from "coordinates.hpp": + pass diff --git a/common/transformations/transformations.py b/common/transformations/transformations.py deleted file mode 100644 index 5cb6220f95ec76..00000000000000 --- a/common/transformations/transformations.py +++ /dev/null @@ -1,342 +0,0 @@ -import numpy as np - - -# Constants -a = 6378137.0 -b = 6356752.3142 -esq = 6.69437999014e-3 -e1sq = 6.73949674228e-3 - - -def geodetic2ecef_single(g): - """ - Convert geodetic coordinates (latitude, longitude, altitude) to ECEF. - """ - try: - if len(g) != 3: - raise ValueError("Geodetic must be size 3") - except TypeError: - raise ValueError("Geodetic must be a sequence of length 3") from None - - lat, lon, alt = g - lat = np.radians(lat) - lon = np.radians(lon) - xi = np.sqrt(1.0 - esq * np.sin(lat)**2) - x = (a / xi + alt) * np.cos(lat) * np.cos(lon) - y = (a / xi + alt) * np.cos(lat) * np.sin(lon) - z = (a / xi * (1.0 - esq) + alt) * np.sin(lat) - return np.array([x, y, z]) - - -def ecef2geodetic_single(e): - """ - Convert ECEF to geodetic coordinates using Ferrari's solution. - """ - x, y, z = e - r = np.sqrt(x**2 + y**2) - Esq = a**2 - b**2 - F = 54 * b**2 * z**2 - G = r**2 + (1 - esq) * z**2 - esq * Esq - C = (esq**2 * F * r**2) / (G**3) - S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C)) - P = F / (3 * (S + 1 / S + 1)**2 * G**2) - Q = np.sqrt(1 + 2 * esq**2 * P) - r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2) - U = np.sqrt((r - esq * r_0)**2 + z**2) - V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2) - Z_0 = b**2 * z / (a * V) - h = U * (1 - b**2 / (a * V)) - lat = np.arctan((z + e1sq * Z_0) / r) - lon = np.arctan2(y, x) - return np.array([np.degrees(lat), np.degrees(lon), h]) - - -def euler2quat_single(euler): - """ - Convert Euler angles (roll, pitch, yaw) to a quaternion. - Rotation order: Z-Y-X (yaw, pitch, roll). - """ - phi, theta, psi = euler - - c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2) - c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2) - c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2) - - w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi - x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi - y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi - z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi - - if w < 0: - return np.array([-w, -x, -y, -z]) - return np.array([w, x, y, z]) - - -def quat2euler_single(q): - """ - Convert a quaternion to Euler angles (roll, pitch, yaw). - """ - w, x, y, z = q - gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2)) - sin_arg = 2 * (w * y - z * x) - sin_arg = np.clip(sin_arg, -1.0, 1.0) - theta = np.arcsin(sin_arg) - psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2)) - return np.array([gamma, theta, psi]) - - -def quat2rot_single(q): - """ - Convert a quaternion to a 3x3 rotation matrix. - """ - w, x, y, z = q - xx, yy, zz = x * x, y * y, z * z - xy, xz, yz = x * y, x * z, y * z - wx, wy, wz = w * x, w * y, w * z - - mat = np.array([ - [1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)], - [2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)], - [2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)] - ]) - return mat - - -def rot2quat_single(rot): - """ - Convert a 3x3 rotation matrix to a quaternion. - """ - trace = np.trace(rot) - if trace > 0: - s = 0.5 / np.sqrt(trace + 1.0) - w = 0.25 / s - x = (rot[2, 1] - rot[1, 2]) * s - y = (rot[0, 2] - rot[2, 0]) * s - z = (rot[1, 0] - rot[0, 1]) * s - else: - if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]: - s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2]) - w = (rot[2, 1] - rot[1, 2]) / s - x = 0.25 * s - y = (rot[0, 1] + rot[1, 0]) / s - z = (rot[0, 2] + rot[2, 0]) / s - elif rot[1, 1] > rot[2, 2]: - s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2]) - w = (rot[0, 2] - rot[2, 0]) / s - x = (rot[0, 1] + rot[1, 0]) / s - y = 0.25 * s - z = (rot[1, 2] + rot[2, 1]) / s - else: - s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1]) - w = (rot[1, 0] - rot[0, 1]) / s - x = (rot[0, 2] + rot[2, 0]) / s - y = (rot[1, 2] + rot[2, 1]) / s - z = 0.25 * s - - if w < 0: - return np.array([-w, -x, -y, -z]) - return np.array([w, x, y, z]) - - -def euler2rot_single(euler): - """ - Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix. - Rotation order: Z-Y-X (yaw, pitch, roll). - """ - phi, theta, psi = euler - - cx, sx = np.cos(phi), np.sin(phi) - cy, sy = np.cos(theta), np.sin(theta) - cz, sz = np.cos(psi), np.sin(psi) - - Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]]) - Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]]) - Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]]) - - return Rz @ Ry @ Rx - - -def rot2euler_single(rot): - """ - Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw). - """ - return quat2euler_single(rot2quat_single(rot)) - - -def rot_matrix(roll, pitch, yaw): - """ - Create a 3x3 rotation matrix from roll, pitch, and yaw angles. - """ - return euler2rot_single([roll, pitch, yaw]) - - -def axis_angle_to_rot(axis, angle): - """ - Convert an axis-angle representation to a 3x3 rotation matrix. - """ - c = np.cos(angle / 2) - s = np.sin(angle / 2) - q = np.array([c, s*axis[0], s*axis[1], s*axis[2]]) - return quat2rot_single(q) - - -class LocalCoord: - """ - A class to handle conversions between ECEF and local NED coordinates. - """ - def __init__(self, geodetic=None, ecef=None): - """ - Initialize LocalCoord with either geodetic or ECEF coordinates. - """ - if geodetic is not None: - self.init_ecef = geodetic2ecef_single(geodetic) - lat, lon, _ = geodetic - elif ecef is not None: - self.init_ecef = np.array(ecef) - lat, lon, _ = ecef2geodetic_single(ecef) - else: - raise ValueError("Must provide geodetic or ecef") - - lat = np.radians(lat) - lon = np.radians(lon) - - self.ned2ecef_matrix = np.array([ - [-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)], - [-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)], - [np.cos(lat), 0, -np.sin(lat)] - ]) - self.ecef2ned_matrix = self.ned2ecef_matrix.T - - @classmethod - def from_geodetic(cls, geodetic): - """ - Create a LocalCoord instance from geodetic coordinates. - """ - return cls(geodetic=geodetic) - - @classmethod - def from_ecef(cls, ecef): - """ - Create a LocalCoord instance from ECEF coordinates. - """ - return cls(ecef=ecef) - - def ecef2ned_single(self, ecef): - """ - Convert a single ECEF point to NED coordinates relative to the origin. - """ - return self.ecef2ned_matrix @ (ecef - self.init_ecef) - - def ned2ecef_single(self, ned): - """ - Convert a single NED point to ECEF coordinates. - """ - return self.ned2ecef_matrix @ ned + self.init_ecef - - def geodetic2ned_single(self, geodetic): - """ - Convert a single geodetic point to NED coordinates. - """ - ecef = geodetic2ecef_single(geodetic) - return self.ecef2ned_single(ecef) - - def ned2geodetic_single(self, ned): - """ - Convert a single NED point to geodetic coordinates. - """ - ecef = self.ned2ecef_single(ned) - return ecef2geodetic_single(ecef) - - @property - def ned_from_ecef_matrix(self): - """ - Returns the rotation matrix from ECEF to NED coordinates. - """ - return self.ecef2ned_matrix - - @property - def ecef_from_ned_matrix(self): - """ - Returns the rotation matrix from NED to ECEF coordinates. - """ - return self.ned2ecef_matrix - - -def ecef_euler_from_ned_single(ecef_init, ned_pose): - """ - Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin - to equivalent ECEF Euler angles. - """ - converter = LocalCoord(ecef=ecef_init) - zero = np.array(ecef_init) - - x0 = converter.ned2ecef_single([1, 0, 0]) - zero - y0 = converter.ned2ecef_single([0, 1, 0]) - zero - z0 = converter.ned2ecef_single([0, 0, 1]) - zero - - phi, theta, psi = ned_pose - - x1 = axis_angle_to_rot(z0, psi) @ x0 - y1 = axis_angle_to_rot(z0, psi) @ y0 - z1 = axis_angle_to_rot(z0, psi) @ z0 - - x2 = axis_angle_to_rot(y1, theta) @ x1 - y2 = axis_angle_to_rot(y1, theta) @ y1 - z2 = axis_angle_to_rot(y1, theta) @ z1 - - x3 = axis_angle_to_rot(x2, phi) @ x2 - y3 = axis_angle_to_rot(x2, phi) @ y2 - - x0 = np.array([1.0, 0, 0]) - y0 = np.array([0, 1.0, 0]) - z0 = np.array([0, 0, 1.0]) - - psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0)) - theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2)) - - y2 = axis_angle_to_rot(z0, psi_out) @ y0 - z2 = axis_angle_to_rot(y2, theta_out) @ z0 - - phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2)) - - return np.array([phi_out, theta_out, psi_out]) - - -def ned_euler_from_ecef_single(ecef_init, ecef_pose): - """ - Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin - to equivalent NED Euler angles. - """ - converter = LocalCoord(ecef=ecef_init) - - x0 = np.array([1.0, 0, 0]) - y0 = np.array([0, 1.0, 0]) - z0 = np.array([0, 0, 1.0]) - - phi, theta, psi = ecef_pose - - x1 = axis_angle_to_rot(z0, psi) @ x0 - y1 = axis_angle_to_rot(z0, psi) @ y0 - z1 = axis_angle_to_rot(z0, psi) @ z0 - - x2 = axis_angle_to_rot(y1, theta) @ x1 - y2 = axis_angle_to_rot(y1, theta) @ y1 - z2 = axis_angle_to_rot(y1, theta) @ z1 - - x3 = axis_angle_to_rot(x2, phi) @ x2 - y3 = axis_angle_to_rot(x2, phi) @ y2 - - zero = np.array(ecef_init) - x0 = converter.ned2ecef_single([1, 0, 0]) - zero - y0 = converter.ned2ecef_single([0, 1, 0]) - zero - z0 = converter.ned2ecef_single([0, 0, 1]) - zero - - psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0)) - theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2)) - - y2 = axis_angle_to_rot(z0, psi_out) @ y0 - z2 = axis_angle_to_rot(y2, theta_out) @ z0 - - phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2)) - - return np.array([phi_out, theta_out, psi_out]) diff --git a/common/transformations/transformations.pyx b/common/transformations/transformations.pyx new file mode 100644 index 00000000000000..ce80d90d29ed07 --- /dev/null +++ b/common/transformations/transformations.pyx @@ -0,0 +1,174 @@ +# distutils: language = c++ +# cython: language_level = 3 +from common.transformations.transformations cimport Matrix3, Vector3, Quaternion +from common.transformations.transformations cimport ECEF, NED, Geodetic + +from common.transformations.transformations cimport euler2quat as euler2quat_c +from common.transformations.transformations cimport quat2euler as quat2euler_c +from common.transformations.transformations cimport quat2rot as quat2rot_c +from common.transformations.transformations cimport rot2quat as rot2quat_c +from common.transformations.transformations cimport euler2rot as euler2rot_c +from common.transformations.transformations cimport rot2euler as rot2euler_c +from common.transformations.transformations cimport rot_matrix as rot_matrix_c +from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c +from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c +from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c +from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c +from common.transformations.transformations cimport LocalCoord_c + + +import cython +import numpy as np +cimport numpy as np + +cdef np.ndarray[double, ndim=2] matrix2numpy(Matrix3 m): + return np.array([ + [m(0, 0), m(0, 1), m(0, 2)], + [m(1, 0), m(1, 1), m(1, 2)], + [m(2, 0), m(2, 1), m(2, 2)], + ]) + +cdef Matrix3 numpy2matrix(np.ndarray[double, ndim=2, mode="fortran"] m): + assert m.shape[0] == 3 + assert m.shape[1] == 3 + return Matrix3(m.data) + +cdef ECEF list2ecef(ecef): + cdef ECEF e; + e.x = ecef[0] + e.y = ecef[1] + e.z = ecef[2] + return e + +cdef NED list2ned(ned): + cdef NED n; + n.n = ned[0] + n.e = ned[1] + n.d = ned[2] + return n + +cdef Geodetic list2geodetic(geodetic): + cdef Geodetic g + g.lat = geodetic[0] + g.lon = geodetic[1] + g.alt = geodetic[2] + return g + +def euler2quat_single(euler): + cdef Vector3 e = Vector3(euler[0], euler[1], euler[2]) + cdef Quaternion q = euler2quat_c(e) + return [q.w(), q.x(), q.y(), q.z()] + +def quat2euler_single(quat): + cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3]) + cdef Vector3 e = quat2euler_c(q); + return [e(0), e(1), e(2)] + +def quat2rot_single(quat): + cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3]) + cdef Matrix3 r = quat2rot_c(q) + return matrix2numpy(r) + +def rot2quat_single(rot): + cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double)) + cdef Quaternion q = rot2quat_c(r) + return [q.w(), q.x(), q.y(), q.z()] + +def euler2rot_single(euler): + cdef Vector3 e = Vector3(euler[0], euler[1], euler[2]) + cdef Matrix3 r = euler2rot_c(e) + return matrix2numpy(r) + +def rot2euler_single(rot): + cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double)) + cdef Vector3 e = rot2euler_c(r) + return [e(0), e(1), e(2)] + +def rot_matrix(roll, pitch, yaw): + return matrix2numpy(rot_matrix_c(roll, pitch, yaw)) + +def ecef_euler_from_ned_single(ecef_init, ned_pose): + cdef ECEF init = list2ecef(ecef_init) + cdef Vector3 pose = Vector3(ned_pose[0], ned_pose[1], ned_pose[2]) + + cdef Vector3 e = ecef_euler_from_ned_c(init, pose) + return [e(0), e(1), e(2)] + +def ned_euler_from_ecef_single(ecef_init, ecef_pose): + cdef ECEF init = list2ecef(ecef_init) + cdef Vector3 pose = Vector3(ecef_pose[0], ecef_pose[1], ecef_pose[2]) + + cdef Vector3 e = ned_euler_from_ecef_c(init, pose) + return [e(0), e(1), e(2)] + +def geodetic2ecef_single(geodetic): + cdef Geodetic g = list2geodetic(geodetic) + cdef ECEF e = geodetic2ecef_c(g) + return [e.x, e.y, e.z] + +def ecef2geodetic_single(ecef): + cdef ECEF e = list2ecef(ecef) + cdef Geodetic g = ecef2geodetic_c(e) + return [g.lat, g.lon, g.alt] + + +cdef class LocalCoord: + cdef LocalCoord_c * lc + + def __init__(self, geodetic=None, ecef=None): + assert (geodetic is not None) or (ecef is not None) + if geodetic is not None: + self.lc = new LocalCoord_c(list2geodetic(geodetic)) + elif ecef is not None: + self.lc = new LocalCoord_c(list2ecef(ecef)) + + @property + def ned2ecef_matrix(self): + return matrix2numpy(self.lc.ned2ecef_matrix) + + @property + def ecef2ned_matrix(self): + return matrix2numpy(self.lc.ecef2ned_matrix) + + @property + def ned_from_ecef_matrix(self): + return self.ecef2ned_matrix + + @property + def ecef_from_ned_matrix(self): + return self.ned2ecef_matrix + + @classmethod + def from_geodetic(cls, geodetic): + return cls(geodetic=geodetic) + + @classmethod + def from_ecef(cls, ecef): + return cls(ecef=ecef) + + def ecef2ned_single(self, ecef): + assert self.lc + cdef ECEF e = list2ecef(ecef) + cdef NED n = self.lc.ecef2ned(e) + return [n.n, n.e, n.d] + + def ned2ecef_single(self, ned): + assert self.lc + cdef NED n = list2ned(ned) + cdef ECEF e = self.lc.ned2ecef(n) + return [e.x, e.y, e.z] + + def geodetic2ned_single(self, geodetic): + assert self.lc + cdef Geodetic g = list2geodetic(geodetic) + cdef NED n = self.lc.geodetic2ned(g) + return [n.n, n.e, n.d] + + def ned2geodetic_single(self, ned): + assert self.lc + cdef NED n = list2ned(ned) + cdef Geodetic g = self.lc.ned2geodetic(n) + return [g.lat, g.lon, g.alt] + + def __dealloc__(self): + del self.lc diff --git a/common/util.cc b/common/util.cc index 26a2bd60bc4639..92add63997f8b8 100644 --- a/common/util.cc +++ b/common/util.cc @@ -1,9 +1,7 @@ #include "common/util.h" -#include "common/swaglog.h" -#include #include -#include +#include #include #include @@ -11,9 +9,7 @@ #include #include #include -#include #include -#include #ifdef __linux__ #include @@ -62,27 +58,12 @@ int set_core_affinity(std::vector cores) { #endif } -int set_file_descriptor_limit(uint64_t limit_val) { - struct rlimit limit; - int status; - - if ((status = getrlimit(RLIMIT_NOFILE, &limit)) < 0) - return status; - - limit.rlim_cur = limit_val; - if ((status = setrlimit(RLIMIT_NOFILE, &limit)) < 0) - return status; - - return 0; -} - std::string read_file(const std::string& fn) { std::ifstream f(fn, std::ios::binary | std::ios::in); if (f.is_open()) { f.seekg(0, std::ios::end); - std::streamsize size = f.tellg(); - // seekg and tellg on a directory doesn't return pos_type(-1) but max(streamsize) - if (f.good() && size > 0 && size < std::numeric_limits::max()) { + int size = f.tellg(); + if (f.good() && size > 0) { std::string result(size, '\0'); f.seekg(0, std::ios::beg); f.read(result.data(), size); @@ -152,19 +133,6 @@ int safe_fflush(FILE *stream) { return ret; } -int safe_ioctl(int fd, unsigned long request, void *argp, const char* exception_msg) { - int ret; - do { - ret = ioctl(fd, request, argp); - } while ((ret == -1) && (errno == EINTR)); - - if (ret == -1 && exception_msg) { - LOGE("safe_ioctl error: %s %s(%d) (fd: %d request: %lx argp: %p)", exception_msg, strerror(errno), errno, fd, request, argp); - throw std::runtime_error(exception_msg); - } - return ret; -} - std::string readlink(const std::string &path) { char buff[4096]; ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1); @@ -211,7 +179,7 @@ bool create_directories(const std::string& dir, mode_t mode) { return createDirectory(dir, mode); } -std::string getenv(const char* key, std::string default_val) { +std::string getenv(const char* key, const char* default_val) { const char* val = ::getenv(key); return val ? val : default_val; } @@ -235,54 +203,10 @@ std::string hexdump(const uint8_t* in, const size_t size) { return ss.str(); } -int random_int(int min, int max) { - std::random_device dev; - std::mt19937 rng(dev()); - std::uniform_int_distribution dist(min, max); - return dist(rng); -} - -std::string random_string(std::string::size_type length) { - const std::string chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"; - std::mt19937 rg{std::random_device{}()}; - std::uniform_int_distribution pick(0, chrs.length() - 1); - std::string s; - s.reserve(length); - while (length--) { - s += chrs[pick(rg)]; - } - return s; -} - -bool starts_with(const std::string &s1, const std::string &s2) { - return strncmp(s1.c_str(), s2.c_str(), s2.size()) == 0; -} - -bool ends_with(const std::string& s, const std::string& suffix) { - return s.size() >= suffix.size() && - strcmp(s.c_str() + (s.size() - suffix.size()), suffix.c_str()) == 0; -} - -std::string strip(const std::string &str) { - auto should_trim = [](unsigned char ch) { - return std::isspace(ch) || ch == '\0'; - }; - - size_t start = 0; - while (start < str.size() && should_trim(static_cast(str[start]))) { - start++; - } - - if (start == str.size()) { - return ""; - } - - size_t end = str.size() - 1; - while (end > 0 && should_trim(static_cast(str[end]))) { - end--; - } - - return str.substr(start, end - start + 1); +std::string dir_name(std::string const &path) { + size_t pos = path.find_last_of("/"); + if (pos == std::string::npos) return ""; + return path.substr(0, pos); } std::string check_output(const std::string& command) { @@ -301,17 +225,20 @@ std::string check_output(const std::string& command) { return result; } -bool system_time_valid() { - // Default to August 26, 2024 - tm min_tm = {.tm_year = 2024 - 1900, .tm_mon = 7, .tm_mday = 26}; - time_t min_date = mktime(&min_tm); +struct tm get_time() { + time_t rawtime; + time(&rawtime); - struct stat st; - if (stat("/lib/systemd/systemd", &st) == 0) { - min_date = std::max(min_date, st.st_mtime + 86400); // Add 1 day (86400 seconds) - } + struct tm sys_time; + gmtime_r(&rawtime, &sys_time); + + return sys_time; +} - return time(nullptr) > min_date; +bool time_valid(struct tm sys_time) { + int year = 1900 + sys_time.tm_year; + int month = 1 + sys_time.tm_mon; + return (year > 2021) || (year == 2021 && month >= 6); } } // namespace util diff --git a/common/util.h b/common/util.h index f46db4d9fa2cc4..f3a24723b4f51e 100644 --- a/common/util.h +++ b/common/util.h @@ -3,11 +3,13 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -37,14 +39,15 @@ 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; -#define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1)) - namespace util { void set_thread_name(const char* name); int set_realtime_priority(int level); int set_core_affinity(std::vector cores); -int set_file_descriptor_limit(uint64_t limit); + +// ***** Time helpers ***** +struct tm get_time(); +bool time_valid(struct tm sys_time); // ***** math helpers ***** @@ -67,20 +70,14 @@ std::string string_format(const std::string& format, Args... args) { return std::string(buf.get(), buf.get() + size - 1); } -std::string getenv(const char* key, std::string default_val = ""); +std::string getenv(const char* key, const char* default_val = ""); int getenv(const char* key, int default_val); float getenv(const char* key, float default_val); std::string hexdump(const uint8_t* in, const size_t size); -bool starts_with(const std::string &s1, const std::string &s2); -bool ends_with(const std::string &s, const std::string &suffix); -std::string strip(const std::string &str); +std::string dir_name(std::string const& path); -// ***** random helpers ***** -int random_int(int min, int max); -std::string random_string(std::string::size_type length); - -// **** file helpers ***** +// **** file fhelpers ***** std::string read_file(const std::string& fn); std::map read_files_in_dir(const std::string& path); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); @@ -88,7 +85,6 @@ int write_file(const char* path, const void* data, size_t size, int flags = O_WR FILE* safe_fopen(const char* filename, const char* mode); size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream); int safe_fflush(FILE *stream); -int safe_ioctl(int fd, unsigned long request, void *argp, const char* exception_msg = nullptr); std::string readlink(const std::string& path); bool file_exists(const std::string& fn); @@ -96,8 +92,6 @@ bool create_directories(const std::string &dir, mode_t mode); std::string check_output(const std::string& command); -bool system_time_valid(); - inline void sleep_for(const int milliseconds) { if (milliseconds > 0) { std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); @@ -115,7 +109,7 @@ class ExitHandler { #ifndef __APPLE__ std::signal(SIGPWR, (sighandler_t)set_do_exit); #endif - } + }; inline static std::atomic power_failure = false; inline static std::atomic signal = 0; inline operator bool() { return do_exit; } @@ -151,18 +145,12 @@ struct unique_fd { class FirstOrderFilter { public: - FirstOrderFilter(float x0, float ts, float dt, bool initialized = true) { + FirstOrderFilter(float x0, float ts, float dt) { k_ = (dt / ts) / (1.0 + dt / ts); x_ = x0; - initialized_ = initialized; } inline float update(float x) { - if (initialized_) { - x_ = (1. - k_) * x_ + k_ * x; - } else { - initialized_ = true; - x_ = x; - } + x_ = (1. - k_) * x_ + k_ * x; return x_; } inline void reset(float x) { x_ = x; } @@ -170,18 +158,43 @@ class FirstOrderFilter { private: float x_, k_; - bool initialized_; }; template void update_max_atomic(std::atomic& max, T const& value) { T prev = max; - while (prev < value && !max.compare_exchange_weak(prev, value)) {} + while(prev < value && !max.compare_exchange_weak(prev, value)) {} } -typedef struct Rect { - int x; - int y; - int w; - int h; -} Rect; +class LogState { + public: + bool initialized = false; + std::mutex lock; + void *zctx = nullptr; + void *sock = nullptr; + int print_level; + const char* endpoint; + + LogState(const char* _endpoint) { + endpoint = _endpoint; + } + + inline void initialize() { + zctx = zmq_ctx_new(); + sock = zmq_socket(zctx, ZMQ_PUSH); + + // Timeout on shutdown for messages to be received by the logging process + int timeout = 100; + zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout)); + + zmq_connect(sock, endpoint); + initialized = true; + } + + ~LogState() { + if (initialized) { + zmq_close(sock); + zmq_ctx_destroy(zctx); + } + } +}; diff --git a/common/utils.py b/common/utils.py deleted file mode 100644 index ccc6719f5f2ee4..00000000000000 --- a/common/utils.py +++ /dev/null @@ -1,186 +0,0 @@ -import io -import os -import tempfile -import contextlib -import subprocess -import time -import functools -from subprocess import Popen, PIPE, TimeoutExpired -import zstandard as zstd - -LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change - -class Timer: - """Simple lap timer for profiling sequential operations.""" - - def __init__(self): - self._start = self._lap = time.monotonic() - self._sections = {} - - def lap(self, name): - now = time.monotonic() - self._sections[name] = now - self._lap - self._lap = now - - @property - def total(self): - return time.monotonic() - self._start - - def fmt(self, duration): - parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items()) - total = self.total - realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A" - return f"{duration}s in {total:.1f}s ({realtime}) | {parts}" - -def sudo_write(val: str, path: str) -> None: - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - os.system(f"sudo chmod a+w {path}") - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - # fallback for debugfs files - os.system(f"sudo su -c 'echo {val} > {path}'") - - -def sudo_read(path: str) -> str: - try: - return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() - except Exception: - return "" - - -class MovingAverage: - def __init__(self, window_size: int): - self.window_size: int = window_size - self.buffer: list[float] = [0.0] * window_size - self.index: int = 0 - self.count: int = 0 - self.sum: float = 0.0 - - def add_value(self, new_value: float): - # Update the sum: subtract the value being replaced and add the new value - self.sum -= self.buffer[self.index] - self.buffer[self.index] = new_value - self.sum += new_value - - # Update the index in a circular manner - self.index = (self.index + 1) % self.window_size - - # Track the number of added values (for partial windows) - self.count = min(self.count + 1, self.window_size) - - def get_average(self) -> float: - if self.count == 0: - return float('nan') - return self.sum / self.count - - -class CallbackReader: - """Wraps a file, but overrides the read method to also - call a callback function with the number of bytes read so far.""" - - def __init__(self, f, callback, *args): - self.f = f - self.callback = callback - self.cb_args = args - self.total_read = 0 - - def __getattr__(self, attr): - return getattr(self.f, attr) - - def read(self, *args, **kwargs): - chunk = self.f.read(*args, **kwargs) - self.total_read += len(chunk) - self.callback(*self.cb_args, self.total_read) - return chunk - - -@contextlib.contextmanager -def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None, - overwrite: bool = False): - """Write to a file atomically using a temporary file in the same directory as the destination file.""" - dir_name = os.path.dirname(path) - - if not overwrite and os.path.exists(path): - raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.") - - with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file: - yield tmp_file - tmp_file_name = tmp_file.name - os.replace(tmp_file_name, path) - - -def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]: - if not should_compress: - file_size = os.path.getsize(filepath) - file_stream = open(filepath, "rb") - return file_stream, file_size - - # Compress the file on the fly - compressed_stream = io.BytesIO() - compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL) - - with open(filepath, "rb") as f: - compressor.copy_stream(f, compressed_stream) - compressed_size = compressed_stream.tell() - compressed_stream.seek(0) - return compressed_stream, compressed_size - - -# remove all keys that end in DEPRECATED -def strip_deprecated_keys(d): - for k in list(d.keys()): - if isinstance(k, str): - if k.endswith('DEPRECATED'): - d.pop(k) - elif isinstance(d[k], dict): - strip_deprecated_keys(d[k]) - return d - - -def run_cmd(cmd: list[str], cwd=None, env=None) -> str: - return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip() - - -def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str: - try: - return run_cmd(cmd, cwd=cwd, env=env) - except subprocess.CalledProcessError: - return default - - -@contextlib.contextmanager -def managed_proc(cmd: list[str], env: dict[str, str]): - proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE) - try: - yield proc - finally: - if proc.poll() is None: - proc.terminate() - try: - proc.wait(timeout=5) - except TimeoutExpired: - proc.kill() - - -def retry(attempts=3, delay=1.0, ignore_failure=False): - def decorator(func): - @functools.wraps(func) - def wrapper(*args, **kwargs): - for _ in range(attempts): - try: - return func(*args, **kwargs) - except Exception: - print(f"{func.__name__} failed, trying again") - time.sleep(delay) - - if ignore_failure: - print(f"{func.__name__} failed after retry") - else: - raise Exception(f"{func.__name__} failed after retry") - return wrapper - return decorator diff --git a/common/version.h b/common/version.h index 7e78d64b2233a4..0a109c1faac060 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.10.4" +#define COMMA_VERSION "0.8.17" diff --git a/common/watchdog.cc b/common/watchdog.cc new file mode 100644 index 00000000000000..920df4030a9303 --- /dev/null +++ b/common/watchdog.cc @@ -0,0 +1,9 @@ +#include "common/watchdog.h" +#include "common/util.h" + +const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + + +bool watchdog_kick(uint64_t ts) { + static std::string fn = watchdog_fn_prefix + std::to_string(getpid()); + return util::write_file(fn.c_str(), &ts, sizeof(ts), O_WRONLY | O_CREAT) > 0; +} diff --git a/common/watchdog.h b/common/watchdog.h new file mode 100644 index 00000000000000..12dd2ca0355f09 --- /dev/null +++ b/common/watchdog.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +bool watchdog_kick(uint64_t ts); diff --git a/common/window.py b/common/window.py new file mode 100644 index 00000000000000..613b3b201bc0c5 --- /dev/null +++ b/common/window.py @@ -0,0 +1,61 @@ +import sys +import pygame # pylint: disable=import-error +import cv2 # pylint: disable=import-error + +class Window: + def __init__(self, w, h, caption="window", double=False, halve=False): + self.w = w + self.h = h + pygame.display.init() + pygame.display.set_caption(caption) + self.double = double + self.halve = halve + if self.double: + self.rw, self.rh = w*2, h*2 + elif self.halve: + self.rw, self.rh = w//2, h//2 + else: + self.rw, self.rh = w, h + self.screen = pygame.display.set_mode((self.rw, self.rh)) + pygame.display.flip() + + # hack for xmonad, it shrinks the window by 6 pixels after the display.flip + if self.screen.get_width() != self.rw: + self.screen = pygame.display.set_mode((self.rw+(self.rw-self.screen.get_width()), self.rh+(self.rh-self.screen.get_height()))) + pygame.display.flip() + + def draw(self, out): + pygame.event.pump() + if self.double: + out2 = cv2.resize(out, (self.w*2, self.h*2)) + pygame.surfarray.blit_array(self.screen, out2.swapaxes(0, 1)) + elif self.halve: + out2 = cv2.resize(out, (self.w//2, self.h//2)) + pygame.surfarray.blit_array(self.screen, out2.swapaxes(0, 1)) + else: + pygame.surfarray.blit_array(self.screen, out.swapaxes(0, 1)) + pygame.display.flip() + + def getkey(self): + while 1: + event = pygame.event.wait() + if event.type == pygame.QUIT: + pygame.quit() + sys.exit() + if event.type == pygame.KEYDOWN: + return event.key + + def getclick(self): + for event in pygame.event.get(): + if event.type == pygame.MOUSEBUTTONDOWN: + mx, my = pygame.mouse.get_pos() + return mx, my + +if __name__ == "__main__": + import numpy as np + win = Window(200, 200, double=True) + img: np.ndarray = np.zeros((200, 200, 3), np.uint8) + while 1: + print("draw") + img += 1 + win.draw(img) diff --git a/common/xattr.py b/common/xattr.py new file mode 100644 index 00000000000000..26616fd638c701 --- /dev/null +++ b/common/xattr.py @@ -0,0 +1,46 @@ +import os +from cffi import FFI +from typing import Any, List + +# Workaround for the EON/termux build of Python having os.*xattr removed. +ffi = FFI() +ffi.cdef(""" +int setxattr(const char *path, const char *name, const void *value, size_t size, int flags); +ssize_t getxattr(const char *path, const char *name, void *value, size_t size); +ssize_t listxattr(const char *path, char *list, size_t size); +int removexattr(const char *path, const char *name); +""") +libc = ffi.dlopen(None) + +def setxattr(path, name, value, flags=0) -> None: + path = path.encode() + name = name.encode() + if libc.setxattr(path, name, value, len(value), flags) == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: setxattr({path}, {name}, {value}, {flags})") + +def getxattr(path, name, size=128): + path = path.encode() + name = name.encode() + value = ffi.new(f"char[{size}]") + l = libc.getxattr(path, name, value, size) + if l == -1: + # errno 61 means attribute hasn't been set + if ffi.errno == 61: + return None + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: getxattr({path}, {name}, {size})") + return ffi.buffer(value)[:l] + +def listxattr(path, size=128) -> List[Any]: + path = path.encode() + attrs = ffi.new(f"char[{size}]") + l = libc.listxattr(path, attrs, size) + if l == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: listxattr({path}, {size})") + # attrs is b'\0' delimited values (so chop off trailing empty item) + return [a.decode() for a in ffi.buffer(attrs)[:l].split(b"\0")[0:-1]] + +def removexattr(path, name) -> None: + path = path.encode() + name = name.encode() + if libc.removexattr(path, name) == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: removexattr({path}, {name})") diff --git a/conftest.py b/conftest.py deleted file mode 100644 index 7e40ec3ed7a9d5..00000000000000 --- a/conftest.py +++ /dev/null @@ -1,111 +0,0 @@ -import contextlib -import gc -import os -import pytest - -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.system.manager import manager -from openpilot.system.hardware import TICI, HARDWARE - -# TODO: pytest-cpp doesn't support FAIL, and we need to create test translations in sessionstart -# pending https://github.com/pytest-dev/pytest-cpp/pull/147 -collect_ignore = [ - "selfdrive/ui/tests/test_translations", - "selfdrive/test/process_replay/test_processes.py", - "selfdrive/test/process_replay/test_regen.py", -] -collect_ignore_glob = [ - "selfdrive/debug/*.py", - "selfdrive/modeld/*.py", -] - - -def pytest_sessionstart(session): - # TODO: fix tests and enable test order randomization - if session.config.pluginmanager.hasplugin('randomly'): - session.config.option.randomly_reorganize = False - - -@pytest.hookimpl(hookwrapper=True, trylast=True) -def pytest_runtest_call(item): - # ensure we run as a hook after capturemanager's - if item.get_closest_marker("nocapture") is not None: - capmanager = item.config.pluginmanager.getplugin("capturemanager") - with capmanager.global_and_fixture_disabled(): - yield - else: - yield - - -@contextlib.contextmanager -def clean_env(): - starting_env = dict(os.environ) - yield - os.environ.clear() - os.environ.update(starting_env) - - -@pytest.fixture(scope="function", autouse=True) -def openpilot_function_fixture(request): - with clean_env(): - # setup a clean environment for each test - with OpenpilotPrefix(shared_download_cache=request.node.get_closest_marker("shared_download_cache") is not None) as prefix: - prefix = os.environ["OPENPILOT_PREFIX"] - - yield - - # ensure the test doesn't change the prefix - assert "OPENPILOT_PREFIX" in os.environ and prefix == os.environ["OPENPILOT_PREFIX"] - - # cleanup any started processes - manager.manager_cleanup() - - # some processes disable gc for performance, re-enable here - if not gc.isenabled(): - gc.enable() - gc.collect() - -# If you use setUpClass, the environment variables won't be cleared properly, -# so we need to hook both the function and class pytest fixtures -@pytest.fixture(scope="class", autouse=True) -def openpilot_class_fixture(): - with clean_env(): - yield - - -@pytest.fixture(scope="function") -def tici_setup_fixture(request, openpilot_function_fixture): - """Ensure a consistent state for tests on-device. Needs the openpilot function fixture to run first.""" - if 'skip_tici_setup' in request.keywords: - return - HARDWARE.initialize_hardware() - HARDWARE.set_power_save(False) - os.system("pkill -9 -f athena") - - -@pytest.hookimpl(tryfirst=True) -def pytest_collection_modifyitems(config, items): - skipper = pytest.mark.skip(reason="Skipping tici test on PC") - for item in items: - if "tici" in item.keywords: - if not TICI: - item.add_marker(skipper) - else: - item.fixturenames.append('tici_setup_fixture') - - if "xdist_group_class_property" in item.keywords: - class_property_name = item.get_closest_marker('xdist_group_class_property').args[0] - class_property_value = getattr(item.cls, class_property_name) - item.add_marker(pytest.mark.xdist_group(class_property_value)) - - -@pytest.hookimpl(trylast=True) -def pytest_configure(config): - config_line = "xdist_group_class_property: group tests by a property of the class that contains them" - config.addinivalue_line("markers", config_line) - - config_line = "nocapture: don't capture test output" - config.addinivalue_line("markers", config_line) - - config_line = "shared_download_cache: share download cache between tests" - config.addinivalue_line("markers", config_line) diff --git a/docs/CARS.md b/docs/CARS.md index 65f79cdba4d477..47694fffeecfba 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -2,357 +2,227 @@ # Supported Cars -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. +A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. -# 328 Supported Cars +# 205 Supported Cars -|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
     |Video|Setup Video| -|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| -|Acura|ILX 2016-18|Technology Plus Package or AcuraWatch Plus|openpilot|26 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|ILX 2019|All|openpilot|26 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|MDX 2025-26|All except Type S|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|RDX 2016-18|AcuraWatch Plus or Advance Package|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|RDX 2019-21|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|TLX 2021|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 A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Acura|TLX 2025|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Audi|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim, without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chevrolet|Equinox 2019-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chrysler|Pacifica 2021-23|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Chrysler|Pacifica Hybrid 2019-25|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None||| -|CUPRA|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Dodge|Durango 2020-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Bronco Sport 2021-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Escape 2023-24|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Escape Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Escape Plug-in Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Escape Plug-in Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Expedition 2022-24|Co-Pilot360 Assist 2.0|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Explorer 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Explorer Hybrid 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Focus 2018[2](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Focus Hybrid 2018[2](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Kuga 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Kuga Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Kuga Hybrid 2024|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Kuga Plug-in Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Kuga Plug-in Hybrid 2024|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Maverick 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Maverick 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Maverick Hybrid 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Ford|Mustang Mach-E 2021-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q4 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Genesis|G70 2018|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G70 2019-21|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G70 2022-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G80 (2.5T Advanced Trim, with HDA II) 2024|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 P connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|G90 2017-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV60 (Advanced Trim) 2023|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV60 (Performance Trim) 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV70 (2.5T Trim, without HDA II) 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 Hyundai L connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV70 (3.5T Trim, without HDA II) 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV70 Electrified (Australia Only) 2022|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV70 Electrified (with HDA II) 2023-24|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Genesis|GV80 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Accord 2018-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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Accord 2023-25|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Accord Hybrid 2018-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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Accord Hybrid 2023-25|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|City (Brazil only) 2023|All|openpilot available[1](#footnotes)|0 mph|14 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch B connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Civic 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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Civic 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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|CR-V 2015-16|Touring Trim|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|15 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|CR-V 2023-26|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|CR-V Hybrid 2017-22|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|CR-V Hybrid 2023-26|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|e 2020|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Fit 2018-20|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Freed 2020|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|HR-V 2019-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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Odyssey 2021-26|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Odyssey (Taiwan) 2018-19|Honda Sensing|openpilot|19 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Passport 2026|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Pilot 2023-25|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Honda|Ridgeline 2017-25|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Azera 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Azera Hybrid 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Azera Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Custin 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Elantra 2021-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 K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Elantra GT 2017-20|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Elantra Hybrid 2021-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 K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq 5 (Southeast Asia and Europe only) 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 Hyundai Q connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq 5 (with HDA II) 2022-24|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq 5 (without HDA II) 2022-24|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 K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq 6 (with HDA II) 2023-24|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Electric 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Kona 2020|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 B connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Kona Electric 2018-21|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 G connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Kona Electric 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 O connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai R connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Kona Hybrid 2020|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 I connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Nexo 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Santa Cruz 2022-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 N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Santa Fe 2021-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Santa Fe Hybrid 2022-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Santa Fe Plug-in Hybrid 2022-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Sonata 2020-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Sonata Hybrid 2020-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Staria 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson 2022|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 N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson 2023-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson Diesel 2019|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 L connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson Hybrid 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 Hyundai N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Carnival 2022-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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Carnival (China only) 2023|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 K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Ceed 2019-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|EV6 (Southeast Asia only) 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 Hyundai P connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|EV6 (with HDA II) 2022-24|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 P connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|EV6 (without HDA II) 2022-24|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|K8 Hybrid (with HDA II) 2023|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV (with HDA II) 2025|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 R connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro EV (without HDA II) 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 Hyundai A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Hybrid 2018|Smart Cruise Control (SCC)|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Hybrid 2021|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 D connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Hybrid 2022|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 F connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Hybrid 2023|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Plug-in Hybrid 2018-19|All|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Plug-in Hybrid 2020|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Plug-in Hybrid 2021|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 D connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Niro Plug-in Hybrid 2022|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 F connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Seltos 2021|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sorento 2018|Advanced Smart Cruise Control & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sorento 2021-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 K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sorento Hybrid 2021-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sorento Plug-in Hybrid 2022-23|All|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 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sportage 2023-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 N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Sportage Hybrid 2023|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 N connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Stinger 2018-20|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 C connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Stinger 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|CT Hybrid 2017-18|Lexus Safety System+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|ES 2017-18|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|ES 2019-25|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|ES Hybrid 2017-18|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|ES Hybrid 2019-25|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|GS F 2016|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|IS 2022-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|LC 2024-25|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|LS 2018|All except Lexus Safety System+ A|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|NX 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|NX Hybrid 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RC 2023|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX 2016|Lexus Safety System+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX Hybrid 2016|Lexus Safety System+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|RX Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lexus|UX Hybrid 2019-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lincoln|Aviator 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Lincoln|Aviator Plug-in Hybrid 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|MAN|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|MAN|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Mazda|CX-5 2022-25|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Mazda connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Mazda connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Nissan[5](#footnotes)|Altima 2019-20, 2024|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan B connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Nissan[5](#footnotes)|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Nissan[5](#footnotes)|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Nissan[5](#footnotes)|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan A connector
    - 1 OBD-C cable (2 ft)
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Ram connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Rivian|R1S 2022-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Rivian A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Rivian|R1T 2022-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Rivian A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Subaru|Ascent 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Forester 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Impreza 2017-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Impreza 2020-22|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Legacy 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru B connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|Outback 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru B connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|XV 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Subaru|XV 2020-21|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Subaru A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    ||| -|Škoda|Fabia 2022-23[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Škoda|Kamiq 2021-23[11,13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Škoda|Karoq 2019-23[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Škoda|Kodiaq 2017-23[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Škoda|Octavia 2015-19[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Škoda|Octavia RS 2016[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Škoda|Octavia Scout 2017-19[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Škoda|Scala 2020-23[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Škoda|Superb 2015-22[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Tesla[9](#footnotes)|Model 3 (with HW3) 2019-23[8](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Tesla A connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Tesla[9](#footnotes)|Model 3 (with HW4) 2024-25[8](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Tesla B connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Tesla[9](#footnotes)|Model Y (with HW3) 2020-23[8](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Tesla A connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Tesla[9](#footnotes)|Model Y (with HW4) 2024-25[8](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Tesla B connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    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 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon 2016|Toyota Safety Sense P|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon 2017-18|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon Hybrid 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|C-HR 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|C-HR Hybrid 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Camry 2018-20|All|Stock|0 mph[10](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Camry 2021-24|All|openpilot|0 mph[10](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Camry Hybrid 2021-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Corolla Hybrid (South America only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Highlander 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Highlander Hybrid 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius 2016|Toyota Safety Sense P|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius Prime 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Prius v 2017|Toyota Safety Sense P|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 2016|Toyota Safety Sense P|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 2017-18|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 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 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 Hybrid 2017-18|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|RAV4 Hybrid 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 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Toyota|Sienna 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 Toyota A connector
    - 1 comma four
    - 1 comma power v3
    - 1 harness box
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Jetta 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Passat 2015-22[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    [15](#footnotes)||| -|Volkswagen|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Taos 2022-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| -|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 OBD-C cable (2 ft)
    - 1 VW J533 connector
    - 1 comma four
    - 1 harness box
    - 1 long OBD-C cable (9.5 ft)
    - 1 mount
    Buy Here
    ||| +|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| +|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| +|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Acura|RDX 2019-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Audi|A3 2014-19|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Audi|Q2 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Audi|Q3 2020-21|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Audi|RS3 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Audi|S3 2015-17|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Cadillac|Escalade ESV 2016[1](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|Chrysler|Pacifica 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Chrysler|Pacifica 2019-20|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None| +|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| +|Genesis|G70 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| +|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Genesis|G90 2017-18|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Honda|Accord 2018-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Accord Hybrid 2018-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Civic 2019-21|All|openpilot|0 mph|2 mph[2](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Civic 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B| +|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B| +|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|CR-V 2017-22|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|e 2020|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Insight 2019-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Inspire 2018|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| +|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J| +|Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| +|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Hyundai|Ioniq Electric 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC) & LFA|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Hyundai|Ioniq Plug-in Hybrid 2020-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| +|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O| +|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I| +|Hyundai|Palisade 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Hyundai|Santa Fe 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D| +|Hyundai|Santa Fe 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Santa Fe Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Hyundai|Sonata 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Hyundai|Sonata Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| +|Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| +|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Kia|Forte 2019-21|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| +|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| +|Kia|Niro Electric 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Kia|Niro Electric 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| +|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Kia|Niro Plug-in Hybrid 2018-19|Smart Cruise Control (SCC) & LKAS|openpilot|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Kia|Optima 2017|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Kia|Optima 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| +|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Kia|Sorento 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Kia|Sorento 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Kia|Stinger 2018-20|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Kia|Telluride 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Lexus|CT Hybrid 2017-18|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|ES Hybrid 2017-18|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|ES Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|NX 2018-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|NX Hybrid 2018-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|RX Hybrid 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| +|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| +|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B| +|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| +|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| +|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| +|Ram|1500 2019-22|Adaptive Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram| +|SEAT|Ateca 2018|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|SEAT|Leon 2014-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B| +|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B| +|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Škoda|Kamiq 2021[5](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Karoq 2019-21[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Kodiaq 2018-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Octavia 2015, 2018-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Octavia RS 2016|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Scala 2020|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Škoda|Superb 2015-18|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Avalon 2016|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Avalon 2017-18|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Avalon 2019-21|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Avalon Hybrid 2019-21|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry 2018-20|All|Stock|0 mph[4](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry 2021-22|All|openpilot|0 mph[4](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Corolla 2017-19|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Corolla Cross (Non-US only) 2020-21|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Highlander 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Highlander 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Highlander Hybrid 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Highlander Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Prius 2016|Toyota Safety Sense P|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Prius 2017-20|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Prius Prime 2017-20|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Prius v 2017|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 2016|Toyota Safety Sense P|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 2017-18|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 Hybrid 2017-18|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Sienna 2018-20|All|Stock[3](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Volkswagen|Arteon 2018-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon eHybrid 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon R 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Atlas 2018-23[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Atlas Cross Sport 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|California 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|CC 2018-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|e-Golf 2014-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf 2015-20[8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf Alltrack 2015-19|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf GTD 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf GTE 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf GTI 2015-21|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf R 2015-19[8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Golf SportsVan 2015-20|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| +|Volkswagen|Jetta 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Jetta GLI 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat 2015-22[6,7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat Alltrack 2015-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat GTE 2015-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Polo 2020-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Polo GTI 2020-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|T-Cross 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|T-Roc 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Taos 2022[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont Cross Sport 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont X 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Tiguan 2019-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Touran 2017|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| -### Footnotes -1openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `nightly-dev`.
    -2Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in North and South America/Southeast Asia.
    -3See more setup details for GM.
    -42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
    -5See more setup details for Nissan.
    -6In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.
    -7Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB.
    -8Some 2023 model years have HW4. To check which hardware type your vehicle has, look for Autopilot computer under Software -> Additional Vehicle Information on your vehicle's touchscreen. See this page for more information.
    -9See more setup details for Tesla.
    -10openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
    -11Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
    -12Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
    -13Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma four functionality.
    -14Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
    -15Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
    + +1Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
    +22019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
    +3When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
    +4openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
    +5Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
    +6Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
    +7Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway inside the dashboard, choose "VW J533 Development" from the vehicle drop-down for a suitable harness. (Some newer models are also observed to not have a J533 connector.)
    +8Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.)
    ## Community Maintained Cars Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). @@ -370,8 +240,7 @@ If your car has the following packages or features, then it's a good candidate f | Make | Required Package/Features | | ---- | ------------------------- | -| Acura | Any car with AcuraWatch will work. AcuraWatch comes standard on many newer models. | -| Ford | Any car with Lane Centering will likely work. | +| Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | | Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | | Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | | Nissan | Any car with ProPILOT will likely work. | @@ -381,21 +250,19 @@ If your car has the following packages or features, then it's a good candidate f ### FlexRay -All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. +All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the cars in your computer can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. ### Toyota Security openpilot does not yet support these Toyota models due to a new message authentication method. -[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models. +[Vote](https://comma.ai/shop/products/vote) if you'd like to see openpilot support on these models. * Toyota RAV4 Prime 2021+ * Toyota Sienna 2021+ * Toyota Venza 2021+ * Toyota Sequoia 2023+ * Toyota Tundra 2022+ -* Toyota Highlander 2024+ * Toyota Corolla Cross 2022+ (only US model) -* Toyota Camry 2025+ * Lexus NX 2022+ * Toyota bZ4x 2023+ * Subaru Solterra 2023+ diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md index 7583095eaf4794..7a074f12de7905 100644 --- a/docs/CONTRIBUTING.md +++ b/docs/CONTRIBUTING.md @@ -2,48 +2,25 @@ Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. Check out our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/). -Development is coordinated through [Discord](https://discord.comma.ai) and GitHub. +Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available at https://docs.comma.ai and on our [blog](https://blog.comma.ai/). ### Getting Started -* Set up your [development environment](/tools/) -* Join our [Discord](https://discord.comma.ai) -* Docs are at https://docs.comma.ai and https://blog.comma.ai - -## What contributions are we looking for? - -**openpilot's priorities are [safety](SAFETY.md), stability, quality, and features, in that order.** -openpilot is part of comma's mission to *solve self-driving cars while delivering shippable intermediaries*, and all development is towards that goal. - -### What gets merged? - -The probability of a pull request being merged is a function of its value to the project and the effort it will take us to get it merged. -If a PR offers *some* value but will take lots of time to get merged, it will be closed. -Simple, well-tested bug fixes are the easiest to merge, and new features are the hardest to get merged. - -All of these are examples of good PRs: -* typo fix: https://github.com/commaai/openpilot/pull/30678 -* removing unused code: https://github.com/commaai/openpilot/pull/30573 -* simple car model port: https://github.com/commaai/openpilot/pull/30245 -* car brand port: https://github.com/commaai/openpilot/pull/23331 - -### What doesn't get merged? - -* **style changes**: code is art, and it's up to the author to make it beautiful -* **500+ line PRs**: clean it up, break it up into smaller PRs, or both -* **PRs without a clear goal**: every PR must have a singular and clear goal -* **UI design**: we do not have a good review process for this yet -* **New features**: We believe openpilot is mostly feature-complete, and the rest is a matter of refinement and fixing bugs. As a result of this, most feature PRs will be immediately closed, however the beauty of open source is that forks can and do offer features that upstream openpilot doesn't. -* **Negative expected value**: This a class of PRs that makes an improvement, but the risk or validation costs more than the improvement. The risk can be mitigated by first getting a failing test merged. + * Setup your [development environment](../tools/) + * Join our [Discord](https://discord.comma.ai) + * Make sure you have a [GitHub account](https://github.com/signup/free) + * Fork [our repositories](https://github.com/commaai) on GitHub ### First contribution +Try out some of these first pull requests ideas to dive into the codebase: -[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 3X or a car. +* Increase our [mypy](http://mypy-lang.org/) coverage +* Write some documentation +* Tackle an open [good first issue](https://github.com/commaai/openpilot/issues?q=is%3Aissue+is%3Aopen+label%3A%22good+first+issue%22) ## Pull Requests -Pull requests should be against the master branch. +Pull requests should be against the master branch. Welcomed contributions include bug reports, car ports, and any [open issue](https://github.com/commaai/openpilot/issues). If you're unsure about a contribution, feel free to open a discussion, issue, or draft PR to discuss the problem you're trying to solve. A good pull request has all of the following: * a clearly stated purpose @@ -54,20 +31,18 @@ A good pull request has all of the following: * if you've improved your car's tuning, post before and after plots * passes the CI tests -## Contributing without Code +### Car Ports + +We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models. + +If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/). + +## Testing -* Report bugs in GitHub issues. -* Report driving issues in the `#driving-feedback` Discord channel. -* Consider opting into driver camera uploads to improve the driver monitoring model. -* Connect your device to Wi-Fi regularly, so that we can pull data for training better driving models. -* Run the `nightly` branch and report issues. This branch is like `master` but it's built just like a release. -* Annotate images in the [comma10k dataset](https://github.com/commaai/comma10k). +### Automated Testing -## Contributing Training Data +All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions. -### A guide for forks +### Code Style and Linting -In order for your fork's data to be eligible for the training set: -* **Your cereal messaging structs must be [compatible](../cereal#custom-forks)** -* **The definitions of all the stock messaging structs must not change**: Do not change how any of the fields are set, including everything from `selfdriveState.enabled` to `carState.steeringAngleDeg`. Instead, create your own structs and set them however you'd like. -* **Do not include cars that are not supported in upstream platforms**: Instead, create new opendbc platforms for cars that you'd like to support outside of upstream, even if it's just a trim-level difference. +Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`. diff --git a/docs/DEBUGGING_SAFETY.md b/docs/DEBUGGING_SAFETY.md deleted file mode 100644 index cd0a46b446c94b..00000000000000 --- a/docs/DEBUGGING_SAFETY.md +++ /dev/null @@ -1,30 +0,0 @@ -# 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/INTEGRATION.md b/docs/INTEGRATION.md index ba6291c1e35d81..97b72e39d3cdc0 100644 --- a/docs/INTEGRATION.md +++ b/docs/INTEGRATION.md @@ -8,4 +8,4 @@ Additionally, on specific supported cars (see ACC column in [supported cars](CAR * Stock ACC is replaced by openpilot ACC. * openpilot FCW operates in addition to stock FCW. -openpilot should preserve all other vehicle's stock features, including, but not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. +openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 00000000000000..d0aa841c4ddcc4 --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,53 @@ +# Minimal makefile for Sphinx documentation +# + +OPENPILOT_ROOT = `git rev-parse --show-toplevel` + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +DOCSDIR = "$(OPENPILOT_ROOT)/docs" +SOURCEDIR = "$(OPENPILOT_ROOT)/build/docs" +DOCSBUILDDIR = "$(OPENPILOT_ROOT)/build/docs" +BUILDDIR = "$(OPENPILOT_ROOT)/build" + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(DOCSBUILDDIR)" $(SPHINXOPTS) $(O) + +clean: + @echo "Cleaning build folder..." + rm -rf "$(BUILDDIR)" + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @echo "Cleaning build folder..." + rm -rf "$(BUILDDIR)" + mkdir -p "$(DOCSBUILDDIR)" + + @echo "Copying docs & config to build folder..." + cp -a "$(DOCSDIR)" "$(BUILDDIR)" + cd "$(OPENPILOT_ROOT)" && \ + find . -type f \( -name "*.md" -o -name "*.rst" -o -name "*.png" -o -name "*.jpg" -o -name "*.svg" \) \ + -not -path "*/.*" \ + -not -path "./build/*" \ + -not -path "./docs/*" \ + -not -path "./xx/*" \ + -exec cp --parents "{}" ./build/docs/ \; + + @echo "Building rst files..." + sphinx-apidoc -o "$(DOCSBUILDDIR)" ../ \ + ../xx ../laika_repo ../rednose_repo ../pyextra ../notebooks ../panda_jungle \ + ../third_party \ + ../panda/examples \ + ../scripts \ + ../selfdrive/modeld \ + ../selfdrive/debug \ + $(shell find .. -type d -name "*test*") + + @echo "Building html files..." + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(DOCSBUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/README.md b/docs/README.md deleted file mode 100644 index 08dd4fa8bcca9b..00000000000000 --- a/docs/README.md +++ /dev/null @@ -1,26 +0,0 @@ -# openpilot docs - -This is the source for [docs.comma.ai](https://docs.comma.ai). -The site is updated on pushes to master by this [workflow](../.github/workflows/docs.yaml). - -## Development -NOTE: Those commands must be run in the root directory of openpilot, **not /docs** - -**1. Install the docs dependencies** -``` bash -pip install .[docs] -``` - -**2. Build the new site** -``` bash -mkdocs build -``` - -**3. Run the new site locally** -``` bash -mkdocs serve -``` - -References: -* https://www.mkdocs.org/getting-started/ -* https://github.com/ntno/mkdocs-terminal diff --git a/docs/SAFETY.md b/docs/SAFETY.md index 25815e3372e57b..49f88df8c01753 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. @@ -25,22 +25,9 @@ ensuring two main safety requirements. by stepping on the brake pedal or by pressing the cancel button. 2. The vehicle must not alter its trajectory too quickly for the driver to safely react. This means that while the system is engaged, the actuators are constrained - to operate within reasonable limits[^1]. + to operate within reasonable limits. -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). +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 [panda/board/safety/](https://github.com/commaai/panda/tree/master/board/safety). -[^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.** +**Extra note**: 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/_static/favicon.ico b/docs/_static/favicon.ico new file mode 100644 index 00000000000000..976929954f3dac Binary files /dev/null and b/docs/_static/favicon.ico differ diff --git a/docs/_static/logo.png b/docs/_static/logo.png new file mode 100644 index 00000000000000..269956508556e4 Binary files /dev/null and b/docs/_static/logo.png differ diff --git a/docs/_static/robots.txt b/docs/_static/robots.txt new file mode 100644 index 00000000000000..3bcd24fb5d5f9d --- /dev/null +++ b/docs/_static/robots.txt @@ -0,0 +1,2 @@ +User-agent: * +Sitemap: https://docs.comma.ai/sitemap.xml \ No newline at end of file diff --git a/docs/assets/icon-star-empty.svg b/docs/assets/icon-star-empty.svg index 448b20b9377882..5d3c32d6711aba 100644 --- a/docs/assets/icon-star-empty.svg +++ b/docs/assets/icon-star-empty.svg @@ -1,3 +1,56 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3142658fa2782d266028dcab491e02ab329a2cadcf9efb1846a5f9631d64cac1 -size 1947 + + + + + + image/svg+xml + + + + + + + + diff --git a/docs/assets/icon-star-full.svg b/docs/assets/icon-star-full.svg index 0e0bf52cd8461c..294db2b7f27c01 100644 --- a/docs/assets/icon-star-full.svg +++ b/docs/assets/icon-star-full.svg @@ -1,3 +1,56 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5fdcc85f3d8c9d0c2fb76b98ddd9f118c0e4a12c859a4281a04a0870d5de12c8 -size 1950 + + + + + + image/svg+xml + + + + + + + + diff --git a/docs/assets/icon-star-half.svg b/docs/assets/icon-star-half.svg index 11398fa6f1b7ef..ab905fddcb4de2 100644 --- a/docs/assets/icon-star-half.svg +++ b/docs/assets/icon-star-half.svg @@ -1,3 +1,66 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cdf5e7ae57626f4cda35973873a4696252babbbe473913fe0aafdd8ba39d7fc3 -size 2508 + + + + + + image/svg+xml + + + + + + + + + + diff --git a/docs/assets/icon-youtube.svg b/docs/assets/icon-youtube.svg deleted file mode 100644 index f738dca10a6f09..00000000000000 --- a/docs/assets/icon-youtube.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6cb64f9da10b818c56763a7c48347f6043da20a2a77fb14f6d60d9457c575b6b -size 1278 diff --git a/docs/assets/three-back.svg b/docs/assets/three-back.svg deleted file mode 100644 index 4dfeeeb46a502a..00000000000000 --- a/docs/assets/three-back.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8a5245f9458982b608fee67fc689d899ca638a405ff62bf9db5e4978b177ef3e -size 121394 diff --git a/docs/c_docs.rst b/docs/c_docs.rst new file mode 100644 index 00000000000000..77be7e51d89cc1 --- /dev/null +++ b/docs/c_docs.rst @@ -0,0 +1,97 @@ +openpilot +========== + + +opendbc +------ +.. autodoxygenindex:: + :project: opendbc_can + + +cereal +------ + +messaging +^^^^^^^^^ +.. autodoxygenindex:: + :project: cereal_messaging + +visionipc +^^^^^^^^^ +.. autodoxygenindex:: + :project: cereal_visionipc + + +selfdrive +--------- + +camerad +^^^^^^^ +.. autodoxygenindex:: + :project: system_camerad_cameras +.. autodoxygenindex:: + :project: system_camerad_imgproc + +locationd +^^^^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_locationd + +ui +^^ + +.. autodoxygenindex:: + :project: selfdrive_ui + +soundd +"""""" +.. autodoxygenindex:: + :project: selfdrive_ui_soundd + + +replay +"""""" +.. autodoxygenindex:: + :project: tools_replay + +qt +"" +.. autodoxygenindex:: + :project: selfdrive_ui_qt_offroad +.. autodoxygenindex:: + :project: selfdrive_ui_qt_maps + +proclogd +^^^^^^^^ +.. autodoxygenindex:: + :project: system_proclogd + +modeld +^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_modeld_transforms +.. autodoxygenindex:: + :project: selfdrive_modeld_models +.. autodoxygenindex:: + :project: selfdrive_modeld_runners + +common +^^^^^^ +.. autodoxygenindex:: + :project: common + +sensorsd +^^^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_sensord_sensors + +boardd +^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_boardd + + +rednose +------- +.. autodoxygenindex:: + :project: rednose_repo_rednose_helpers diff --git a/docs/car-porting/brand-port.md b/docs/car-porting/brand-port.md deleted file mode 100644 index a3daa7a84850f3..00000000000000 --- a/docs/car-porting/brand-port.md +++ /dev/null @@ -1,5 +0,0 @@ -# Developing a car brand port - -A brand port is a port of openpilot to a substantially new car brand or platform within a brand. - -Here's an example of one: https://github.com/commaai/openpilot/pull/23331. diff --git a/docs/car-porting/car-state-signals.md b/docs/car-porting/car-state-signals.md deleted file mode 100644 index 669bd0ee2372b4..00000000000000 --- a/docs/car-porting/car-state-signals.md +++ /dev/null @@ -1,65 +0,0 @@ -# CarState signals - -## Required for basic lateral control - -* `brakePressed` -* `cruiseState` -* `doorOpen` -* `espDisabled` -* `gasPressed` -* `gearShifter` -* `leftBlinker` / `rightBlinker` -* `seatbeltUnlatched` -* `standstill` -* `steeringAngleDeg` -* `steeringPressed` -* `steeringTorque` -* `steerFaultPermanent` -* `steerFaultTemporary` -* `vCruise` -* `wheelSpeeds.[fl|fr|rl|rr]`: Speed of each of the car's four wheels, in m/s. The car's CAN bus often broadcasts the -speed in kph, so the helper function `parse_wheel_speeds` performs this conversion by default. - -## Recommended / Required for openpilot longitudinal control - -* `accFaulted` -* `espActive` -* `parkingBrake` - -## Application Dependent - -* `blockPcmEnable` -* `buttonEnable` -* `brakeHoldActive` -* `carFaultedNonCritical` -* `invalidLkasSetting` -* `lowSpeedAlert` -* `regenBraking` -* `steeringAngleOffsetDeg` -* `steeringDisengage` -* `steeringTorqueEps` -* `stockLkas` -* `vCruiseCluster` -* `vEgoCluster` -* `vehicleSensorsInvalid` - -## Automatically populated - -* `buttonEvents` - -These values are populated automatically by `parse_wheel_speeds`: - -* `aEgo`: Acceleration of the ego vehicle, Kalman filtered derivative of `vEgo`. -* `vEgo`: Speed of the ego vehicle, Kalman filtered from `vEgoRaw`. -* `vEgoRaw`: Speed of the ego vehicle, based on the average of all four wheel speeds, unfiltered. - -## Optional - -* `brake` -* `charging` -* `fuelGauge` -* `leftBlindspot` / `rightBlindspot` -* `steeringRateDeg` -* `stockAeb` -* `stockFcw` -* `yawRate` diff --git a/docs/car-porting/model-port.md b/docs/car-porting/model-port.md deleted file mode 100644 index e148a40ecb131b..00000000000000 --- a/docs/car-porting/model-port.md +++ /dev/null @@ -1,5 +0,0 @@ -# Developing a car model port - -A model port is a port of openpilot to a new car model within an already supported brand. Model ports are easier than brand ports because the car's existing APIs are already known. - -Here's an example of one: https://github.com/commaai/openpilot/pull/30672/. diff --git a/docs/car-porting/reverse-engineering.md b/docs/car-porting/reverse-engineering.md deleted file mode 100644 index 128ec8e7769968..00000000000000 --- a/docs/car-porting/reverse-engineering.md +++ /dev/null @@ -1,85 +0,0 @@ -# Stimulus-Response Tests - -These are example test drives that can help identify the CAN bus messaging necessary for ADAS control. Each scripted -test should be done in a separate route (ignition cycle). These tests are a guide, not necessarily exhaustive. - -While testing, constant power to the comma device is highly recommended, using [comma power](https://comma.ai/shop/comma-power) if -necessary to make sure all test activity is fully captured and for ease of uploading. If constant power isn't -available, keep the ignition on for at least one minute after your test to make sure power loss doesn't result -in loss of the last minute of testing data. - -## Stationary ignition-only tests, part 1 - -1. Ignition on, but don't start engine, remain in Park -2. Open and close each door in a defined order: driver, passenger, rear left, rear right -3. Re-enter the vehicle, close the driver's door, and fasten the driver's seatbelt -4. Slowly press and release the accelerator pedal 3 times -5. Slowly press and release the brake pedal 3 times -6. Hold the brake and move the gearshift to reverse, then neutral, then drive, then sport/eco/etc if applicable -7. Return to Park, ignition off - -Brake-pressed information may show up in several messages and signals, both as on/off states and as a percentage or -pressure. It may reflect a switch on the driver's brake pedal, or a pressure-threshold state, or signals to turn on -the rear brake lights. Start by identifying all the potential signals, and confirm while driving with ACC later. - -Locate signals for all four door states if possible, but some cars only expose the driver's door state on the ADAS bus. -Driver/passenger door signals may or may not change positions for LHD vs RHD cars. For cars where only the driver's -door signal is available, the same signal may follow the driver. - -## Stationary ignition-only tests, part 2 - -1. Ignition on, but don't start engine, remain in Park -2. Press each ACC button in a defined order: main switch on/off, set, resume, cancel, accel, decel, gap adjust -3. Set the left turn signal for about five seconds -4. Operate the left turn signal one time in its touch-to-pass mode -5. Set the right turn signal for about five seconds -6. Operate the right turn signal one time in its touch-to-pass mode -7. Set the hazard / emergency indicator switch for about five seconds -8. Ignition off - -Your vehicle may have a momentary-press main ACC switch or a physical toggle that remains set. Actual ACC engagement -isn't necessary for purposes of detecting the ACC button presses. - -## Steering angle and steering torque tests - -Power steering should be available. On ICE cars, engine RPM may be present. - -1. Ignition on, start engine if applicable, remain in Park -2. Rotate the steering wheel as follows, with a few seconds pause between each step - * Start as close to exact center as possible - * Turn to 45 degrees right and hold - * Turn to 90 degrees right and hold - * Turn to 180 degrees right and hold - * Turn to full lock right and hold, with firm pressure against lock - * Release the wheel and allow it to bounce back slightly from lock - * Turn to 180 degrees left and hold - * Return to center and release -3. Ignition off - -Performing the full test to the right, followed by an abbreviated test to the left, helps give additional confirmation -of signal scale, and sign/direction for both the steering wheel angle and driver input torque signals. - -## Low speed / parking lot driving tests - -Before this test, drive to a place like an empty parking lot where you are free to drive in a series of curves. - -1. Ignition on, start engine if applicable, prepare to drive -2. Slowly (10-20mph at most) drive a figure-8 if possible, or at least one sharp left and one sharp right. -3. Come to a complete stop -4. When and where safe, drive in reverse for a short distance (10-15 feet) -5. Park the car in a safe place, ignition off - -## High speed / highway driving tests - -Select a place and time where you can safely set cruise control at normal travel speeds with little interference from -traffic ahead, and safely test the response of your factory lane guidance system. - -1. Ignition on, start engine if applicable, prepare to drive -2. When safely able, engage adaptive cruise control below 50 mph -3. When safely able, use the ACC buttons to accelerate to 50mph, then 55mph, then 60mph -4. Disengage adaptive cruise -5. When safely able, allow your factory lane guidance to prevent lane departures, 2-3 times on both the left and right - -The series of setpoints can be adjusted to local traffic regulations, and of course metric units. The specific cruise -setpoints are useful for locating the ACC HUD signals later, and confirming their precise scaling. When the car reaches -and holds the setpoint, that can also provide additional confirmation of wheel speed scaling. diff --git a/docs/car-porting/what-is-a-car-port.md b/docs/car-porting/what-is-a-car-port.md deleted file mode 100644 index 55cce94da1f39d..00000000000000 --- a/docs/car-porting/what-is-a-car-port.md +++ /dev/null @@ -1,39 +0,0 @@ -# What is a car port? - -A car port enables openpilot support on a particular car. Each car model openpilot supports needs to be individually ported. The complexity of a car port varies depending on many factors including: - -* existing openpilot support for similar cars -* architecture and APIs available in the car - - -# Structure of a car port - -Virtually all car-specific code is contained in two other repositories: [opendbc](https://github.com/commaai/opendbc) and [panda](https://github.com/commaai/panda). - -## opendbc - -Each car brand is supported by a standard interface structure in `opendbc/car/[brand]`: - -* `interface.py`: Interface for the car, defines the CarInterface class -* `carstate.py`: Reads CAN messages from the car and builds openpilot CarState messages -* `carcontroller.py`: Control logic for executing openpilot CarControl actions on the car -* `[brand]can.py`: Composes CAN messages for carcontroller to send -* `values.py`: Limits for actuation, general constants for cars, and supported car documentation -* `radar_interface.py`: Interface for parsing radar points from the car, if applicable - -## panda - -* `board/safety/safety_[brand].h`: Brand-specific safety logic -* `tests/safety/test_[brand].py`: Brand-specific safety CI tests - -## openpilot - -For historical reasons, openpilot still contains a small amount of car-specific logic. This will eventually be migrated to opendbc or otherwise removed. - -* `selfdrive/car/car_specific.py`: Brand-specific event logic - -# Overview - -[Jason Young](https://github.com/jyoung8607) gave a talk at COMMA_CON with an overview of the car porting process. The talk is available on YouTube: - -https://www.youtube.com/watch?v=XxPS5TpTUnI diff --git a/docs/concepts/glossary.md b/docs/concepts/glossary.md deleted file mode 100644 index a09b0f07853e78..00000000000000 --- a/docs/concepts/glossary.md +++ /dev/null @@ -1,9 +0,0 @@ -# openpilot glossary - -* **onroad**: openpilot's system state while ignition is on -* **offroad**: openpilot's system state while ignition is off -* **route**: a route is a recording of an onroad session -* **segment**: routes are split into one minute chunks called segments. -* **comma connect**: the web viewer for all your routes; check it out at [connect.comma.ai](https://connect.comma.ai). -* **panda**: this is the secondary processor on the device that implements the functional safety and directly talks to the car over CAN. See the [panda repo](https://github.com/commaai/panda). -* **comma 3X**: the latest hardware by comma.ai for running openpilot. more info at [comma.ai/shop](https://comma.ai/shop). diff --git a/docs/concepts/logs.md b/docs/concepts/logs.md deleted file mode 100644 index 46ab2897df9eec..00000000000000 --- a/docs/concepts/logs.md +++ /dev/null @@ -1,29 +0,0 @@ -# Logging - -openpilot records routes in one minute chunks called segments. A route starts on the rising edge of ignition and ends on the falling edge. - -Check out our [Python library](https://github.com/commaai/openpilot/blob/master/tools/lib/logreader.py) for reading openpilot logs. Also checkout our [tools](https://github.com/commaai/openpilot/tree/master/tools) to replay and view your data. These are the same tools we use to debug and develop openpilot. - -For each segment, openpilot records the following log types: - -## rlog.bz2 - -rlogs contain all the messages passed amongst openpilot's processes. See [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for a list of all the logged services. They're a bzip2 archive of the serialized capnproto messages. - -## {f,e,d}camera.hevc - -Each camera stream is H.265 encoded and written to its respective file. - -* `fcamera.hevc` is the road camera -* `ecamera.hevc` is the wide road camera -* `dcamera.hevc` is the driver camera - -## qlog.bz2 & qcamera.ts - -qlogs are a decimated subset of the rlogs. Check out [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for the decimation. - - -qcameras are H.264 encoded, lower res versions of the fcamera.hevc. The video shown in [comma connect](https://connect.comma.ai/) is from the qcameras. - - -qlogs and qcameras are designed to be small enough to upload instantly on slow internet and store forever, yet useful enough for most analysis and debugging. diff --git a/docs/concepts/safety.md b/docs/concepts/safety.md deleted file mode 120000 index f286ad4b152ffc..00000000000000 --- a/docs/concepts/safety.md +++ /dev/null @@ -1 +0,0 @@ -../SAFETY.md \ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 00000000000000..fea921de1f0e54 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,148 @@ +# type: ignore +# pylint: skip-file + +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +from os.path import exists + +from common.basedir import BASEDIR +from system.version import get_version + +sys.path.insert(0, os.path.abspath('.')) +sys.path.insert(0, os.path.abspath('..')) + +VERSION = get_version() + + +# -- Project information ----------------------------------------------------- + +project = 'openpilot docs' +copyright = '2021, comma.ai' +author = 'comma.ai' +version = VERSION +release = VERSION +language = 'en' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', # Auto-generate docs + 'sphinx.ext.viewcode', # Add view code link to modules + 'sphinx_rtd_theme', # Read The Docs theme + 'myst_parser', # Markdown parsing + 'breathe', # Doxygen C/C++ integration + 'sphinx_sitemap', # sitemap generation for SEO +] + +myst_html_meta = { + "description": "openpilot docs", + "keywords": "op, openpilot, docs, documentation", + "robots": "all,follow", + "googlebot": "index,follow,snippet,archive", + "property=og:locale": "en_US", + "property=og:site_name": "docs.comma.ai", + "property=og:url": "https://docs.comma.ai", + "property=og:title": "openpilot Documentation", + "property=og:type": "website", + "property=og:image:type": "image/jpeg", + "property=og:image:width": "400", + "property=og:image": "https://docs.comma.ai/_static/logo.png", + "property=og:image:url": "https://docs.comma.ai/_static/logo.png", + "property=og:image:secure_url": "https://docs.comma.ai/_static/logo.png", + "property=og:description": "openpilot Documentation", + "property=twitter:card": "summary_large_image", + "property=twitter:logo": "https://docs.comma.ai/_static/logo.png", + "property=twitter:title": "openpilot Documentation", + "property=twitter:description": "openpilot Documentation" +} + +html_baseurl = 'https://docs.comma.ai/' +sitemap_filename = "sitemap.xml" + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + + +# -- c docs configuration --------------------------------------------------- + +# Breathe Configuration +# breathe_default_project = "c_docs" +breathe_build_directory = f"{BASEDIR}/build/docs/html/xml" +breathe_separate_member_pages = True +breathe_default_members = ('members', 'private-members', 'undoc-members') +breathe_domain_by_extension = { + "h": "cc", +} +breathe_implementation_filename_extensions = ['.c', '.cc'] +breathe_doxygen_config_options = {} +breathe_projects_source = {} + +# only document files that have accompanying .cc files next to them +print("searching for c_docs...") +for root, dirs, files in os.walk(BASEDIR): + found = False + breath_src = {} + breathe_srcs_list = [] + + for file in files: + ccFile = os.path.join(root, file)[:-2] + ".cc" + + if file.endswith(".h") and exists(ccFile): + f = os.path.join(root, file) + + parent_dir_abs = os.path.dirname(f) + parent_dir = parent_dir_abs[len(BASEDIR) + 1:] + parent_project = parent_dir.replace('/', '_') + print(f"\tFOUND: {f} in {parent_project}") + + breathe_srcs_list.append(file) + found = True + + if found: + breath_src[parent_project] = (parent_dir_abs, breathe_srcs_list) + breathe_projects_source.update(breath_src) + +print(f"breathe_projects_source: {breathe_projects_source.keys()}") + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'sphinx_rtd_theme' +html_show_copyright = True + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] +html_logo = '_static/logo.png' +html_favicon = '_static/favicon.ico' +html_theme_options = { + 'logo_only': False, + 'display_version': True, + 'vcs_pageview_mode': 'blob', + 'style_nav_header_background': '#000000', +} +html_extra_path = ['_static'] diff --git a/docs/contributing/architecture.md b/docs/contributing/architecture.md deleted file mode 100644 index c79bec1ac69aea..00000000000000 --- a/docs/contributing/architecture.md +++ /dev/null @@ -1 +0,0 @@ -# Architecture diff --git a/docs/contributing/roadmap.md b/docs/contributing/roadmap.md deleted file mode 100644 index 1262017a0b817c..00000000000000 --- a/docs/contributing/roadmap.md +++ /dev/null @@ -1,31 +0,0 @@ -# Roadmap - -This is the roadmap for the next major openpilot releases. Also check out - -* [Milestones](https://github.com/commaai/openpilot/milestones) for minor releases -* [Projects](https://github.com/commaai/openpilot/projects?query=is%3Aopen) for shorter-term projects not tied to releases -* [Bounties](https://comma.ai/bounties) for paid individual issues -* [#current-projects](https://discord.com/channels/469524606043160576/1249579909739708446) in Discord for discussion on work-in-progress projects - -## openpilot 0.10 - -openpilot 0.10 will be the first release with a driving policy trained in -a [learned simulator](https://youtu.be/EqQNZXqzFSI). - -* Driving model trained in a learned simulator -* Always-on driver monitoring (behind a toggle) -* GPS removed from the driving stack -* 100KB qlogs -* `nightly` pushed after 1000 hours of hardware-in-the-loop testing -* Car interface code moved into [opendbc](https://github.com/commaai/opendbc) -* openpilot on PC for Linux x86, Linux arm64, and Mac (Apple Silicon) - -## openpilot 1.0 - -openpilot 1.0 will feature a fully end-to-end driving policy. - -* End-to-end longitudinal control in Chill mode -* Automatic Emergency Braking (AEB) -* Driver monitoring with sleep detection -* Rolling updates/releases pushed out by CI -* [panda safety 1.0](https://github.com/orgs/commaai/projects/27) diff --git a/docs/css/tooltip.css b/docs/css/tooltip.css deleted file mode 100644 index b9a54f793f9fe5..00000000000000 --- a/docs/css/tooltip.css +++ /dev/null @@ -1,44 +0,0 @@ -[data-tooltip] { - position: relative; - display: inline-block; - border-bottom: 1px dotted black; -} - -[data-tooltip] .tooltip-content { - width: max-content; - max-width: 25em; - position: absolute; - top: 100%; - left: 50%; - transform: translateX(-50%); - background-color: white; - color: #404040; - box-shadow: 0 4px 14px 0 rgba(0,0,0,.2), 0 0 0 1px rgba(0,0,0,.05); - padding: 10px; - font: 14px/1.5 Lato, proxima-nova, Helvetica Neue, Arial, sans-serif; - text-decoration: none; - opacity: 0; - visibility: hidden; - transition: opacity 0.1s, visibility 0s; - z-index: 1000; - pointer-events: none; /* Prevent accidental interaction */ -} - -[data-tooltip]:hover .tooltip-content { - opacity: 1; - visibility: visible; - pointer-events: auto; /* Allow interaction when visible */ -} - -.tooltip-content .tooltip-glossary-link { - display: inline-block; - margin-top: 8px; - font-size: 12px; - color: #007bff; - text-decoration: none; -} - -.tooltip-content .tooltip-glossary-link:hover { - color: #0056b3; - text-decoration: underline; -} diff --git a/docs/docker/Dockerfile b/docs/docker/Dockerfile new file mode 100644 index 00000000000000..cefe9be855d955 --- /dev/null +++ b/docs/docker/Dockerfile @@ -0,0 +1,46 @@ +FROM ghcr.io/commaai/openpilot-base:latest + +ENV PYTHONUNBUFFERED 1 + +ENV OPENPILOT_PATH /home/batman/openpilot/ +ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} + +RUN mkdir -p ${OPENPILOT_PATH} +WORKDIR ${OPENPILOT_PATH} + +COPY Pipfile Pipfile.lock $OPENPILOT_PATH +RUN pip install --no-cache-dir pipenv==2021.5.29 pip==21.3.1 && \ + pipenv install --system --deploy --dev --clear && \ + pip uninstall -y pipenv + + +COPY SConstruct ${OPENPILOT_PATH} + +COPY ./pyextra ${OPENPILOT_PATH}/pyextra +COPY ./third_party ${OPENPILOT_PATH}/third_party +COPY ./site_scons ${OPENPILOT_PATH}/site_scons +COPY ./laika ${OPENPILOT_PATH}/laika +COPY ./laika_repo ${OPENPILOT_PATH}/laika_repo +COPY ./rednose ${OPENPILOT_PATH}/rednose +COPY ./rednose_repo ${OPENPILOT_PATH}/rednose_repo +COPY ./tools ${OPENPILOT_PATH}/tools +COPY ./release ${OPENPILOT_PATH}/release +COPY ./common ${OPENPILOT_PATH}/common +COPY ./opendbc ${OPENPILOT_PATH}/opendbc +COPY ./cereal ${OPENPILOT_PATH}/cereal +COPY ./panda ${OPENPILOT_PATH}/panda +COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive +COPY ./system ${OPENPILOT_PATH}/system +COPY ./*.md ${OPENPILOT_PATH}/ + +RUN scons -j$(nproc) + +RUN apt update && apt install doxygen -y +COPY ./docs ${OPENPILOT_PATH}/docs +RUN git init . +WORKDIR ${OPENPILOT_PATH}/docs +RUN make html + +FROM nginx:1.21 +COPY --from=0 /home/batman/openpilot/build/docs/html /usr/share/nginx/html +COPY ./docs/docker/nginx.conf /etc/nginx/conf.d/default.conf diff --git a/docs/docker/nginx.conf b/docs/docker/nginx.conf new file mode 100644 index 00000000000000..21fb2263687679 --- /dev/null +++ b/docs/docker/nginx.conf @@ -0,0 +1,15 @@ +server { + listen 80; + listen [::]:80; + server_name localhost; + + gzip on; + gzip_types text/html text/plain text/css text/xml text/javascript application/javascript application/x-javascript; + gzip_min_length 1024; + gzip_vary on; + + root /usr/share/nginx/html; + location / { + try_files $uri $uri/ /index.html; + } +} diff --git a/docs/getting-started/what-is-openpilot.md b/docs/getting-started/what-is-openpilot.md deleted file mode 100644 index b3c56c8410d803..00000000000000 --- a/docs/getting-started/what-is-openpilot.md +++ /dev/null @@ -1,12 +0,0 @@ -# What is openpilot? - -[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW), and Lane Departure Warning (LDW) for a growing variety of [supported car makes, models, and model years](https://github.com/commaai/openpilot/blob/master/docs/CARS.md). In addition, while openpilot is engaged, a camera-based Driver Monitoring (DM) feature alerts distracted and asleep drivers. See more about [the vehicle integration](https://github.com/commaai/openpilot/blob/master/docs/INTEGRATION.md) and [limitations](https://github.com/commaai/openpilot/blob/master/docs/LIMITATIONS.md). - - -## How do I use it? - -openpilot is designed to be used on the comma 3X. - -## How does it work? - -In short, openpilot uses the car's existing APIs for the built-in [ADAS](https://en.wikipedia.org/wiki/Advanced_driver-assistance_system) system and simply provides better acceleration, braking, and steering inputs than the stock system. diff --git a/docs/hooks/glossary.py b/docs/hooks/glossary.py deleted file mode 100644 index e2fa3d51e04cb9..00000000000000 --- a/docs/hooks/glossary.py +++ /dev/null @@ -1,68 +0,0 @@ -import re -import tomllib - -def load_glossary(file_path="docs/glossary.toml"): - with open(file_path, "rb") as f: - glossary_data = tomllib.load(f) - return glossary_data.get("glossary", {}) - -def generate_anchor_id(name): - return name.replace(" ", "-").replace("_", "-").lower() - -def format_markdown_term(name, definition): - anchor_id = generate_anchor_id(name) - markdown = f"* [**{name.replace('_', ' ').title()}**](#{anchor_id})" - if definition.get("abbreviation"): - markdown += f" *({definition['abbreviation']})*" - if definition.get("description"): - markdown += f": {definition['description']}\n" - return markdown - -def glossary_markdown(vocabulary): - markdown = "" - for category, terms in vocabulary.items(): - markdown += f"## {category.replace('_', ' ').title()}\n\n" - for name, definition in terms.items(): - markdown += format_markdown_term(name, definition) - return markdown - -def format_tooltip_html(term_key, definition, html): - display_term = term_key.replace("_", " ").title() - clean_description = re.sub(r"\[(.+)]\(.+\)", r"\1", definition["description"]) - glossary_link = ( - f"Glossary🔗" - ) - return re.sub( - re.escape(display_term), - lambda - match: f"{match.group(0)}{clean_description} {glossary_link}", - html, - flags=re.IGNORECASE, - ) - -def apply_tooltip(_term_key, _definition, pattern, html): - return re.sub( - pattern, - lambda match: format_tooltip_html(_term_key, _definition, match.group(0)), - html, - flags=re.IGNORECASE, - ) - -def tooltip_html(vocabulary, html): - for _category, terms in vocabulary.items(): - for term_key, definition in terms.items(): - if definition.get("description"): - pattern = rf"(?)(?!\([^)]*\))" - html = apply_tooltip(term_key, definition, pattern, html) - return html - -# Page Hooks -def on_page_markdown(markdown, **kwargs): - glossary = load_glossary() - return markdown.replace("{{GLOSSARY_DEFINITIONS}}", glossary_markdown(glossary)) - -def on_page_content(html, **kwargs): - if kwargs.get("page").title == "Glossary": - return html - glossary = load_glossary() - return tooltip_html(glossary, html) diff --git a/docs/how-to/connect-to-comma.md b/docs/how-to/connect-to-comma.md deleted file mode 100644 index 5f02e115994cb1..00000000000000 --- a/docs/how-to/connect-to-comma.md +++ /dev/null @@ -1,99 +0,0 @@ -# connect to a comma 3X - -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 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. - -On the comma 3X, the serial console is accessible through the [panda](https://github.com/commaai/panda) using the `panda/tests/som_debug.sh` script. - - * Username: `comma` - * Password: `comma` - -## SSH - -In order to SSH into your device, you'll need a GitHub account with SSH keys. See this [GitHub article](https://docs.github.com/en/github/authenticating-to-github/connecting-to-github-with-ssh) for getting your account setup with SSH keys. - -* Enable SSH in your device's settings -* Enter your GitHub username in the device's settings -* Connect to your device - * Username: `comma` - * Port: `22` - -Here's an example command for connecting to your device using its tethered connection:
    -`ssh comma@192.168.43.1` - -For doing development work on device, it's recommended to use [SSH agent forwarding](https://docs.github.com/en/developers/overview/using-ssh-agent-forwarding). - - -## ADB - -In order to use ADB on your device, you'll need to perform the following steps using the image below for reference: - -![comma 3/3x back](../assets/three-back.svg) - -* Plug your device into constant power using port 2, letting the device boot up -* Enable ADB in your device's settings -* Plug in your device to your PC using port 1 -* Connect to your device - * `adb shell` over USB - * `adb connect` over WiFi - * 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 3X. - -For more info on ADB, see the [Android Debug Bridge (ADB) documentation](https://developer.android.com/tools/adb). - -### Notes - -The public keys are only fetched from your GitHub account once. In order to update your device's authorized keys, you'll need to re-enter your GitHub username. - -The `id_rsa` key in this directory only works while your device is in the setup state with no software installed. After installation, that default key will be removed. - -#### ssh.comma.ai proxy - -With a [comma prime subscription](https://comma.ai/connect), you can SSH into your comma device from anywhere. - -With the below SSH configuration, you can type `ssh comma-{dongleid}` to connect to your device through `ssh.comma.ai`. - -``` -Host comma-* - Port 22 - User comma - IdentityFile ~/.ssh/my_github_key - ProxyCommand ssh %h@ssh.comma.ai -W %h:%p - -Host ssh.comma.ai - Hostname ssh.comma.ai - Port 22 - IdentityFile ~/.ssh/my_github_key -``` - -### One-off connection - -``` -ssh -i ~/.ssh/my_github_key -o ProxyCommand="ssh -i ~/.ssh/my_github_key -W %h:%p -p %p %h@ssh.comma.ai" comma@ffffffffffffffff -``` -(Replace `ffffffffffffffff` with your dongle_id) - -### ssh.comma.ai host key fingerprint - -``` -Host key fingerprint is SHA256:X22GOmfjGb9J04IA2+egtdaJ7vW9Fbtmpz9/x8/W1X4 -+---[RSA 4096]----+ -| | -| | -| . | -| + o | -| S = + +..| -| + @ = .=| -| . B @ ++=| -| o * B XE| -| .o o OB/| -+----[SHA256]-----+ -``` diff --git a/docs/how-to/replay-a-drive.md b/docs/how-to/replay-a-drive.md deleted file mode 100644 index b0db36a46f0ecd..00000000000000 --- a/docs/how-to/replay-a-drive.md +++ /dev/null @@ -1,14 +0,0 @@ -# Replay - -Replaying is a critical tool for openpilot development and debugging. - -## Replaying a route -*Hardware required: none* - -Just run `tools/replay/replay --demo`. - -## Replaying CAN data -*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 deleted file mode 100644 index 644c35e0abe1b1..00000000000000 --- a/docs/how-to/turn-the-speed-blue.md +++ /dev/null @@ -1,111 +0,0 @@ -# Turn the speed blue -*A getting started guide for openpilot development* - -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 3X, we'll deploy the change to your device for testing. - -## 1. Set up your development environment - -Run this to clone openpilot and install all the dependencies: -```bash -bash <(curl -fsSL openpilot.comma.ai) -``` - -Navigate to openpilot folder & activate a Python virtual environment -```bash -cd openpilot -source .venv/bin/activate -``` - -Then, compile openpilot: -```bash -scons -j$(nproc) -``` - -## 2. Run replay - -We'll run the `replay` tool with the demo route to get data streaming for testing our UI changes. -```bash -# in terminal 1 -tools/replay/replay --demo - -# in terminal 2 -./selfdrive/ui/ui.py -``` - -The openpilot UI should launch and show a replay of the demo route. - -If you have your own comma device, you can replace `--demo` with one of your own routes from comma connect. - - -## 3. Make the speed blue - -Now let’s update the speed display color in the UI. - -Search for the function responsible for rendering the current speed: -```bash -git grep "_draw_current_speed" selfdrive/ui/onroad/hud_renderer.py -``` - -You'll find the relevant code inside `selfdrive/ui/onroad/hud_renderer.py`, in this function: - -```python -def _draw_current_speed(self, rect: rl.Rectangle) -> None: - """Draw the current vehicle speed and unit.""" - speed_text = str(round(self.speed)) - speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed) - speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2) - rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.white) # <- this sets the speed text color -``` - -Change `COLORS.white` to make it **blue** instead of white. A nice soft blue is `#8080FF`, which you can change inline: - -```diff -- rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.white) -+ rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, rl.Color(0x80, 0x80, 0xFF, 255)) -``` - ---- - -## 4. Re-run the UI - -After making changes, re-run the UI to see your new UI: -```bash -./selfdrive/ui/ui.py -``` -![](https://blog.comma.ai/img/blue_speed_ui.png) - -You should now see the speed displayed in a nice blue shade during the demo replay. - ---- - -## 5. Push your fork to GitHub - -Click **"Fork"** on the [Openpilot GitHub repo](https://github.com/commaai/openpilot). Then push with: -```bash -git remote rm origin -git remote add origin git@github.com:/openpilot.git -git add . -git commit -m "Make the speed display blue" -git push --set-upstream origin master -``` - ---- - -## 6. Run your fork on your comma device - -Uninstall Openpilot through the settings on your device. - -Then reinstall using your own GitHub-hosted fork: -``` -installer.comma.ai//master -``` - ---- - -## 7. Admire your work IRL 🚗💨 - -You’ve now successfully modified Openpilot’s UI and deployed it to your own car! - -![](https://blog.comma.ai/img/c3_blue_ui.jpg) diff --git a/docs/index.md b/docs/index.md deleted file mode 120000 index 74ea27aeeb6b72..00000000000000 --- a/docs/index.md +++ /dev/null @@ -1 +0,0 @@ -getting-started/what-is-openpilot.md \ No newline at end of file diff --git a/docs/index.md b/docs/index.md new file mode 100644 index 00000000000000..0fb2617a5b74ab --- /dev/null +++ b/docs/index.md @@ -0,0 +1,42 @@ +# openpilot Documentation + +```{include} README.md +``` + +```{toctree} +:caption: 'General' +:maxdepth: 4 + +CARS.md +CONTRIBUTING.md +INTEGRATION.md +LIMITATIONS.md +SAFETY.md +``` + +```{toctree} +:caption: 'Overview' +:maxdepth: 2 + +overview.rst +``` + +## API Documentation + +- {ref}`genindex` +- {ref}`modindex` +- {ref}`search` + +```{toctree} +:caption: 'Python API' +:maxdepth: 2 + +modules.rst +``` + +```{toctree} +:caption: 'C/C++ API' +:maxdepth: 4 + +c_docs.rst +``` \ No newline at end of file diff --git a/docs/overview.rst b/docs/overview.rst new file mode 100644 index 00000000000000..cda51ba3d44f75 --- /dev/null +++ b/docs/overview.rst @@ -0,0 +1,79 @@ +openpilot +========= + +.. toctree:: + :maxdepth: 4 + + Debugging + selfdrive/loggerd/README.md + Driver Monitoring + Process Replay + +cereal +========= + +.. toctree:: + :maxdepth: 4 + + cereal/README.md + cereal/messaging/msgq.md + +laika +========= + +.. toctree:: + :maxdepth: 4 + + laika_repo/README.md + +models +========= + +.. toctree:: + :maxdepth: 4 + + models/README.md + +opendbc +========= + +.. toctree:: + :maxdepth: 4 + + opendbc/README.md + +panda +========= + +.. toctree:: + :maxdepth: 4 + + panda/README.md + panda/UPDATING.md + panda/board/README.md + panda/drivers/linux/README.md + panda/drivers/windows/README.md + + +rednose +========= +.. toctree:: + :maxdepth: 4 + + rednose_repo/README.md + + +tools +========= +.. toctree:: + :maxdepth: 4 + + tools/CTF.md + tools/joystick/README.md + tools/lib/README.md + tools/plotjuggler/README.md + tools/replay/README.md + tools/serial/README.md + Simulator + tools/ssh/README.md + Webcam diff --git a/laika b/laika new file mode 120000 index 00000000000000..1c7429f4acb62e --- /dev/null +++ b/laika @@ -0,0 +1 @@ +laika_repo/laika/ \ No newline at end of file diff --git a/laika_repo b/laika_repo new file mode 160000 index 00000000000000..c8bc1fa01be9f2 --- /dev/null +++ b/laika_repo @@ -0,0 +1 @@ +Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index d4689aae53aa1b..911774a4eba1df 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -1,21 +1,25 @@ -#!/usr/bin/env bash +#!/usr/bin/bash -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +if [ -z "$BASEDIR" ]; then + BASEDIR="/data/openpilot" +fi + +source "$BASEDIR/launch_env.sh" -source "$DIR/launch_env.sh" +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" function agnos_init { + # wait longer for weston to come up + if [ -f "$BASEDIR/prebuilt" ]; then + sleep 3 + fi + # TODO: move this to agnos sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta # set success flag for current boot slot sudo abctl --set_success - # TODO: do this without udev in AGNOS - # udev does this, but sometimes we startup faster - sudo chgrp gpu /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0 - sudo chmod 660 /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0 - # Check if AGNOS update is required if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then AGNOS_PY="$DIR/system/hardware/tici/agnos.py" @@ -31,28 +35,31 @@ function launch { # Remove orphaned git lock if it exists on boot [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock + # Pull time from panda + $DIR/selfdrive/boardd/set_time.py + # Check to see if there's a valid overlay-based update available. Conditions # are as follows: # - # 1. The DIR init file has to exist, with a newer modtime than anything in - # the DIR Git repo. This checks for local development work or the user + # 1. The BASEDIR init file has to exist, with a newer modtime than anything in + # the BASEDIR Git repo. This checks for local development work or the user # switching branches/forks, which should not be overwritten. # 2. The FINALIZED consistent file has to exist, indicating there's an update # that completed successfully and synced to disk. - if [ -f "${DIR}/.overlay_init" ]; then - find ${DIR}/.git -newer ${DIR}/.overlay_init | grep -q '.' 2> /dev/null + if [ -f "${BASEDIR}/.overlay_init" ]; then + find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null if [ $? -eq 0 ]; then - echo "${DIR} has been modified, skipping overlay update installation" + echo "${BASEDIR} has been modified, skipping overlay update installation" else if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then if [ ! -d /data/safe_staging/old_openpilot ]; then echo "Valid overlay update found, installing" LAUNCHER_LOCATION="${BASH_SOURCE[0]}" - mv $DIR /data/safe_staging/old_openpilot - mv "${STAGING_ROOT}/finalized" $DIR - cd $DIR + mv $BASEDIR /data/safe_staging/old_openpilot + mv "${STAGING_ROOT}/finalized" $BASEDIR + cd $BASEDIR echo "Restarting launch script ${LAUNCHER_LOCATION}" unset AGNOS_VERSION @@ -67,22 +74,17 @@ function launch { # handle pythonpath ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD" + export PYTHONPATH="$PWD:$PWD/pyextra" # hardware specific init - if [ -f /AGNOS ]; then - agnos_init - fi + agnos_init # write tmux scrollback to a file tmux capture-pane -pq -S-1000 > /tmp/launch_log # start manager - cd system/manager - if [ ! -f $DIR/prebuilt ]; then - ./build.py - fi - ./manager.py + cd selfdrive/manager + ./build.py && ./manager.py # if broken, keep on screen error while true; do sleep 1; done diff --git a/launch_env.sh b/launch_env.sh index 314366f429ae41..ac84d6dcbde6f4 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash export OMP_NUM_THREADS=1 export MKL_NUM_THREADS=1 @@ -6,17 +6,12 @@ export NUMEXPR_NUM_THREADS=1 export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 -# models get lower priority than ui -# - ui is ~5ms -# - modeld is 20ms -# - DM is 10ms -# in order to run ui at 60fps (16.67ms), we need to allow -# it to preempt the model workloads. we have enough -# headroom for this until ui is moved to the CPU. -export QCOM_PRIORITY=12 - if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="16" + export AGNOS_VERSION="5.2" +fi + +if [ -z "$PASSIVE" ]; then + export PASSIVE="1" fi export STAGING_ROOT="/data/safe_staging" diff --git a/launch_openpilot.sh b/launch_openpilot.sh index d6e3424c348bf5..1525e1715f99c2 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,3 +1,5 @@ -#!/usr/bin/env bash +#!/usr/bin/bash +export PASSIVE="0" exec ./launch_chffrplus.sh + diff --git a/lgtm.yml b/lgtm.yml new file mode 100644 index 00000000000000..6ce93535620636 --- /dev/null +++ b/lgtm.yml @@ -0,0 +1,19 @@ +path_classifiers: + library: + - external + - third_party + - pyextra + - tools/lib/mkvparse +extraction: + cpp: + after_prepare: + - "pip3 install --upgrade --user pkgconfig cython setuptools wheel" + - "pip3 install --upgrade --user jinja2 pyyaml cython pycapnp numpy sympy tqdm\ + \ cffi logentries zmq scons" + - "export PATH=/opt/work/.local/bin:$PATH" + index: + build_command: "scons" + javascript: + index: + filters: + - exclude: "*" diff --git a/mkdocs.yml b/mkdocs.yml deleted file mode 100644 index 550f807aca02f3..00000000000000 --- a/mkdocs.yml +++ /dev/null @@ -1,44 +0,0 @@ -site_name: openpilot docs -repo_url: https://github.com/commaai/openpilot/ -site_url: https://docs.comma.ai - -exclude_docs: README.md - -strict: true -docs_dir: docs -site_dir: docs_site/ - -hooks: - - docs/hooks/glossary.py -extra_css: - - css/tooltip.css -theme: - name: readthedocs - navigation_depth: 3 - -nav: - - Getting Started: - - 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 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: - - Logs: concepts/logs.md - - Safety: concepts/safety.md - - Glossary: concepts/glossary.md - - Car Porting: - - What is a car port?: car-porting/what-is-a-car-port.md - - Porting a car brand: car-porting/brand-port.md - - Porting a car model: car-porting/model-port.md - - Contributing: - - Roadmap: contributing/roadmap.md - #- Architecture: contributing/architecture.md - - Contributing Guide →: https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md - - Links: - - Blog →: https://blog.comma.ai - - Bounties →: https://comma.ai/bounties - - GitHub →: https://github.com/commaai - - Discord →: https://discord.comma.ai - - X →: https://x.com/comma_ai diff --git a/msgq b/msgq deleted file mode 120000 index df09146f62ea88..00000000000000 --- a/msgq +++ /dev/null @@ -1 +0,0 @@ -msgq_repo/msgq \ No newline at end of file diff --git a/msgq_repo b/msgq_repo deleted file mode 160000 index 20f2493855ef32..00000000000000 --- a/msgq_repo +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 20f2493855ef32339b80f0ad76b3cb82210dc474 diff --git a/mypy.ini b/mypy.ini new file mode 100644 index 00000000000000..e2da60f926965c --- /dev/null +++ b/mypy.ini @@ -0,0 +1,4 @@ +[mypy] +python_version = 3.8 +ignore_missing_imports = True +plugins = numpy.typing.mypy_plugin diff --git a/opendbc b/opendbc deleted file mode 120000 index 7cd9a5bd1ebf2e..00000000000000 --- a/opendbc +++ /dev/null @@ -1 +0,0 @@ -opendbc_repo/opendbc \ No newline at end of file diff --git a/opendbc b/opendbc new file mode 160000 index 00000000000000..e95ed311c10547 --- /dev/null +++ b/opendbc @@ -0,0 +1 @@ +Subproject commit e95ed311c10547026143b539a33341425cbec9ea diff --git a/opendbc_repo b/opendbc_repo deleted file mode 160000 index 7c78ee87b7b54b..00000000000000 --- a/opendbc_repo +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7c78ee87b7b54bb2179d86d5e28c1f65bbf96669 diff --git a/openpilot/common b/openpilot/common deleted file mode 120000 index 60d3b0a6a8f6b0..00000000000000 --- a/openpilot/common +++ /dev/null @@ -1 +0,0 @@ -../common \ No newline at end of file diff --git a/openpilot/selfdrive b/openpilot/selfdrive deleted file mode 120000 index e005fd0d04dbca..00000000000000 --- a/openpilot/selfdrive +++ /dev/null @@ -1 +0,0 @@ -../selfdrive/ \ No newline at end of file diff --git a/openpilot/system b/openpilot/system deleted file mode 120000 index 16f8cc2b2334d4..00000000000000 --- a/openpilot/system +++ /dev/null @@ -1 +0,0 @@ -../system/ \ No newline at end of file diff --git a/openpilot/third_party b/openpilot/third_party deleted file mode 120000 index d838c05a86286e..00000000000000 --- a/openpilot/third_party +++ /dev/null @@ -1 +0,0 @@ -../third_party \ No newline at end of file diff --git a/openpilot/tools b/openpilot/tools deleted file mode 120000 index 4887d6e0c92bc5..00000000000000 --- a/openpilot/tools +++ /dev/null @@ -1 +0,0 @@ -../tools \ No newline at end of file diff --git a/panda b/panda index 81615ad9d53aef..8f13ca3f66bcc7 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 81615ad9d53aef5583e064f340e9cdeb23d4119c +Subproject commit 8f13ca3f66bcc72e3ac9028df7ce04851e7e3a48 diff --git a/third_party/.gitignore b/pyextra/.gitignore similarity index 100% rename from third_party/.gitignore rename to pyextra/.gitignore diff --git a/third_party/acados/acados_template/.gitignore b/pyextra/acados_template/.gitignore similarity index 100% rename from third_party/acados/acados_template/.gitignore rename to pyextra/acados_template/.gitignore diff --git a/pyextra/acados_template/__init__.py b/pyextra/acados_template/__init__.py new file mode 100644 index 00000000000000..f33b75bb7ba35b --- /dev/null +++ b/pyextra/acados_template/__init__.py @@ -0,0 +1,43 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +from .acados_model import * +from .generate_c_code_explicit_ode import * +from .generate_c_code_implicit_ode import * +from .generate_c_code_constraint import * +from .generate_c_code_nls_cost import * +from .acados_ocp import * +from .acados_sim import * +from .acados_ocp_solver import * +from .acados_sim_solver import * +from .utils import * diff --git a/third_party/acados/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json similarity index 95% rename from third_party/acados/acados_template/acados_layout.json rename to pyextra/acados_template/acados_layout.json index a1cc5bbdf30fd3..c9f0b90c73dd22 100644 --- a/third_party/acados/acados_template/acados_layout.json +++ b/pyextra/acados_template/acados_layout.json @@ -6,12 +6,6 @@ "str" ], "cython_include_dirs": [ - "list" - ], - "json_file": [ - "str" - ], - "shared_lib_ext": [ "str" ], "model": { @@ -21,16 +15,7 @@ "dyn_ext_fun_type" : [ "str" ], - "dyn_generic_source" : [ - "str" - ], - "dyn_impl_dae_fun" : [ - "str" - ], - "dyn_impl_dae_fun_jac" : [ - "str" - ], - "dyn_impl_dae_jac" : [ + "dyn_source_discrete" : [ "str" ], "dyn_disc_fun_jac_hess" : [ @@ -749,9 +734,6 @@ "sim_method_newton_iter": [ "int" ], - "sim_method_newton_tol": [ - "float" - ], "sim_method_jac_reuse": [ "ndarray", [ @@ -779,12 +761,6 @@ "qp_solver_iter_max": [ "int" ], - "qp_solver_cond_ric_alg": [ - "int" - ], - "qp_solver_ric_alg": [ - "int" - ], "nlp_solver_tol_stat": [ "float" ], @@ -800,9 +776,6 @@ "nlp_solver_max_iter": [ "int" ], - "nlp_solver_ext_qp_res": [ - "int" - ], "print_level": [ "int" ], @@ -821,9 +794,6 @@ "ext_cost_num_hess": [ "int" ], - "ext_fun_compile_flags": [ - "str" - ], "model_external_shared_lib_dir": [ "str" ], diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py new file mode 100644 index 00000000000000..e292cc747727c5 --- /dev/null +++ b/pyextra/acados_template/acados_model.py @@ -0,0 +1,167 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +class AcadosModel(): + """ + Class containing all the information to code generate the external CasADi functions + that are needed when creating an acados ocp solver or acados integrator. + Thus, this class contains: + + a) the :py:attr:`name` of the model, + b) all CasADi variables/expressions needed in the CasADi function generation process. + """ + def __init__(self): + ## common for OCP and Integrator + self.name = None + """ + The model name is used for code generation. Type: string. Default: :code:`None` + """ + self.x = None #: CasADi variable describing the state of the system; Default: :code:`None` + self.xdot = None #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None` + self.u = None #: CasADi variable describing the input of the system; Default: :code:`None` + self.z = [] #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty` + self.p = [] #: CasADi variable describing parameters of the DAE; Default: :code:`empty` + # dynamics + self.f_impl_expr = None + """ + CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'. + Default: :code:`None` + """ + self.f_expl_expr = None + """ + CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'. + Default: :code:`None` + """ + self.disc_dyn_expr = None + """ + CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`. + Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'. + Default: :code:`None` + """ + + self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi' + self.dyn_source_discrete = None #: name of source file for discrete dyanamics; Default: :code:`None` + self.dyn_disc_fun_jac_hess = None #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None` + self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None` + self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None` + + # for GNSF models + self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0} + """ + dictionary containing information on GNSF structure needed when rendering templates. + Contains integers `nontrivial_f_LO`, `purely_linear`. + """ + + ## for OCP + # constraints + self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None` + self.con_phi_expr = None #: CasADi expression for the constraint phi; Default: :code:`None` + self.con_r_expr = None #: CasADi expression for the constraint phi(r); Default: :code:`None` + self.con_r_in_phi = None + # terminal + self.con_h_expr_e = None #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None` + self.con_r_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` + self.con_phi_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` + self.con_r_in_phi_e = None + # cost + self.cost_y_expr = None #: CasADi expression for nonlinear least squares; Default: :code:`None` + self.cost_y_expr_e = None #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None` + self.cost_y_expr_0 = None #: CasADi expression for nonlinear least squares, initial; Default: :code:`None` + self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None` + self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None` + self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None` + self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None` + self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None` + self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None` + + +def acados_model_strip_casadi_symbolics(model): + out = model + if 'f_impl_expr' in out.keys(): + del out['f_impl_expr'] + if 'f_expl_expr' in out.keys(): + del out['f_expl_expr'] + if 'disc_dyn_expr' in out.keys(): + del out['disc_dyn_expr'] + if 'x' in out.keys(): + del out['x'] + if 'xdot' in out.keys(): + del out['xdot'] + if 'u' in out.keys(): + del out['u'] + if 'z' in out.keys(): + del out['z'] + if 'p' in out.keys(): + del out['p'] + # constraints + if 'con_phi_expr' in out.keys(): + del out['con_phi_expr'] + if 'con_h_expr' in out.keys(): + del out['con_h_expr'] + if 'con_r_expr' in out.keys(): + del out['con_r_expr'] + if 'con_r_in_phi' in out.keys(): + del out['con_r_in_phi'] + # terminal + if 'con_phi_expr_e' in out.keys(): + del out['con_phi_expr_e'] + if 'con_h_expr_e' in out.keys(): + del out['con_h_expr_e'] + if 'con_r_expr_e' in out.keys(): + del out['con_r_expr_e'] + if 'con_r_in_phi_e' in out.keys(): + del out['con_r_in_phi_e'] + # cost + if 'cost_y_expr' in out.keys(): + del out['cost_y_expr'] + if 'cost_y_expr_e' in out.keys(): + del out['cost_y_expr_e'] + if 'cost_y_expr_0' in out.keys(): + del out['cost_y_expr_0'] + if 'cost_expr_ext_cost' in out.keys(): + del out['cost_expr_ext_cost'] + if 'cost_expr_ext_cost_e' in out.keys(): + del out['cost_expr_ext_cost_e'] + if 'cost_expr_ext_cost_0' in out.keys(): + del out['cost_expr_ext_cost_0'] + if 'cost_expr_ext_cost_custom_hess' in out.keys(): + del out['cost_expr_ext_cost_custom_hess'] + if 'cost_expr_ext_cost_custom_hess_e' in out.keys(): + del out['cost_expr_ext_cost_custom_hess_e'] + if 'cost_expr_ext_cost_custom_hess_0' in out.keys(): + del out['cost_expr_ext_cost_custom_hess_0'] + + return out diff --git a/third_party/acados/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py similarity index 84% rename from third_party/acados/acados_template/acados_ocp.py rename to pyextra/acados_template/acados_ocp.py index d6236e1f6e9077..80970239eb1023 100644 --- a/third_party/acados/acados_template/acados_ocp.py +++ b/pyextra/acados_template/acados_ocp.py @@ -1,5 +1,9 @@ +# -*- coding: future_fstrings -*- # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -31,7 +35,7 @@ import numpy as np import os from .acados_model import AcadosModel -from .utils import get_acados_path, J_to_idx, J_to_idx_slack, get_lib_ext +from .utils import get_acados_path, J_to_idx, J_to_idx_slack class AcadosOcpDims: """ @@ -269,224 +273,224 @@ def nx(self, nx): if isinstance(nx, int) and nx > 0: self.__nx = nx else: - raise Exception('Invalid nx value, expected positive integer.') + raise Exception('Invalid nx value, expected positive integer. Exiting.') @nz.setter def nz(self, nz): if isinstance(nz, int) and nz > -1: self.__nz = nz else: - raise Exception('Invalid nz value, expected nonnegative integer.') + raise Exception('Invalid nz value, expected nonnegative integer. Exiting.') @nu.setter def nu(self, nu): if isinstance(nu, int) and nu > -1: self.__nu = nu else: - raise Exception('Invalid nu value, expected nonnegative integer.') + raise Exception('Invalid nu value, expected nonnegative integer. Exiting.') @np.setter def np(self, np): if isinstance(np, int) and np > -1: self.__np = np else: - raise Exception('Invalid np value, expected nonnegative integer.') + raise Exception('Invalid np value, expected nonnegative integer. Exiting.') @ny_0.setter def ny_0(self, ny_0): if isinstance(ny_0, int) and ny_0 > -1: self.__ny_0 = ny_0 else: - raise Exception('Invalid ny_0 value, expected nonnegative integer.') + raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.') @ny.setter def ny(self, ny): if isinstance(ny, int) and ny > -1: self.__ny = ny else: - raise Exception('Invalid ny value, expected nonnegative integer.') + raise Exception('Invalid ny value, expected nonnegative integer. Exiting.') @ny_e.setter def ny_e(self, ny_e): if isinstance(ny_e, int) and ny_e > -1: self.__ny_e = ny_e else: - raise Exception('Invalid ny_e value, expected nonnegative integer.') + raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.') @nr.setter def nr(self, nr): if isinstance(nr, int) and nr > -1: self.__nr = nr else: - raise Exception('Invalid nr value, expected nonnegative integer.') + raise Exception('Invalid nr value, expected nonnegative integer. Exiting.') @nr_e.setter def nr_e(self, nr_e): if isinstance(nr_e, int) and nr_e > -1: self.__nr_e = nr_e else: - raise Exception('Invalid nr_e value, expected nonnegative integer.') + raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.') @nh.setter def nh(self, nh): if isinstance(nh, int) and nh > -1: self.__nh = nh else: - raise Exception('Invalid nh value, expected nonnegative integer.') + raise Exception('Invalid nh value, expected nonnegative integer. Exiting.') @nh_e.setter def nh_e(self, nh_e): if isinstance(nh_e, int) and nh_e > -1: self.__nh_e = nh_e else: - raise Exception('Invalid nh_e value, expected nonnegative integer.') + raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.') @nphi.setter def nphi(self, nphi): if isinstance(nphi, int) and nphi > -1: self.__nphi = nphi else: - raise Exception('Invalid nphi value, expected nonnegative integer.') + raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.') @nphi_e.setter def nphi_e(self, nphi_e): if isinstance(nphi_e, int) and nphi_e > -1: self.__nphi_e = nphi_e else: - raise Exception('Invalid nphi_e value, expected nonnegative integer.') + raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.') @nbx.setter def nbx(self, nbx): if isinstance(nbx, int) and nbx > -1: self.__nbx = nbx else: - raise Exception('Invalid nbx value, expected nonnegative integer.') + raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.') @nbxe_0.setter def nbxe_0(self, nbxe_0): if isinstance(nbxe_0, int) and nbxe_0 > -1: self.__nbxe_0 = nbxe_0 else: - raise Exception('Invalid nbxe_0 value, expected nonnegative integer.') + raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.') @nbx_0.setter def nbx_0(self, nbx_0): if isinstance(nbx_0, int) and nbx_0 > -1: self.__nbx_0 = nbx_0 else: - raise Exception('Invalid nbx_0 value, expected nonnegative integer.') + raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.') @nbx_e.setter def nbx_e(self, nbx_e): if isinstance(nbx_e, int) and nbx_e > -1: self.__nbx_e = nbx_e else: - raise Exception('Invalid nbx_e value, expected nonnegative integer.') + raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.') @nbu.setter def nbu(self, nbu): if isinstance(nbu, int) and nbu > -1: self.__nbu = nbu else: - raise Exception('Invalid nbu value, expected nonnegative integer.') + raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.') @nsbx.setter def nsbx(self, nsbx): if isinstance(nsbx, int) and nsbx > -1: self.__nsbx = nsbx else: - raise Exception('Invalid nsbx value, expected nonnegative integer.') + raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.') @nsbx_e.setter def nsbx_e(self, nsbx_e): if isinstance(nsbx_e, int) and nsbx_e > -1: self.__nsbx_e = nsbx_e else: - raise Exception('Invalid nsbx_e value, expected nonnegative integer.') + raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.') @nsbu.setter def nsbu(self, nsbu): if isinstance(nsbu, int) and nsbu > -1: self.__nsbu = nsbu else: - raise Exception('Invalid nsbu value, expected nonnegative integer.') + raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.') @nsg.setter def nsg(self, nsg): if isinstance(nsg, int) and nsg > -1: self.__nsg = nsg else: - raise Exception('Invalid nsg value, expected nonnegative integer.') + raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.') @nsg_e.setter def nsg_e(self, nsg_e): if isinstance(nsg_e, int) and nsg_e > -1: self.__nsg_e = nsg_e else: - raise Exception('Invalid nsg_e value, expected nonnegative integer.') + raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.') @nsh.setter def nsh(self, nsh): if isinstance(nsh, int) and nsh > -1: self.__nsh = nsh else: - raise Exception('Invalid nsh value, expected nonnegative integer.') + raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.') @nsh_e.setter def nsh_e(self, nsh_e): if isinstance(nsh_e, int) and nsh_e > -1: self.__nsh_e = nsh_e else: - raise Exception('Invalid nsh_e value, expected nonnegative integer.') + raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.') @nsphi.setter def nsphi(self, nsphi): if isinstance(nsphi, int) and nsphi > -1: self.__nsphi = nsphi else: - raise Exception('Invalid nsphi value, expected nonnegative integer.') + raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.') @nsphi_e.setter def nsphi_e(self, nsphi_e): if isinstance(nsphi_e, int) and nsphi_e > -1: self.__nsphi_e = nsphi_e else: - raise Exception('Invalid nsphi_e value, expected nonnegative integer.') + raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.') @ns.setter def ns(self, ns): if isinstance(ns, int) and ns > -1: self.__ns = ns else: - raise Exception('Invalid ns value, expected nonnegative integer.') + raise Exception('Invalid ns value, expected nonnegative integer. Exiting.') @ns_e.setter def ns_e(self, ns_e): if isinstance(ns_e, int) and ns_e > -1: self.__ns_e = ns_e else: - raise Exception('Invalid ns_e value, expected nonnegative integer.') + raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.') @ng.setter def ng(self, ng): if isinstance(ng, int) and ng > -1: self.__ng = ng else: - raise Exception('Invalid ng value, expected nonnegative integer.') + raise Exception('Invalid ng value, expected nonnegative integer. Exiting.') @ng_e.setter def ng_e(self, ng_e): if isinstance(ng_e, int) and ng_e > -1: self.__ng_e = ng_e else: - raise Exception('Invalid ng_e value, expected nonnegative integer.') + raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.') @N.setter def N(self, N): if isinstance(N, int) and N > 0: self.__N = N else: - raise Exception('Invalid N value, expected positive integer.') + raise Exception('Invalid N value, expected positive integer. Exiting.') def set(self, attr, value): setattr(self, attr, value) @@ -496,12 +500,6 @@ class AcadosOcpCost: """ Class containing the numerical data of the cost: - NOTE: all cost terms, except for the terminal one are weighted with the corresponding time step. - This means given the time steps are :math:`\Delta t_0,..., \Delta t_N`, the total cost is given by: - :math:`c_\\text{total} = \Delta t_0 \cdot c_0(x_0, u_0, p_0, z_0) + ... + \Delta t_{N-1} \cdot c_{N-1}(x_0, u_0, p_0, z_0) + c_N(x_N, p_N)`. - - This means the Lagrange cost term is given in continuous time, this makes up for a seeminglessly OCP discretization with a nonuniform time grid. - In case of LINEAR_LS: stage cost is :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, @@ -510,15 +508,9 @@ class AcadosOcpCost: In case of NONLINEAR_LS: stage cost is - :math:`l(x,u,z,p) = || y(x,u,z,p) - y_\\text{ref}||^2_W`, - terminal cost is - :math:`m(x,p) = || y^e(x,p) - y_\\text{ref}^e||^2_{W^e}` - - In case of CONVEX_OVER_NONLINEAR: - stage cost is - :math:`l(x,u,p) = \psi(y(x,u,p) - y_\\text{ref}, p)`, + :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`, terminal cost is - :math:`m(x, p) = \psi^e (y^e(x,p) - y_\\text{ref}^e, p)` + :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}` """ def __init__(self): # initial stage @@ -556,7 +548,7 @@ def __init__(self): @property def cost_type_0(self): """Cost type at initial shooting node (0) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR} or :code:`None`. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`. Default: :code:`None`. .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. @@ -612,10 +604,10 @@ def cost_ext_fun_type_0(self): @yref_0.setter def yref_0(self, yref_0): - if isinstance(yref_0, np.ndarray) and len(yref_0.shape) == 1: + if isinstance(yref_0, np.ndarray): self.__yref_0 = yref_0 else: - raise Exception('Invalid yref_0 value, expected 1-dimensional numpy array.') + raise Exception('Invalid yref_0 value, expected numpy array. Exiting.') @W_0.setter def W_0(self, W_0): @@ -623,7 +615,7 @@ def W_0(self, W_0): self.__W_0 = W_0 else: raise Exception('Invalid cost W_0 value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vx_0.setter def Vx_0(self, Vx_0): @@ -631,7 +623,7 @@ def Vx_0(self, Vx_0): self.__Vx_0 = Vx_0 else: raise Exception('Invalid cost Vx_0 value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vu_0.setter def Vu_0(self, Vu_0): @@ -639,7 +631,7 @@ def Vu_0(self, Vu_0): self.__Vu_0 = Vu_0 else: raise Exception('Invalid cost Vu_0 value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vz_0.setter def Vz_0(self, Vz_0): @@ -647,21 +639,21 @@ def Vz_0(self, Vz_0): self.__Vz_0 = Vz_0 else: raise Exception('Invalid cost Vz_0 value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @cost_ext_fun_type_0.setter def cost_ext_fun_type_0(self, cost_ext_fun_type_0): if cost_ext_fun_type_0 in ['casadi', 'generic']: self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 else: - raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array.') + raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.') # Lagrange term @property def cost_type(self): """ Cost type at intermediate shooting nodes (1 to N-1) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. Default: 'LINEAR_LS'. """ return self.__cost_type @@ -703,28 +695,28 @@ def yref(self): @property def Zl(self): - """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (0 to N-1). + """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1). Default: :code:`np.array([])`. """ return self.__Zl @property def Zu(self): - """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (0 to N-1). + """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1). Default: :code:`np.array([])`. """ return self.__Zu @property def zl(self): - """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (0 to N-1). + """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1). Default: :code:`np.array([])`. """ return self.__zl @property def zu(self): - """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (0 to N-1). + """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1). Default: :code:`np.array([])`. """ return self.__zu @@ -739,19 +731,19 @@ def cost_ext_fun_type(self): @cost_type.setter def cost_type(self, cost_type): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') if cost_type in cost_types: self.__cost_type = cost_type else: - raise Exception('Invalid cost_type value.') + raise Exception('Invalid cost_type value. Exiting.') @cost_type_0.setter def cost_type_0(self, cost_type_0): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') if cost_type_0 in cost_types: self.__cost_type_0 = cost_type_0 else: - raise Exception('Invalid cost_type_0 value.') + raise Exception('Invalid cost_type_0 value. Exiting.') @W.setter def W(self, W): @@ -759,7 +751,7 @@ def W(self, W): self.__W = W else: raise Exception('Invalid cost W value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vx.setter @@ -768,7 +760,7 @@ def Vx(self, Vx): self.__Vx = Vx else: raise Exception('Invalid cost Vx value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vu.setter def Vu(self, Vu): @@ -776,7 +768,7 @@ def Vu(self, Vu): self.__Vu = Vu else: raise Exception('Invalid cost Vu value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vz.setter def Vz(self, Vz): @@ -784,56 +776,56 @@ def Vz(self, Vz): self.__Vz = Vz else: raise Exception('Invalid cost Vz value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @yref.setter def yref(self, yref): - if isinstance(yref, np.ndarray) and len(yref.shape) == 1: + if isinstance(yref, np.ndarray): self.__yref = yref else: - raise Exception('Invalid yref value, expected 1-dimensional numpy array.') + raise Exception('Invalid yref value, expected numpy array. Exiting.') @Zl.setter def Zl(self, Zl): if isinstance(Zl, np.ndarray): self.__Zl = Zl else: - raise Exception('Invalid Zl value, expected numpy array.') + raise Exception('Invalid Zl value, expected numpy array. Exiting.') @Zu.setter def Zu(self, Zu): if isinstance(Zu, np.ndarray): self.__Zu = Zu else: - raise Exception('Invalid Zu value, expected numpy array.') + raise Exception('Invalid Zu value, expected numpy array. Exiting.') @zl.setter def zl(self, zl): if isinstance(zl, np.ndarray): self.__zl = zl else: - raise Exception('Invalid zl value, expected numpy array.') + raise Exception('Invalid zl value, expected numpy array. Exiting.') @zu.setter def zu(self, zu): if isinstance(zu, np.ndarray): self.__zu = zu else: - raise Exception('Invalid zu value, expected numpy array.') + raise Exception('Invalid zu value, expected numpy array. Exiting.') @cost_ext_fun_type.setter def cost_ext_fun_type(self, cost_ext_fun_type): if cost_ext_fun_type in ['casadi', 'generic']: self.__cost_ext_fun_type = cost_ext_fun_type else: - raise Exception("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].") + raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.') # Mayer term @property def cost_type_e(self): """ Cost type at terminal shooting node (N) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. Default: 'LINEAR_LS'. """ return self.__cost_type_e @@ -889,7 +881,7 @@ def zu_e(self): @property def cost_ext_fun_type_e(self): - """Type of external function for cost at terminal shooting node (N). + """Type of external function for cost at intermediate shooting nodes (1 to N-1). -- string in {casadi, generic} Default: :code:'casadi'. """ @@ -897,12 +889,12 @@ def cost_ext_fun_type_e(self): @cost_type_e.setter def cost_type_e(self, cost_type_e): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') if cost_type_e in cost_types: self.__cost_type_e = cost_type_e else: - raise Exception('Invalid cost_type_e value.') + raise Exception('Invalid cost_type_e value. Exiting.') @W_e.setter def W_e(self, W_e): @@ -910,7 +902,7 @@ def W_e(self, W_e): self.__W_e = W_e else: raise Exception('Invalid cost W_e value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @Vx_e.setter def Vx_e(self, Vx_e): @@ -918,49 +910,49 @@ def Vx_e(self, Vx_e): self.__Vx_e = Vx_e else: raise Exception('Invalid cost Vx_e value. ' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @yref_e.setter def yref_e(self, yref_e): - if isinstance(yref_e, np.ndarray) and len(yref_e.shape) == 1: + if isinstance(yref_e, np.ndarray): self.__yref_e = yref_e else: - raise Exception('Invalid yref_e value, expected 1-dimensional numpy array.') + raise Exception('Invalid yref_e value, expected numpy array. Exiting.') @Zl_e.setter def Zl_e(self, Zl_e): if isinstance(Zl_e, np.ndarray): self.__Zl_e = Zl_e else: - raise Exception('Invalid Zl_e value, expected numpy array.') + raise Exception('Invalid Zl_e value, expected numpy array. Exiting.') @Zu_e.setter def Zu_e(self, Zu_e): if isinstance(Zu_e, np.ndarray): self.__Zu_e = Zu_e else: - raise Exception('Invalid Zu_e value, expected numpy array.') + raise Exception('Invalid Zu_e value, expected numpy array. Exiting.') @zl_e.setter def zl_e(self, zl_e): if isinstance(zl_e, np.ndarray): self.__zl_e = zl_e else: - raise Exception('Invalid zl_e value, expected numpy array.') + raise Exception('Invalid zl_e value, expected numpy array. Exiting.') @zu_e.setter def zu_e(self, zu_e): if isinstance(zu_e, np.ndarray): self.__zu_e = zu_e else: - raise Exception('Invalid zu_e value, expected numpy array.') + raise Exception('Invalid zu_e value, expected numpy array. Exiting.') @cost_ext_fun_type_e.setter def cost_ext_fun_type_e(self, cost_ext_fun_type_e): if cost_ext_fun_type_e in ['casadi', 'generic']: self.__cost_ext_fun_type_e = cost_ext_fun_type_e else: - raise Exception("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].") + raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.') def set(self, attr, value): setattr(self, attr, value) @@ -1586,7 +1578,7 @@ def constr_type(self, constr_type): self.__constr_type = constr_type else: raise Exception('Invalid constr_type value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n') + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.') @constr_type_e.setter def constr_type_e(self, constr_type_e): @@ -1595,7 +1587,7 @@ def constr_type_e(self, constr_type_e): self.__constr_type_e = constr_type_e else: raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n') + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.') # initial x @lbx_0.setter @@ -1603,35 +1595,35 @@ def lbx_0(self, lbx_0): if isinstance(lbx_0, np.ndarray): self.__lbx_0 = lbx_0 else: - raise Exception('Invalid lbx_0 value.') + raise Exception('Invalid lbx_0 value. Exiting.') @ubx_0.setter def ubx_0(self, ubx_0): if isinstance(ubx_0, np.ndarray): self.__ubx_0 = ubx_0 else: - raise Exception('Invalid ubx_0 value.') + raise Exception('Invalid ubx_0 value. Exiting.') @idxbx_0.setter def idxbx_0(self, idxbx_0): if isinstance(idxbx_0, np.ndarray): self.__idxbx_0 = idxbx_0 else: - raise Exception('Invalid idxbx_0 value.') + raise Exception('Invalid idxbx_0 value. Exiting.') @Jbx_0.setter def Jbx_0(self, Jbx_0): if isinstance(Jbx_0, np.ndarray): self.__idxbx_0 = J_to_idx(Jbx_0) else: - raise Exception('Invalid Jbx_0 value.') + raise Exception('Invalid Jbx_0 value. Exiting.') @idxbxe_0.setter def idxbxe_0(self, idxbxe_0): if isinstance(idxbxe_0, np.ndarray): self.__idxbxe_0 = idxbxe_0 else: - raise Exception('Invalid idxbxe_0 value.') + raise Exception('Invalid idxbxe_0 value. Exiting.') @x0.setter @@ -1642,7 +1634,7 @@ def x0(self, x0): self.__idxbx_0 = np.arange(x0.size) self.__idxbxe_0 = np.arange(x0.size) else: - raise Exception('Invalid x0 value.') + raise Exception('Invalid x0 value. Exiting.') # bounds on x @lbx.setter @@ -1650,28 +1642,28 @@ def lbx(self, lbx): if isinstance(lbx, np.ndarray): self.__lbx = lbx else: - raise Exception('Invalid lbx value.') + raise Exception('Invalid lbx value. Exiting.') @ubx.setter def ubx(self, ubx): if isinstance(ubx, np.ndarray): self.__ubx = ubx else: - raise Exception('Invalid ubx value.') + raise Exception('Invalid ubx value. Exiting.') @idxbx.setter def idxbx(self, idxbx): if isinstance(idxbx, np.ndarray): self.__idxbx = idxbx else: - raise Exception('Invalid idxbx value.') + raise Exception('Invalid idxbx value. Exiting.') @Jbx.setter def Jbx(self, Jbx): if isinstance(Jbx, np.ndarray): self.__idxbx = J_to_idx(Jbx) else: - raise Exception('Invalid Jbx value.') + raise Exception('Invalid Jbx value. Exiting.') # bounds on u @lbu.setter @@ -1679,28 +1671,28 @@ def lbu(self, lbu): if isinstance(lbu, np.ndarray): self.__lbu = lbu else: - raise Exception('Invalid lbu value.') + raise Exception('Invalid lbu value. Exiting.') @ubu.setter def ubu(self, ubu): if isinstance(ubu, np.ndarray): self.__ubu = ubu else: - raise Exception('Invalid ubu value.') + raise Exception('Invalid ubu value. Exiting.') @idxbu.setter def idxbu(self, idxbu): if isinstance(idxbu, np.ndarray): self.__idxbu = idxbu else: - raise Exception('Invalid idxbu value.') + raise Exception('Invalid idxbu value. Exiting.') @Jbu.setter def Jbu(self, Jbu): if isinstance(Jbu, np.ndarray): self.__idxbu = J_to_idx(Jbu) else: - raise Exception('Invalid Jbu value.') + raise Exception('Invalid Jbu value. Exiting.') # bounds on x at shooting node N @lbx_e.setter @@ -1708,28 +1700,28 @@ def lbx_e(self, lbx_e): if isinstance(lbx_e, np.ndarray): self.__lbx_e = lbx_e else: - raise Exception('Invalid lbx_e value.') + raise Exception('Invalid lbx_e value. Exiting.') @ubx_e.setter def ubx_e(self, ubx_e): if isinstance(ubx_e, np.ndarray): self.__ubx_e = ubx_e else: - raise Exception('Invalid ubx_e value.') + raise Exception('Invalid ubx_e value. Exiting.') @idxbx_e.setter def idxbx_e(self, idxbx_e): if isinstance(idxbx_e, np.ndarray): self.__idxbx_e = idxbx_e else: - raise Exception('Invalid idxbx_e value.') + raise Exception('Invalid idxbx_e value. Exiting.') @Jbx_e.setter def Jbx_e(self, Jbx_e): if isinstance(Jbx_e, np.ndarray): self.__idxbx_e = J_to_idx(Jbx_e) else: - raise Exception('Invalid Jbx_e value.') + raise Exception('Invalid Jbx_e value. Exiting.') # polytopic constraints @D.setter @@ -1738,7 +1730,7 @@ def D(self, D): self.__D = D else: raise Exception('Invalid constraint D value.' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @C.setter def C(self, C): @@ -1746,21 +1738,21 @@ def C(self, C): self.__C = C else: raise Exception('Invalid constraint C value.' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @lg.setter def lg(self, lg): if isinstance(lg, np.ndarray): self.__lg = lg else: - raise Exception('Invalid lg value.') + raise Exception('Invalid lg value. Exiting.') @ug.setter def ug(self, ug): if isinstance(ug, np.ndarray): self.__ug = ug else: - raise Exception('Invalid ug value.') + raise Exception('Invalid ug value. Exiting.') # polytopic constraints at shooting node N @C_e.setter @@ -1769,21 +1761,21 @@ def C_e(self, C_e): self.__C_e = C_e else: raise Exception('Invalid constraint C_e value.' \ - + 'Should be 2 dimensional numpy array.') + + 'Should be 2 dimensional numpy array. Exiting.') @lg_e.setter def lg_e(self, lg_e): if isinstance(lg_e, np.ndarray): self.__lg_e = lg_e else: - raise Exception('Invalid lg_e value.') + raise Exception('Invalid lg_e value. Exiting.') @ug_e.setter def ug_e(self, ug_e): if isinstance(ug_e, np.ndarray): self.__ug_e = ug_e else: - raise Exception('Invalid ug_e value.') + raise Exception('Invalid ug_e value. Exiting.') # nonlinear constraints @lh.setter @@ -1791,14 +1783,14 @@ def lh(self, lh): if isinstance(lh, np.ndarray): self.__lh = lh else: - raise Exception('Invalid lh value.') + raise Exception('Invalid lh value. Exiting.') @uh.setter def uh(self, uh): if isinstance(uh, np.ndarray): self.__uh = uh else: - raise Exception('Invalid uh value.') + raise Exception('Invalid uh value. Exiting.') # convex-over-nonlinear constraints @lphi.setter @@ -1806,14 +1798,14 @@ def lphi(self, lphi): if isinstance(lphi, np.ndarray): self.__lphi = lphi else: - raise Exception('Invalid lphi value.') + raise Exception('Invalid lphi value. Exiting.') @uphi.setter def uphi(self, uphi): if isinstance(uphi, np.ndarray): self.__uphi = uphi else: - raise Exception('Invalid uphi value.') + raise Exception('Invalid uphi value. Exiting.') # nonlinear constraints at shooting node N @lh_e.setter @@ -1821,14 +1813,14 @@ def lh_e(self, lh_e): if isinstance(lh_e, np.ndarray): self.__lh_e = lh_e else: - raise Exception('Invalid lh_e value.') + raise Exception('Invalid lh_e value. Exiting.') @uh_e.setter def uh_e(self, uh_e): if isinstance(uh_e, np.ndarray): self.__uh_e = uh_e else: - raise Exception('Invalid uh_e value.') + raise Exception('Invalid uh_e value. Exiting.') # convex-over-nonlinear constraints at shooting node N @lphi_e.setter @@ -1836,14 +1828,14 @@ def lphi_e(self, lphi_e): if isinstance(lphi_e, np.ndarray): self.__lphi_e = lphi_e else: - raise Exception('Invalid lphi_e value.') + raise Exception('Invalid lphi_e value. Exiting.') @uphi_e.setter def uphi_e(self, uphi_e): if isinstance(uphi_e, np.ndarray): self.__uphi_e = uphi_e else: - raise Exception('Invalid uphi_e value.') + raise Exception('Invalid uphi_e value. Exiting.') # SLACK bounds # soft bounds on x @@ -1852,28 +1844,28 @@ def lsbx(self, lsbx): if isinstance(lsbx, np.ndarray): self.__lsbx = lsbx else: - raise Exception('Invalid lsbx value.') + raise Exception('Invalid lsbx value. Exiting.') @usbx.setter def usbx(self, usbx): if isinstance(usbx, np.ndarray): self.__usbx = usbx else: - raise Exception('Invalid usbx value.') + raise Exception('Invalid usbx value. Exiting.') @idxsbx.setter def idxsbx(self, idxsbx): if isinstance(idxsbx, np.ndarray): self.__idxsbx = idxsbx else: - raise Exception('Invalid idxsbx value.') + raise Exception('Invalid idxsbx value. Exiting.') @Jsbx.setter def Jsbx(self, Jsbx): if isinstance(Jsbx, np.ndarray): self.__idxsbx = J_to_idx_slack(Jsbx) else: - raise Exception('Invalid Jsbx value, expected numpy array.') + raise Exception('Invalid Jsbx value, expected numpy array. Exiting.') # soft bounds on u @lsbu.setter @@ -1881,28 +1873,28 @@ def lsbu(self, lsbu): if isinstance(lsbu, np.ndarray): self.__lsbu = lsbu else: - raise Exception('Invalid lsbu value.') + raise Exception('Invalid lsbu value. Exiting.') @usbu.setter def usbu(self, usbu): if isinstance(usbu, np.ndarray): self.__usbu = usbu else: - raise Exception('Invalid usbu value.') + raise Exception('Invalid usbu value. Exiting.') @idxsbu.setter def idxsbu(self, idxsbu): if isinstance(idxsbu, np.ndarray): self.__idxsbu = idxsbu else: - raise Exception('Invalid idxsbu value.') + raise Exception('Invalid idxsbu value. Exiting.') @Jsbu.setter def Jsbu(self, Jsbu): if isinstance(Jsbu, np.ndarray): self.__idxsbu = J_to_idx_slack(Jsbu) else: - raise Exception('Invalid Jsbu value.') + raise Exception('Invalid Jsbu value. Exiting.') # soft bounds on x at shooting node N @lsbx_e.setter @@ -1910,28 +1902,28 @@ def lsbx_e(self, lsbx_e): if isinstance(lsbx_e, np.ndarray): self.__lsbx_e = lsbx_e else: - raise Exception('Invalid lsbx_e value.') + raise Exception('Invalid lsbx_e value. Exiting.') @usbx_e.setter def usbx_e(self, usbx_e): if isinstance(usbx_e, np.ndarray): self.__usbx_e = usbx_e else: - raise Exception('Invalid usbx_e value.') + raise Exception('Invalid usbx_e value. Exiting.') @idxsbx_e.setter def idxsbx_e(self, idxsbx_e): if isinstance(idxsbx_e, np.ndarray): self.__idxsbx_e = idxsbx_e else: - raise Exception('Invalid idxsbx_e value.') + raise Exception('Invalid idxsbx_e value. Exiting.') @Jsbx_e.setter def Jsbx_e(self, Jsbx_e): if isinstance(Jsbx_e, np.ndarray): self.__idxsbx_e = J_to_idx_slack(Jsbx_e) else: - raise Exception('Invalid Jsbx_e value.') + raise Exception('Invalid Jsbx_e value. Exiting.') # soft bounds on general linear constraints @@ -1940,28 +1932,28 @@ def lsg(self, lsg): if isinstance(lsg, np.ndarray): self.__lsg = lsg else: - raise Exception('Invalid lsg value.') + raise Exception('Invalid lsg value. Exiting.') @usg.setter def usg(self, usg): if isinstance(usg, np.ndarray): self.__usg = usg else: - raise Exception('Invalid usg value.') + raise Exception('Invalid usg value. Exiting.') @idxsg.setter def idxsg(self, idxsg): if isinstance(idxsg, np.ndarray): self.__idxsg = idxsg else: - raise Exception('Invalid idxsg value.') + raise Exception('Invalid idxsg value. Exiting.') @Jsg.setter def Jsg(self, Jsg): if isinstance(Jsg, np.ndarray): self.__idxsg = J_to_idx_slack(Jsg) else: - raise Exception('Invalid Jsg value, expected numpy array.') + raise Exception('Invalid Jsg value, expected numpy array. Exiting.') # soft bounds on nonlinear constraints @@ -1970,21 +1962,21 @@ def lsh(self, lsh): if isinstance(lsh, np.ndarray): self.__lsh = lsh else: - raise Exception('Invalid lsh value.') + raise Exception('Invalid lsh value. Exiting.') @ush.setter def ush(self, ush): if isinstance(ush, np.ndarray): self.__ush = ush else: - raise Exception('Invalid ush value.') + raise Exception('Invalid ush value. Exiting.') @idxsh.setter def idxsh(self, idxsh): if isinstance(idxsh, np.ndarray): self.__idxsh = idxsh else: - raise Exception('Invalid idxsh value.') + raise Exception('Invalid idxsh value. Exiting.') @Jsh.setter @@ -1992,7 +1984,7 @@ def Jsh(self, Jsh): if isinstance(Jsh, np.ndarray): self.__idxsh = J_to_idx_slack(Jsh) else: - raise Exception('Invalid Jsh value, expected numpy array.') + raise Exception('Invalid Jsh value, expected numpy array. Exiting.') # soft bounds on convex-over-nonlinear constraints @lsphi.setter @@ -2000,28 +1992,28 @@ def lsphi(self, lsphi): if isinstance(lsphi, np.ndarray): self.__lsphi = lsphi else: - raise Exception('Invalid lsphi value.') + raise Exception('Invalid lsphi value. Exiting.') @usphi.setter def usphi(self, usphi): if isinstance(usphi, np.ndarray): self.__usphi = usphi else: - raise Exception('Invalid usphi value.') + raise Exception('Invalid usphi value. Exiting.') @idxsphi.setter def idxsphi(self, idxsphi): if isinstance(idxsphi, np.ndarray): self.__idxsphi = idxsphi else: - raise Exception('Invalid idxsphi value.') + raise Exception('Invalid idxsphi value. Exiting.') @Jsphi.setter def Jsphi(self, Jsphi): if isinstance(Jsphi, np.ndarray): self.__idxsphi = J_to_idx_slack(Jsphi) else: - raise Exception('Invalid Jsphi value, expected numpy array.') + raise Exception('Invalid Jsphi value, expected numpy array. Exiting.') # soft bounds on general linear constraints at shooting node N @lsg_e.setter @@ -2029,28 +2021,28 @@ def lsg_e(self, lsg_e): if isinstance(lsg_e, np.ndarray): self.__lsg_e = lsg_e else: - raise Exception('Invalid lsg_e value.') + raise Exception('Invalid lsg_e value. Exiting.') @usg_e.setter def usg_e(self, usg_e): if isinstance(usg_e, np.ndarray): self.__usg_e = usg_e else: - raise Exception('Invalid usg_e value.') + raise Exception('Invalid usg_e value. Exiting.') @idxsg_e.setter def idxsg_e(self, idxsg_e): if isinstance(idxsg_e, np.ndarray): self.__idxsg_e = idxsg_e else: - raise Exception('Invalid idxsg_e value.') + raise Exception('Invalid idxsg_e value. Exiting.') @Jsg_e.setter def Jsg_e(self, Jsg_e): if isinstance(Jsg_e, np.ndarray): self.__idxsg_e = J_to_idx_slack(Jsg_e) else: - raise Exception('Invalid Jsg_e value, expected numpy array.') + raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.') # soft bounds on nonlinear constraints at shooting node N @lsh_e.setter @@ -2058,28 +2050,28 @@ def lsh_e(self, lsh_e): if isinstance(lsh_e, np.ndarray): self.__lsh_e = lsh_e else: - raise Exception('Invalid lsh_e value.') + raise Exception('Invalid lsh_e value. Exiting.') @ush_e.setter def ush_e(self, ush_e): if isinstance(ush_e, np.ndarray): self.__ush_e = ush_e else: - raise Exception('Invalid ush_e value.') + raise Exception('Invalid ush_e value. Exiting.') @idxsh_e.setter def idxsh_e(self, idxsh_e): if isinstance(idxsh_e, np.ndarray): self.__idxsh_e = idxsh_e else: - raise Exception('Invalid idxsh_e value.') + raise Exception('Invalid idxsh_e value. Exiting.') @Jsh_e.setter def Jsh_e(self, Jsh_e): if isinstance(Jsh_e, np.ndarray): self.__idxsh_e = J_to_idx_slack(Jsh_e) else: - raise Exception('Invalid Jsh_e value, expected numpy array.') + raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.') # soft bounds on convex-over-nonlinear constraints at shooting node N @@ -2088,28 +2080,28 @@ def lsphi_e(self, lsphi_e): if isinstance(lsphi_e, np.ndarray): self.__lsphi_e = lsphi_e else: - raise Exception('Invalid lsphi_e value.') + raise Exception('Invalid lsphi_e value. Exiting.') @usphi_e.setter def usphi_e(self, usphi_e): if isinstance(usphi_e, np.ndarray): self.__usphi_e = usphi_e else: - raise Exception('Invalid usphi_e value.') + raise Exception('Invalid usphi_e value. Exiting.') @idxsphi_e.setter def idxsphi_e(self, idxsphi_e): if isinstance(idxsphi_e, np.ndarray): self.__idxsphi_e = idxsphi_e else: - raise Exception('Invalid idxsphi_e value.') + raise Exception('Invalid idxsphi_e value. Exiting.') @Jsphi_e.setter def Jsphi_e(self, Jsphi_e): if isinstance(Jsphi_e, np.ndarray): self.__idxsphi_e = J_to_idx_slack(Jsphi_e) else: - raise Exception('Invalid Jsphi_e value.') + raise Exception('Invalid Jsphi_e value. Exiting.') def set(self, attr, value): setattr(self, attr, value) @@ -2132,7 +2124,6 @@ def __init__(self): self.__sim_method_num_stages = 4 # number of stages in the integrator self.__sim_method_num_steps = 1 # number of steps in the integrator self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method - self.__sim_method_newton_tol = 0.0 self.__sim_method_jac_reuse = 0 self.__qp_solver_tol_stat = None # QP solver stationarity tolerance self.__qp_solver_tol_eq = None # QP solver equality tolerance @@ -2141,17 +2132,16 @@ def __init__(self): self.__qp_solver_iter_max = 50 # QP solver max iter self.__qp_solver_cond_N = None # QP solver: new horizon after partial condensing self.__qp_solver_warm_start = 0 - self.__qp_solver_cond_ric_alg = 1 - self.__qp_solver_ric_alg = 1 self.__nlp_solver_tol_stat = 1e-6 # NLP solver stationarity tolerance self.__nlp_solver_tol_eq = 1e-6 # NLP solver equality tolerance self.__nlp_solver_tol_ineq = 1e-6 # NLP solver inequality self.__nlp_solver_tol_comp = 1e-6 # NLP solver complementarity self.__nlp_solver_max_iter = 100 # NLP solver maximum number of iterations - self.__nlp_solver_ext_qp_res = 0 self.__Tsim = None # automatically calculated as tf/N self.__print_level = 0 # print level self.__initialize_t_slacks = 0 # possible values: 0, 1 + self.__model_external_shared_lib_dir = None # path to the the .so lib + self.__model_external_shared_lib_name = None # name of the the .so lib self.__regularize_method = None self.__time_steps = None self.__shooting_nodes = None @@ -2166,93 +2156,16 @@ def __init__(self): self.__full_step_dual = 0 self.__eps_sufficient_descent = 1e-4 self.__hpipm_mode = 'BALANCE' - # TODO: move those out? they are more about generation than about the acados OCP solver. - self.__ext_fun_compile_flags = '-O2' - self.__model_external_shared_lib_dir = None # path to the the .so lib - self.__model_external_shared_lib_name = None # name of the the .so lib - self.__custom_update_filename = '' - self.__custom_update_header_filename = '' - self.__custom_templates = [] - self.__custom_update_copy = True + @property def qp_solver(self): """QP solver to be used in the NLP solver. - String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'). + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP'). Default: 'PARTIAL_CONDENSING_HPIPM'. """ return self.__qp_solver - @property - def ext_fun_compile_flags(self): - """ - String with compiler flags for external function compilation. - Default: '-O2'. - """ - return self.__ext_fun_compile_flags - - - @property - def custom_update_filename(self): - """ - Filename of the custom C function to update solver data and parameters in between solver calls - - This file has to implement the functions - int custom_update_init_function([model.name]_solver_capsule* capsule); - int custom_update_function([model.name]_solver_capsule* capsule); - int custom_update_terminate_function([model.name]_solver_capsule* capsule); - - - Default: ''. - """ - return self.__custom_update_filename - - - @property - def custom_templates(self): - """ - List of tuples of the form: - (input_filename, output_filename) - - Custom templates are render in OCP solver generation. - - Default: []. - """ - return self.__custom_templates - - - @property - def custom_update_header_filename(self): - """ - Header filename of the custom C function to update solver data and parameters in between solver calls - - This file has to declare the custom_update functions and look as follows: - - ``` - // Called at the end of solver creation. - // This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory. - int custom_update_init_function([model.name]_solver_capsule* capsule); - - // Custom update function that can be called between solver calls - int custom_update_function([model.name]_solver_capsule* capsule, double* data, int data_len); - - // Called just before destroying the solver. - // Responsible to free allocated memory, stored at capsule->custom_update_memory. - int custom_update_terminate_function([model.name]_solver_capsule* capsule); - - Default: ''. - """ - return self.__custom_update_header_filename - - @property - def custom_update_copy(self): - """ - Boolean; - If True, the custom update function files are copied into the `code_export_directory`. - """ - return self.__custom_update_copy - - @property def hpipm_mode(self): """ @@ -2317,13 +2230,6 @@ def regularize_method(self): """Regularization method for the Hessian. String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. - - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii)) - - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) - - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf - - PROJECT_REDUC_HESS: experimental - - Note: default eps = 1e-4 - Default: :code:`None`. """ return self.__regularize_method @@ -2373,15 +2279,6 @@ def sim_method_newton_iter(self): """ return self.__sim_method_newton_iter - @property - def sim_method_newton_tol(self): - """ - Tolerance of Newton system in simulation method. - Type: float: 0.0 means not used - Default: 0.0 - """ - return self.__sim_method_newton_tol - @property def sim_method_jac_reuse(self): """ @@ -2431,42 +2328,10 @@ def qp_solver_cond_N(self): @property def qp_solver_warm_start(self): - """ - QP solver: Warm starting. - 0: no warm start; 1: warm start; 2: hot start. - Default: 0 - """ + """QP solver: Warm starting. + 0: no warm start; 1: warm start; 2: hot start.""" return self.__qp_solver_warm_start - @property - def qp_solver_cond_ric_alg(self): - """ - QP solver: Determines which algorithm is used in HPIPM condensing. - 0: dont factorize hessian in the condensing; 1: factorize. - Default: 1 - """ - return self.__qp_solver_cond_ric_alg - - @property - def qp_solver_ric_alg(self): - """ - QP solver: Determines which algorithm is used in HPIPM OCP QP solver. - 0 classical Riccati, 1 square-root Riccati. - - Note: taken from [HPIPM paper]: - - (a) the classical implementation requires the reduced Hessian with respect to the dynamics - equality constraints to be positive definite, but allows the full-space Hessian to be indefinite) - (b) the square-root implementation, which in order to reduce the flop count employs the Cholesky - factorization of the Riccati recursion matrix, and therefore requires the full-space Hessian to be positive definite - - [HPIPM paper]: HPIPM: a high-performance quadratic programming framework for model predictive control, Frison and Diehl, 2020 - https://cdn.syscop.de/publications/Frison2020a.pdf - - Default: 1 - """ - return self.__qp_solver_ric_alg - @property def qp_solver_iter_max(self): """ @@ -2564,15 +2429,6 @@ def nlp_solver_tol_ineq(self): """NLP solver inequality tolerance""" return self.__nlp_solver_tol_ineq - @property - def nlp_solver_ext_qp_res(self): - """Determines if residuals of QP are computed externally within NLP solver (for debugging) - - Type: int; 0 or 1; - Default: 0. - """ - return self.__nlp_solver_ext_qp_res - @property def nlp_solver_tol_comp(self): """NLP solver complementarity tolerance""" @@ -2675,13 +2531,12 @@ def ext_cost_num_hess(self): def qp_solver(self, qp_solver): qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ - 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ - 'FULL_CONDENSING_DAQP') + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP') if qp_solver in qp_solvers: self.__qp_solver = qp_solver else: raise Exception('Invalid qp_solver value. Possible values are:\n\n' \ - + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\n') + + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.') @regularize_method.setter def regularize_method(self, regularize_method): @@ -2691,7 +2546,7 @@ def regularize_method(self, regularize_method): self.__regularize_method = regularize_method else: raise Exception('Invalid regularize_method value. Possible values are:\n\n' \ - + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\n') + + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.') @collocation_type.setter def collocation_type(self, collocation_type): @@ -2700,7 +2555,7 @@ def collocation_type(self, collocation_type): self.__collocation_type = collocation_type else: raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ - + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n') + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') @hpipm_mode.setter def hpipm_mode(self, hpipm_mode): @@ -2709,48 +2564,7 @@ def hpipm_mode(self, hpipm_mode): self.__hpipm_mode = hpipm_mode else: raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \ - + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\n') - - @ext_fun_compile_flags.setter - def ext_fun_compile_flags(self, ext_fun_compile_flags): - if isinstance(ext_fun_compile_flags, str): - self.__ext_fun_compile_flags = ext_fun_compile_flags - else: - raise Exception('Invalid ext_fun_compile_flags, expected a string.\n') - - - @custom_update_filename.setter - def custom_update_filename(self, custom_update_filename): - if isinstance(custom_update_filename, str): - self.__custom_update_filename = custom_update_filename - else: - raise Exception('Invalid custom_update_filename, expected a string.\n') - - @custom_templates.setter - def custom_templates(self, custom_templates): - if not isinstance(custom_templates, list): - raise Exception('Invalid custom_templates, expected a list.\n') - for tup in custom_templates: - if not isinstance(tup, tuple): - raise Exception('Invalid custom_templates, shoubld be list of tuples.\n') - for s in tup: - if not isinstance(s, str): - raise Exception('Invalid custom_templates, shoubld be list of tuples of strings.\n') - self.__custom_templates = custom_templates - - @custom_update_header_filename.setter - def custom_update_header_filename(self, custom_update_header_filename): - if isinstance(custom_update_header_filename, str): - self.__custom_update_header_filename = custom_update_header_filename - else: - raise Exception('Invalid custom_update_header_filename, expected a string.\n') - - @custom_update_copy.setter - def custom_update_copy(self, custom_update_copy): - if isinstance(custom_update_copy, bool): - self.__custom_update_copy = custom_update_copy - else: - raise Exception('Invalid custom_update_copy, expected a bool.\n') + + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\nExiting.') @hessian_approx.setter def hessian_approx(self, hessian_approx): @@ -2759,7 +2573,7 @@ def hessian_approx(self, hessian_approx): self.__hessian_approx = hessian_approx else: raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \ - + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\n') + + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.') @integrator_type.setter def integrator_type(self, integrator_type): @@ -2768,7 +2582,7 @@ def integrator_type(self, integrator_type): self.__integrator_type = integrator_type else: raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ - + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n') + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') @tf.setter def tf(self, tf): @@ -2805,7 +2619,7 @@ def globalization(self, globalization): self.__globalization = globalization else: raise Exception('Invalid globalization value. Possible values are:\n\n' \ - + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\n') + + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.') @alpha_min.setter def alpha_min(self, alpha_min): @@ -2841,7 +2655,7 @@ def eps_sufficient_descent(self, eps_sufficient_descent): if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0: self.__eps_sufficient_descent = eps_sufficient_descent else: - raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float.') + raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting') @sim_method_num_stages.setter def sim_method_num_stages(self, sim_method_num_stages): @@ -2849,7 +2663,7 @@ def sim_method_num_stages(self, sim_method_num_stages): # if isinstance(sim_method_num_stages, int): # self.__sim_method_num_stages = sim_method_num_stages # else: - # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer.') + # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.') self.__sim_method_num_stages = sim_method_num_stages @@ -2859,7 +2673,7 @@ def sim_method_num_steps(self, sim_method_num_steps): # if isinstance(sim_method_num_steps, int): # self.__sim_method_num_steps = sim_method_num_steps # else: - # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer.') + # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.') self.__sim_method_num_steps = sim_method_num_steps @@ -2869,7 +2683,7 @@ def sim_method_newton_iter(self, sim_method_newton_iter): if isinstance(sim_method_newton_iter, int): self.__sim_method_newton_iter = sim_method_newton_iter else: - raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer.') + raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.') @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): @@ -2885,57 +2699,42 @@ def nlp_solver_type(self, nlp_solver_type): self.__nlp_solver_type = nlp_solver_type else: raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ - + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\n') + + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.') @nlp_solver_step_length.setter def nlp_solver_step_length(self, nlp_solver_step_length): if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0: self.__nlp_solver_step_length = nlp_solver_step_length else: - raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float.') + raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting') @levenberg_marquardt.setter def levenberg_marquardt(self, levenberg_marquardt): if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0: self.__levenberg_marquardt = levenberg_marquardt else: - raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float.') + raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting') @qp_solver_iter_max.setter def qp_solver_iter_max(self, qp_solver_iter_max): if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0: self.__qp_solver_iter_max = qp_solver_iter_max else: - raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.') - - @qp_solver_ric_alg.setter - def qp_solver_ric_alg(self, qp_solver_ric_alg): - if qp_solver_ric_alg in [0, 1]: - self.__qp_solver_ric_alg = qp_solver_ric_alg - else: - raise Exception(f'Invalid qp_solver_ric_alg value. qp_solver_ric_alg must be in [0, 1], got {qp_solver_ric_alg}.') - - @qp_solver_cond_ric_alg.setter - def qp_solver_cond_ric_alg(self, qp_solver_cond_ric_alg): - if qp_solver_cond_ric_alg in [0, 1]: - self.__qp_solver_cond_ric_alg = qp_solver_cond_ric_alg - else: - raise Exception(f'Invalid qp_solver_cond_ric_alg value. qp_solver_cond_ric_alg must be in [0, 1], got {qp_solver_cond_ric_alg}.') - + raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting') @qp_solver_cond_N.setter def qp_solver_cond_N(self, qp_solver_cond_N): if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0: self.__qp_solver_cond_N = qp_solver_cond_N else: - raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int.') + raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting') @qp_solver_warm_start.setter def qp_solver_warm_start(self, qp_solver_warm_start): if qp_solver_warm_start in [0, 1, 2]: self.__qp_solver_warm_start = qp_solver_warm_start else: - raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2.') + raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting') @qp_tol.setter def qp_tol(self, qp_tol): @@ -2945,35 +2744,35 @@ def qp_tol(self, qp_tol): self.__qp_solver_tol_stat = qp_tol self.__qp_solver_tol_comp = qp_tol else: - raise Exception('Invalid qp_tol value. qp_tol must be a positive float.') + raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting') @qp_solver_tol_stat.setter def qp_solver_tol_stat(self, qp_solver_tol_stat): if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0: self.__qp_solver_tol_stat = qp_solver_tol_stat else: - raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float.') + raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting') @qp_solver_tol_eq.setter def qp_solver_tol_eq(self, qp_solver_tol_eq): if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0: self.__qp_solver_tol_eq = qp_solver_tol_eq else: - raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float.') + raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting') @qp_solver_tol_ineq.setter def qp_solver_tol_ineq(self, qp_solver_tol_ineq): if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0: self.__qp_solver_tol_ineq = qp_solver_tol_ineq else: - raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float.') + raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting') @qp_solver_tol_comp.setter def qp_solver_tol_comp(self, qp_solver_tol_comp): if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0: self.__qp_solver_tol_comp = qp_solver_tol_comp else: - raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float.') + raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting') @tol.setter def tol(self, tol): @@ -2983,42 +2782,35 @@ def tol(self, tol): self.__nlp_solver_tol_stat = tol self.__nlp_solver_tol_comp = tol else: - raise Exception('Invalid tol value. tol must be a positive float.') + raise Exception('Invalid tol value. tol must be a positive float. Exiting') @nlp_solver_tol_stat.setter def nlp_solver_tol_stat(self, nlp_solver_tol_stat): if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0: self.__nlp_solver_tol_stat = nlp_solver_tol_stat else: - raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float.') + raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting') @nlp_solver_tol_eq.setter def nlp_solver_tol_eq(self, nlp_solver_tol_eq): if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0: self.__nlp_solver_tol_eq = nlp_solver_tol_eq else: - raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float.') + raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting') @nlp_solver_tol_ineq.setter def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq): if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0: self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq else: - raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float.') - - @nlp_solver_ext_qp_res.setter - def nlp_solver_ext_qp_res(self, nlp_solver_ext_qp_res): - if nlp_solver_ext_qp_res in [0, 1]: - self.__nlp_solver_ext_qp_res = nlp_solver_ext_qp_res - else: - raise Exception('Invalid nlp_solver_ext_qp_res value. nlp_solver_ext_qp_res must be in [0, 1].') + raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting') @nlp_solver_tol_comp.setter def nlp_solver_tol_comp(self, nlp_solver_tol_comp): if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0: self.__nlp_solver_tol_comp = nlp_solver_tol_comp else: - raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float.') + raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting') @nlp_solver_max_iter.setter def nlp_solver_max_iter(self, nlp_solver_max_iter): @@ -3026,14 +2818,14 @@ def nlp_solver_max_iter(self, nlp_solver_max_iter): if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0: self.__nlp_solver_max_iter = nlp_solver_max_iter else: - raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int.') + raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting') @print_level.setter def print_level(self, print_level): if isinstance(print_level, int) and print_level >= 0: self.__print_level = print_level else: - raise Exception('Invalid print_level value. print_level takes one of the values >=0.') + raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting') @model_external_shared_lib_dir.setter def model_external_shared_lib_dir(self, model_external_shared_lib_dir): @@ -3041,47 +2833,47 @@ def model_external_shared_lib_dir(self, model_external_shared_lib_dir): self.__model_external_shared_lib_dir = model_external_shared_lib_dir else: raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\n') + + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.') @model_external_shared_lib_name.setter def model_external_shared_lib_name(self, model_external_shared_lib_name): if isinstance(model_external_shared_lib_name, str) : if model_external_shared_lib_name[-3:] == '.so' : raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') else : self.__model_external_shared_lib_name = model_external_shared_lib_name else: raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') @exact_hess_constr.setter def exact_hess_constr(self, exact_hess_constr): if exact_hess_constr in [0, 1]: self.__exact_hess_constr = exact_hess_constr else: - raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1.') + raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting') @exact_hess_cost.setter def exact_hess_cost(self, exact_hess_cost): if exact_hess_cost in [0, 1]: self.__exact_hess_cost = exact_hess_cost else: - raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1.') + raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting') @exact_hess_dyn.setter def exact_hess_dyn(self, exact_hess_dyn): if exact_hess_dyn in [0, 1]: self.__exact_hess_dyn = exact_hess_dyn else: - raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1.') + raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting') @ext_cost_num_hess.setter def ext_cost_num_hess(self, ext_cost_num_hess): if ext_cost_num_hess in [0, 1]: self.__ext_cost_num_hess = ext_cost_num_hess else: - raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.') + raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting') def set(self, attr, value): setattr(self, attr, value) @@ -3101,7 +2893,6 @@ class AcadosOcp: - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions` - :py:attr:`acados_include_path` (set automatically) - - :py:attr:`shared_lib_ext` (set automatically) - :py:attr:`acados_lib_path` (set automatically) - :py:attr:`parameter_values` - used to initialize the parameters (can be changed) """ @@ -3123,16 +2914,14 @@ def __init__(self, acados_path=''): """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`""" self.solver_options = AcadosOcpOptions() """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`""" - + self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake """Path to acados include directory (set automatically), type: `string`""" self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake """Path to where acados library is located, type: `string`""" - self.shared_lib_ext = get_lib_ext() - # get cython paths - from sysconfig import get_paths - self.cython_include_dirs = [np.get_include(), get_paths()['include']] + import numpy + self.cython_include_dirs = numpy.get_include() self.__parameter_values = np.array([]) self.__problem_class = 'OCP' diff --git a/third_party/acados/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py similarity index 75% rename from third_party/acados/acados_template/acados_ocp_solver.py rename to pyextra/acados_template/acados_ocp_solver.py index 229bdf60398e58..beeda2cb0c8749 100644 --- a/third_party/acados/acados_template/acados_ocp_solver.py +++ b/pyextra/acados_template/acados_ocp_solver.py @@ -1,5 +1,9 @@ +# -*- coding: future_fstrings -*- # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -34,29 +38,26 @@ import numpy as np from datetime import datetime import importlib -import shutil - -from subprocess import DEVNULL, call, STDOUT - from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref from copy import deepcopy -from pathlib import Path -from .casadi_function_generation import generate_c_code_explicit_ode, \ - generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_discrete_dynamics, \ - generate_c_code_constraint, generate_c_code_nls_cost, generate_c_code_conl_cost, \ - generate_c_code_external_cost -from .gnsf.detect_gnsf_structure import detect_gnsf_structure +from .generate_c_code_explicit_ode import generate_c_code_explicit_ode +from .generate_c_code_implicit_ode import generate_c_code_implicit_ode +from .generate_c_code_gnsf import generate_c_code_gnsf +from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics +from .generate_c_code_constraint import generate_c_code_constraint +from .generate_c_code_nls_cost import generate_c_code_nls_cost +from .generate_c_code_external_cost import generate_c_code_external_cost from .acados_ocp import AcadosOcp -from .acados_model import AcadosModel +from .acados_model import acados_model_strip_casadi_symbolics from .utils import is_column, is_empty, casadi_length, render_template,\ - format_class_dict, make_object_json_dumpable, make_model_consistent,\ - set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path, get_lib_ext, check_casadi_version + format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ + set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path from .builders import CMakeBuilder -def make_ocp_dims_consistent(acados_ocp: AcadosOcp): +def make_ocp_dims_consistent(acados_ocp): dims = acados_ocp.dims cost = acados_ocp.cost constraints = acados_ocp.constraints @@ -104,9 +105,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess - model.cost_psi_expr_0 = model.cost_psi_expr - model.cost_r_in_psi_expr_0 = model.cost_r_in_psi_expr - if cost.cost_type_0 == 'LINEAR_LS': ny_0 = cost.W_0.shape[0] if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0: @@ -135,22 +133,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n') dims.ny_0 = ny_0 - elif cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR': - if is_empty(model.cost_y_expr_0): - raise Exception('cost_y_expr_0 and/or cost_y_expr not provided.') - ny_0 = casadi_length(model.cost_y_expr_0) - if is_empty(model.cost_r_in_psi_expr_0) or casadi_length(model.cost_r_in_psi_expr_0) != ny_0: - raise Exception('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_0.') - if is_empty(model.cost_psi_expr_0) or casadi_length(model.cost_psi_expr_0) != 1: - raise Exception('cost_psi_expr_0 not provided or not scalar-valued.') - if cost.yref_0.shape[0] != ny_0: - raise Exception('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_0.') - dims.ny_0 = ny_0 - - if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" - "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") - elif cost.cost_type_0 == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_0 is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -189,23 +171,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') dims.ny = ny - elif cost.cost_type == 'CONVEX_OVER_NONLINEAR': - if is_empty(model.cost_y_expr): - raise Exception('cost_y_expr and/or cost_y_expr not provided.') - ny = casadi_length(model.cost_y_expr) - if is_empty(model.cost_r_in_psi_expr) or casadi_length(model.cost_r_in_psi_expr) != ny: - raise Exception('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi.') - if is_empty(model.cost_psi_expr) or casadi_length(model.cost_psi_expr) != 1: - raise Exception('cost_psi_expr not provided or not scalar-valued.') - if cost.yref.shape[0] != ny: - raise Exception('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi.') - dims.ny = ny - - if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" - "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") - - elif cost.cost_type == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -237,24 +202,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): raise Exception('inconsistent dimension: regarding W_e, yref_e.') dims.ny_e = ny_e - elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': - if is_empty(model.cost_y_expr_e): - raise Exception('cost_y_expr_e not provided.') - ny_e = casadi_length(model.cost_y_expr_e) - if is_empty(model.cost_r_in_psi_expr_e) or casadi_length(model.cost_r_in_psi_expr_e) != ny_e: - raise Exception('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_e.') - if is_empty(model.cost_psi_expr_e) or casadi_length(model.cost_psi_expr_e) != 1: - raise Exception('cost_psi_expr_e not provided or not scalar-valued.') - if cost.yref_e.shape[0] != ny_e: - raise Exception('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_e.') - dims.ny_e = ny_e - - if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" - "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") - - - elif cost.cost_type_e == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_e is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -266,13 +213,16 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): ## constraints # initial - this_shape = constraints.lbx_0.shape - other_shape = constraints.ubx_0.shape - if not this_shape == other_shape: - raise Exception('lbx_0, ubx_0 have different shapes!') - if not is_column(constraints.lbx_0): - raise Exception('lbx_0, ubx_0 must be column vectors!') - dims.nbx_0 = constraints.lbx_0.size + if (constraints.lbx_0 == [] and constraints.ubx_0 == []): + dims.nbx_0 = 0 + else: + this_shape = constraints.lbx_0.shape + other_shape = constraints.ubx_0.shape + if not this_shape == other_shape: + raise Exception('lbx_0, ubx_0 have different shapes!') + if not is_column(constraints.lbx_0): + raise Exception('lbx_0, ubx_0 must be column vectors!') + dims.nbx_0 = constraints.lbx_0.size if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \ and dims.nbxe_0 is None \ @@ -280,10 +230,8 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): and all(constraints.idxbxe_0 == constraints.idxbx_0): # case: x0 was set: nbx0 are all equlities. dims.nbxe_0 = dims.nbx_0 - elif constraints.idxbxe_0 is not None: - dims.nbxe_0 = constraints.idxbxe_0.shape[0] elif dims.nbxe_0 is None: - # case: x0 and idxbxe_0 were not set -> dont assume nbx0 to be equality constraints. + # case: x0 was not set -> dont assume nbx0 to be equality constraints. dims.nbxe_0 = 0 # path @@ -361,8 +309,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): # Slack dimensions nsbx = constraints.idxsbx.shape[0] - if nsbx > nbx: - raise Exception(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') if is_empty(constraints.lsbx): constraints.lsbx = np.zeros((nsbx,)) elif constraints.lsbx.shape[0] != nsbx: @@ -374,8 +320,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsbx = nsbx nsbu = constraints.idxsbu.shape[0] - if nsbu > nbu: - raise Exception(f'inconsistent dimension nsbu = {nsbu}. Is greater than nbu = {nbu}.') if is_empty(constraints.lsbu): constraints.lsbu = np.zeros((nsbu,)) elif constraints.lsbu.shape[0] != nsbu: @@ -387,8 +331,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsbu = nsbu nsh = constraints.idxsh.shape[0] - if nsh > nh: - raise Exception(f'inconsistent dimension nsh = {nsh}. Is greater than nh = {nh}.') if is_empty(constraints.lsh): constraints.lsh = np.zeros((nsh,)) elif constraints.lsh.shape[0] != nsh: @@ -400,8 +342,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsh = nsh nsphi = constraints.idxsphi.shape[0] - if nsphi > dims.nphi: - raise Exception(f'inconsistent dimension nsphi = {nsphi}. Is greater than nphi = {dims.nphi}.') if is_empty(constraints.lsphi): constraints.lsphi = np.zeros((nsphi,)) elif constraints.lsphi.shape[0] != nsphi: @@ -413,8 +353,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsphi = nsphi nsg = constraints.idxsg.shape[0] - if nsg > ng: - raise Exception(f'inconsistent dimension nsg = {nsg}. Is greater than ng = {ng}.') if is_empty(constraints.lsg): constraints.lsg = np.zeros((nsg,)) elif constraints.lsg.shape[0] != nsg: @@ -448,8 +386,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.ns = ns nsbx_e = constraints.idxsbx_e.shape[0] - if nsbx_e > nbx_e: - raise Exception(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') if is_empty(constraints.lsbx_e): constraints.lsbx_e = np.zeros((nsbx_e,)) elif constraints.lsbx_e.shape[0] != nsbx_e: @@ -461,8 +397,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsbx_e = nsbx_e nsh_e = constraints.idxsh_e.shape[0] - if nsh_e > nh_e: - raise Exception(f'inconsistent dimension nsh_e = {nsh_e}. Is greater than nh_e = {nh_e}.') if is_empty(constraints.lsh_e): constraints.lsh_e = np.zeros((nsh_e,)) elif constraints.lsh_e.shape[0] != nsh_e: @@ -474,8 +408,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsh_e = nsh_e nsg_e = constraints.idxsg_e.shape[0] - if nsg_e > ng_e: - raise Exception(f'inconsistent dimension nsg_e = {nsg_e}. Is greater than ng_e = {ng_e}.') if is_empty(constraints.lsg_e): constraints.lsg_e = np.zeros((nsg_e,)) elif constraints.lsg_e.shape[0] != nsg_e: @@ -487,8 +419,6 @@ def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims.nsg_e = nsg_e nsphi_e = constraints.idxsphi_e.shape[0] - if nsphi_e > dims.nphi_e: - raise Exception(f'inconsistent dimension nsphi_e = {nsphi_e}. Is greater than nphi_e = {dims.nphi_e}.') if is_empty(constraints.lsphi_e): constraints.lsphi_e = np.zeros((nsphi_e,)) elif constraints.lsphi_e.shape[0] != nsphi_e: @@ -595,7 +525,7 @@ def get_simulink_default_opts(): return simulink_default_opts -def ocp_formulation_json_dump(acados_ocp, simulink_opts=None, json_file='acados_ocp_nlp.json'): +def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'): # Load acados_ocp_nlp structure description ocp_layout = get_ocp_nlp_layout() @@ -613,11 +543,20 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts=None, json_file='acados_ ocp_nlp_dict = format_class_dict(ocp_nlp_dict) - if simulink_opts is not None: - ocp_nlp_dict['simulink_opts'] = simulink_opts + # strip symbolics + ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model']) + + # strip shooting_nodes + ocp_nlp_dict['solver_options'].pop('shooting_nodes', None) + dims_dict = format_class_dict(acados_ocp.dims.__dict__) + + ocp_check_against_layout(ocp_nlp_dict, dims_dict) + + # add simulink options + ocp_nlp_dict['simulink_opts'] = simulink_opts with open(json_file, 'w') as f: - json.dump(ocp_nlp_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True) + json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True) @@ -648,7 +587,7 @@ def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'): return acados_ocp -def ocp_generate_external_functions(acados_ocp: AcadosOcp, model: AcadosModel): +def ocp_generate_external_functions(acados_ocp, model): model = make_model_consistent(model) @@ -656,32 +595,27 @@ def ocp_generate_external_functions(acados_ocp: AcadosOcp, model: AcadosModel): opts = dict(generate_hess=1) else: opts = dict(generate_hess=0) - - # create code_export_dir, model_dir code_export_dir = acados_ocp.code_export_directory opts['code_export_directory'] = code_export_dir - model_dir = os.path.join(code_export_dir, model.name + '_model') - if not os.path.exists(model_dir): - os.makedirs(model_dir) - - check_casadi_version() - # TODO: remove dir gen from all the generate_c_* functions - if acados_ocp.model.dyn_ext_fun_type == 'casadi': - if acados_ocp.solver_options.integrator_type == 'ERK': - generate_c_code_explicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'IRK': - generate_c_code_implicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': - generate_c_code_implicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'GNSF': - generate_c_code_gnsf(model, opts) - elif acados_ocp.solver_options.integrator_type == 'DISCRETE': - generate_c_code_discrete_dynamics(model, opts) - else: - raise Exception("ocp_generate_external_functions: unknown integrator type.") + + if acados_ocp.model.dyn_ext_fun_type != 'casadi': + raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\ + Extending the Python interface with generic function support is welcome.") + + if acados_ocp.solver_options.integrator_type == 'ERK': + # explicit model -- generate C code + generate_c_code_explicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'IRK': + # implicit model -- generate C code + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + elif acados_ocp.solver_options.integrator_type == 'DISCRETE': + generate_c_code_discrete_dynamics(model, opts) else: - target_location = os.path.join(code_export_dir, model_dir, model.dyn_generic_source) - shutil.copyfile(model.dyn_generic_source, target_location) + raise Exception("ocp_generate_external_functions: unknown integrator type.") if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0: generate_c_code_constraint(model, model.name, False, opts) @@ -689,24 +623,28 @@ def ocp_generate_external_functions(acados_ocp: AcadosOcp, model: AcadosModel): if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0: generate_c_code_constraint(model, model.name, True, opts) + # dummy matrices + if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS': + acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx)) + acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type == 'LINEAR_LS': + acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx)) + acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu)) + if not acados_ocp.cost.cost_type_e == 'LINEAR_LS': + acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx)) + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'initial', opts) - elif acados_ocp.cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR': - generate_c_code_conl_cost(model, model.name, 'initial', opts) elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': generate_c_code_external_cost(model, 'initial', opts) if acados_ocp.cost.cost_type == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'path', opts) - elif acados_ocp.cost.cost_type == 'CONVEX_OVER_NONLINEAR': - generate_c_code_conl_cost(model, model.name, 'path', opts) elif acados_ocp.cost.cost_type == 'EXTERNAL': generate_c_code_external_cost(model, 'path', opts) if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'terminal', opts) - elif acados_ocp.cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': - generate_c_code_conl_cost(model, model.name, 'terminal', opts) elif acados_ocp.cost.cost_type_e == 'EXTERNAL': generate_c_code_external_cost(model, 'terminal', opts) @@ -721,8 +659,9 @@ def ocp_get_default_cmake_builder() -> CMakeBuilder: return cmake_builder +def ocp_render_templates(acados_ocp, json_file, cmake_builder=None): -def ocp_render_templates(acados_ocp: AcadosOcp, json_file, cmake_builder=None, simulink_opts=None): + name = acados_ocp.model.name # setting up loader and environment json_path = os.path.abspath(json_file) @@ -730,69 +669,132 @@ def ocp_render_templates(acados_ocp: AcadosOcp, json_file, cmake_builder=None, s if not os.path.exists(json_path): raise Exception(f'Path "{json_path}" not found!') - # Render templates - template_list = __ocp_get_template_list(acados_ocp, cmake_builder=cmake_builder, simulink_opts=simulink_opts) - for tup in template_list: - if len(tup) > 2: - output_dir = tup[2] - else: - output_dir = acados_ocp.code_export_directory - render_template(tup[0], tup[1], output_dir, json_path) - - # Custom templates - acados_template_path = os.path.dirname(os.path.abspath(__file__)) - custom_template_glob = os.path.join(acados_template_path, 'custom_update_templates', '*') - for tup in acados_ocp.solver_options.custom_templates: - render_template(tup[0], tup[1], acados_ocp.code_export_directory, json_path, template_glob=custom_template_glob) + code_export_dir = acados_ocp.code_export_directory + template_dir = code_export_dir - return + ## Render templates + in_file = 'main.in.c' + out_file = f'main_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + in_file = 'acados_solver.in.c' + out_file = f'acados_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + in_file = 'acados_solver.in.h' + out_file = f'acados_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) -def __ocp_get_template_list(acados_ocp: AcadosOcp, cmake_builder=None, simulink_opts=None) -> list: - """ - returns a list of tuples in the form: - (input_filename, output_filname) - or - (input_filename, output_filname, output_directory) - """ - name = acados_ocp.model.name - code_export_directory = acados_ocp.code_export_directory - template_list = [] + in_file = 'acados_solver.in.pxd' + out_file = f'acados_solver.pxd' + render_template(in_file, out_file, template_dir, json_path) - template_list.append(('main.in.c', f'main_{name}.c')) - template_list.append(('acados_solver.in.c', f'acados_solver_{name}.c')) - template_list.append(('acados_solver.in.h', f'acados_solver_{name}.h')) - template_list.append(('acados_solver.in.pxd', f'acados_solver.pxd')) if cmake_builder is not None: - template_list.append(('CMakeLists.in.txt', 'CMakeLists.txt')) + in_file = 'CMakeLists.in.txt' + out_file = 'CMakeLists.txt' + render_template(in_file, out_file, template_dir, json_path) else: - template_list.append(('Makefile.in', 'Makefile')) + in_file = 'Makefile.in' + out_file = 'Makefile' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_solver_sfun.in.c' + out_file = f'acados_solver_sfunction_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + in_file = 'make_sfun.in.m' + out_file = f'make_sfun_{name}.m' + render_template(in_file, out_file, template_dir, json_path) # sim - template_list.append(('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c')) - template_list.append(('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h')) - template_list.append(('main_sim.in.c', f'main_sim_{name}.c')) - - # model - model_dir = os.path.join(code_export_directory, f'{name}_model') - template_list.append(('model.in.h', f'{name}_model.h', model_dir)) - # constraints - constraints_dir = os.path.join(code_export_directory, f'{name}_constraints') - template_list.append(('constraints.in.h', f'{name}_constraints.h', constraints_dir)) - # cost - cost_dir = os.path.join(code_export_directory, f'{name}_cost') - template_list.append(('cost.in.h', f'{name}_cost.h', cost_dir)) - - # Simulink - if simulink_opts is not None: - template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') - template_list.append((template_file, f'acados_solver_sfunction_{name}.c')) - template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') - template_list.append((template_file, f'make_sfun_{name}.m')) - - return template_list + in_file = 'acados_sim_solver.in.c' + out_file = f'acados_sim_solver_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_sim_solver.in.h' + out_file = f'acados_sim_solver_{name}.h' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'main_sim.in.c' + out_file = f'main_sim_{name}.c' + render_template(in_file, out_file, template_dir, json_path) + + ## folder model + template_dir = os.path.join(code_export_dir, name + '_model') + in_file = 'model.in.h' + out_file = f'{name}_model.h' + render_template(in_file, out_file, template_dir, json_path) + + # constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0: + # constraints on outer function + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'phi_constraint.in.h' + out_file = f'{name}_phi_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal constraints on convex over nonlinear function + if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0: + # terminal constraints on outer function + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'phi_e_constraint.in.h' + out_file = f'{name}_phi_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # nonlinear constraints + if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0: + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'h_constraint.in.h' + out_file = f'{name}_h_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal nonlinear constraints + if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0: + template_dir = os.path.join(code_export_dir, name + '_constraints') + in_file = 'h_e_constraint.in.h' + out_file = f'{name}_h_e_constraint.h' + render_template(in_file, out_file, template_dir, json_path) + + # initial stage Nonlinear LS cost function + if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_0_fun.in.h' + out_file = f'{name}_cost_y_0_fun.h' + render_template(in_file, out_file, template_dir, json_path) + # external cost - terminal + elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost_0.in.h' + out_file = f'{name}_external_cost_0.h' + render_template(in_file, out_file, template_dir, json_path) + + # path Nonlinear LS cost function + if acados_ocp.cost.cost_type == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_fun.in.h' + out_file = f'{name}_cost_y_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # terminal Nonlinear LS cost function + if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'cost_y_e_fun.in.h' + out_file = f'{name}_cost_y_e_fun.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost + if acados_ocp.cost.cost_type == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost.in.h' + out_file = f'{name}_external_cost.h' + render_template(in_file, out_file, template_dir, json_path) + + # external cost - terminal + if acados_ocp.cost.cost_type_e == 'EXTERNAL': + template_dir = os.path.join(code_export_dir, name + '_cost') + in_file = 'external_cost_e.in.h' + out_file = f'{name}_external_cost_e.h' + render_template(in_file, out_file, template_dir, json_path) def remove_x0_elimination(acados_ocp): @@ -818,7 +820,7 @@ class AcadosOcpSolver: dlclose.argtypes = [c_void_p] @classmethod - def generate(cls, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None): + def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None): """ Generates the code for an acados OCP solver, given the description in acados_ocp. :param acados_ocp: type AcadosOcp - description of the OCP for acados @@ -832,15 +834,15 @@ def generate(cls, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simuli model = acados_ocp.model acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) + if simulink_opts is None: + simulink_opts = get_simulink_default_opts() + # make dims consistent make_ocp_dims_consistent(acados_ocp) # module dependent post processing if acados_ocp.solver_options.integrator_type == 'GNSF': - if 'gnsf_model' in acados_ocp.__dict__: - set_up_imported_gnsf_model(acados_ocp) - else: - detect_gnsf_structure(acados_ocp) + set_up_imported_gnsf_model(acados_ocp) if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': remove_x0_elimination(acados_ocp) @@ -852,23 +854,15 @@ def generate(cls, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simuli ocp_generate_external_functions(acados_ocp, model) # dump to json - acados_ocp.json_file = json_file - ocp_formulation_json_dump(acados_ocp, simulink_opts=simulink_opts, json_file=json_file) + ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file) # render templates - ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder, simulink_opts=simulink_opts) - - # copy custom update function - if acados_ocp.solver_options.custom_update_filename != "" and acados_ocp.solver_options.custom_update_copy: - target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_filename) - shutil.copyfile(acados_ocp.solver_options.custom_update_filename, target_location) - if acados_ocp.solver_options.custom_update_header_filename != "" and acados_ocp.solver_options.custom_update_copy: - target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_header_filename) - shutil.copyfile(acados_ocp.solver_options.custom_update_header_filename, target_location) + ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder) + acados_ocp.json_file = json_file @classmethod - def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): + def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None): """ Builds the code for an acados OCP solver, that has been generated in code_export_dir :param code_export_dir: directory in which acados OCP solver has been generated, see generate() @@ -876,36 +870,19 @@ def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with `MS Visual Studio`); default: `None` - :param verbose: indicating if build command is printed """ code_export_dir = os.path.abspath(code_export_dir) - cwd = os.getcwd() + cwd=os.getcwd() os.chdir(code_export_dir) if with_cython: - call( - ['make', 'clean_all'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) - call( - ['make', 'ocp_cython'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) + os.system('make clean_ocp_cython') + os.system('make ocp_cython') else: if cmake_builder is not None: cmake_builder.exec(code_export_dir) else: - call( - ['make', 'clean_ocp_shared_lib'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) - call( - ['make', 'ocp_shared_lib'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) + os.system('make clean_ocp_shared_lib') + os.system('make ocp_shared_lib') os.chdir(cwd) @@ -933,7 +910,7 @@ def create_cython_solver(cls, json_file): acados_ocp_json['dims']['N']) - def __init__(self, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True): + def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None): self.solver_created = False if generate: @@ -950,13 +927,15 @@ def __init__(self, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simul code_export_directory = acados_ocp_json['code_export_directory'] if build: - self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder, verbose=verbose) + self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder) # prepare library loading lib_prefix = 'lib' - lib_ext = get_lib_ext() + lib_ext = '.so' if os.name == 'nt': lib_prefix = '' + lib_ext = '' + # ToDo: check for mac # Load acados library to avoid unloading the library. # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed. @@ -991,23 +970,16 @@ def __init__(self, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simul assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0 self.solver_created = True - self.acados_ocp = acados_ocp - # get pointers solver self.__get_pointers_solver() self.status = 0 - # gettable fields - self.__qp_dynamics_fields = ['A', 'B', 'b'] - self.__qp_cost_fields = ['Q', 'R', 'S', 'q', 'r'] - self.__qp_constraint_fields = ['C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] - def __get_pointers_solver(self): - # """ - # Private function to get the pointers for solver - # """ + """ + Private function to get the pointers for solver + """ # get pointers solver getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p] getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p @@ -1038,25 +1010,6 @@ def __get_pointers_solver(self): self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule) - - def solve_for_x0(self, x0_bar): - """ - Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0. - """ - self.set(0, "lbx", x0_bar) - self.set(0, "ubx", x0_bar) - - status = self.solve() - - if status == 2: - print("Warning: acados_ocp_solver reached maximum iterations.") - elif status != 0: - raise Exception(f'acados acados_ocp_solver returned status {status}') - - u0 = self.get(0, "u") - return u0 - - def solve(self): """ Solve the ocp with current input. @@ -1068,31 +1021,13 @@ def solve(self): return self.status - def custom_update(self, data_: np.ndarray): - """ - A custom function that can be implemented by a user to be called between solver calls. - By default this does nothing. - The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C, - in a function that is compiled into the solver library and can be conveniently used in the Python environment. - """ - data = np.ascontiguousarray(data_, dtype=np.float64) - c_data = cast(data.ctypes.data, POINTER(c_double)) - data_len = len(data) - - getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").argtypes = [c_void_p, POINTER(c_double), c_int] - getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").restype = c_int - status = getattr(self.shared_lib, f"{self.model_name}_acados_custom_update")(self.capsule, c_data, data_len) - - return status - - - def reset(self, reset_qp_solver_mem=1): + def reset(self): """ Sets current iterate to all zeros. """ - getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p, c_int] + getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p] getattr(self.shared_lib, f"{self.model_name}_acados_reset").restype = c_int - getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule, reset_qp_solver_mem) + getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule) return @@ -1240,17 +1175,18 @@ def get(self, stage_, field_): field = field_ if (field_ not in all_fields): - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): \'{field_}\' is an invalid argument.\ - \n Possible values are {all_fields}.') + raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, all_fields)) if not isinstance(stage_, int): - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be an integer, got type {type(stage_)}.') + raise Exception('AcadosOcpSolver.get(): stage index must be Integer.') if stage_ < 0 or stage_ > self.N: - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be in [0, {self.N}], got: {stage_}.') + raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_)) if stage_ == self.N and field_ == 'pi': - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): field \'{field_}\' does not exist at final stage {stage_}.') + raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\ + .format(field_, stage_)) if field_ in sens_fields: field = field_.replace('sens_', '') @@ -1329,15 +1265,15 @@ def print_statistics(self): return - def store_iterate(self, filename: str = '', overwrite=False): + def store_iterate(self, filename='', overwrite=False): """ Stores the current iterate of the ocp solver in a json file. - :param filename: if not set, use f'{self.model_name}_iterate.json' + :param filename: if not set, use model_name + timestamp + '.json' :param overwrite: if false and filename exists add timestamp to filename """ if filename == '': - filename = f'{self.model_name}_iterate.json' + filename += self.model_name + '_' + 'iterate' + '.json' if not overwrite: # append timestamp @@ -1348,70 +1284,23 @@ def store_iterate(self, filename: str = '', overwrite=False): # get iterate: solution = dict() - lN = len(str(self.N+1)) for i in range(self.N+1): - i_string = f'{i:0{lN}d}' - solution['x_'+i_string] = self.get(i,'x') - solution['u_'+i_string] = self.get(i,'u') - solution['z_'+i_string] = self.get(i,'z') - solution['lam_'+i_string] = self.get(i,'lam') - solution['t_'+i_string] = self.get(i, 't') - solution['sl_'+i_string] = self.get(i, 'sl') - solution['su_'+i_string] = self.get(i, 'su') - if i < self.N: - solution['pi_'+i_string] = self.get(i,'pi') - - for k in list(solution.keys()): - if len(solution[k]) == 0: - del solution[k] + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') # save with open(filename, 'w') as f: - json.dump(solution, f, default=make_object_json_dumpable, indent=4, sort_keys=True) + json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True) print("stored current iterate in ", os.path.join(os.getcwd(), filename)) - - def dump_last_qp_to_json(self, filename: str = '', overwrite=False): - """ - Dumps the latest QP data into a json file - - :param filename: if not set, use model_name + timestamp + '.json' - :param overwrite: if false and filename exists add timestamp to filename - """ - if filename == '': - filename = f'{self.model_name}_QP.json' - - if not overwrite: - # append timestamp - if os.path.isfile(filename): - filename = filename[:-5] - filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json' - - # get QP data: - qp_data = dict() - - lN = len(str(self.N+1)) - for field in self.__qp_dynamics_fields: - for i in range(self.N): - qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field) - - for field in self.__qp_constraint_fields + self.__qp_cost_fields: - for i in range(self.N+1): - qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field) - - # remove empty fields - for k in list(qp_data.keys()): - if len(qp_data[k]) == 0: - del qp_data[k] - - # save - with open(filename, 'w') as f: - json.dump(qp_data, f, default=make_object_json_dumpable, indent=4, sort_keys=True) - print("stored qp from solver memory in ", os.path.join(os.getcwd(), filename)) - - - def load_iterate(self, filename): """ Loads the iterate stored in json file with filename into the ocp solver. @@ -1523,7 +1412,7 @@ def get_stats(self, field_): return self.get_residuals() else: - raise Exception(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' + raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.' + f'\n Possible values are {fields}.') @@ -1551,12 +1440,6 @@ def get_cost(self): def get_residuals(self, recompute=False): """ Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. - This residual has to be computed for SQP_RTI solver, since it is not available by default. - - - res_stat: stationarity residual - - res_eq: residual wrt equality constraints (dynamics) - - res_ineq: residual wrt inequality constraints (constraints) - - res_comp: residual wrt complementarity conditions """ # compute residuals if RTI if self.solver_options['nlp_solver_type'] == 'SQP_RTI' or recompute: @@ -1616,22 +1499,24 @@ def set(self, stage_, field_, value_): value_ = np.array([value_]) value_ = value_.astype(float) - field = field_.encode('utf-8') + field = field_ + field = field.encode('utf-8') stage = c_int(stage_) # treat parameters separately if field_ == 'p': - getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double), c_int] + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int value_data = cast(value_.ctypes.data, POINTER(c_double)) assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 else: - if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: - raise Exception(f"AcadosOcpSolver.set(): '{field}' is not a valid argument.\n" - f" Possible values are {constraints_fields + cost_fields + out_fields + mem_fields + ['p']}.") + if field_ not in constraints_fields + cost_fields + out_fields: + raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ + \nPossible values are {}. Exiting.".format(field, \ + constraints_fields + cost_fields + out_fields + ['p'])) self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p] @@ -1641,8 +1526,8 @@ def set(self, stage_, field_, value_): self.nlp_dims, self.nlp_out, stage_, field) if value_.shape[0] != dims: - msg = f'AcadosOcpSolver.set(): mismatching dimension for field "{field_}" ' - msg += f'with dimension {dims} (you have {value_.shape[0]})' + msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) raise Exception(msg) value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -1668,13 +1553,6 @@ def set(self, stage_, field_, value_): [c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_set(self.nlp_config, \ self.nlp_solver, stage, field, value_data_p) - # also set z_guess, when setting z. - if field_ == 'z': - field = 'z_guess'.encode('utf-8') - self.shared_lib.ocp_nlp_set.argtypes = \ - [c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_set(self.nlp_config, \ - self.nlp_solver, stage, field, value_data_p) return @@ -1683,7 +1561,7 @@ def cost_set(self, stage_, field_, value_, api='warn'): Set numerical data in the cost module of the solver. :param stage: integer corresponding to shooting node - :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu' + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' :param value: of appropriate size """ # cast value_ to avoid conversion issues @@ -1717,7 +1595,7 @@ def cost_set(self, stage_, field_, value_, api='warn'): raise Exception("Ambiguity in API detected.\n" "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" - f" You are relying on a now-fixed bug in cost_set for field '{field_}'.\n" + + " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + " acados_template now correctly passes on any matrices to acados in column major format.\n" + " Two options to fix this error: \n" + " * Add api='old' to cost_set to restore old incorrect behaviour\n" + @@ -1785,7 +1663,7 @@ def constraints_set(self, stage_, field_, value_, api='warn'): raise Exception("Ambiguity in API detected.\n" "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" - f" You are relying on a now-fixed bug in constraints_set for field '{field}'.\n" + + " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + " acados_template now correctly passes on any matrices to acados in column major format.\n" + " Two options to fix this error: \n" + " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + @@ -1798,7 +1676,7 @@ def constraints_set(self, stage_, field_, value_, api='warn'): # Get elements in column major order value_ = np.ravel(value_, order='F') else: - raise Exception(f"Unknown api: '{api}'") + raise Exception("Unknown api: '{}'".format(api)) if value_shape != tuple(dims): raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' + @@ -1815,35 +1693,27 @@ def constraints_set(self, stage_, field_, value_, api='warn'): return - def get_from_qp_in(self, stage_: int, field_: str): + def dynamics_get(self, stage_, field_): """ - Get numerical data from the current QP. + Get numerical data from the dynamics module of the solver: :param stage: integer corresponding to shooting node - :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] + :param field: string, e.g. 'A' """ - # idx* should be added too.. - if not isinstance(stage_, int): - raise TypeError("stage should be int") - if stage_ > self.N: - raise Exception("stage should be <= self.N") - if field_ in self.__qp_dynamics_fields and stage_ >= self.N: - raise ValueError(f"dynamics field {field_} not available at terminal stage") - if field_ not in self.__qp_dynamics_fields + self.__qp_cost_fields + self.__qp_constraint_fields: - raise Exception(f"field {field_} not supported.") - field = field_.encode('utf-8') + field = field_ + field = field.encode('utf-8') stage = c_int(stage_) # get dims - self.shared_lib.ocp_nlp_qp_dims_get_from_attr.argtypes = \ + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] - self.shared_lib.ocp_nlp_qp_dims_get_from_attr.restype = c_int + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) dims_data = cast(dims.ctypes.data, POINTER(c_int)) - self.shared_lib.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, \ + self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage_, field, dims_data) # create output data @@ -1878,34 +1748,32 @@ def options_set(self, field_, value_): - qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness - warm_start_first_qp: indicates if first QP in SQP is warm_started """ - int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', - 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp'] - double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', - 'eps_sufficient_descent', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'] + int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp'] + double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent', + 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'] string_fields = ['globalization'] # check field availability and type if field_ in int_fields: if not isinstance(value_, int): - raise Exception(f'solver option \'{field_}\' must be of type int. You have {type(value_)}.') + raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) else: value_ctypes = c_int(value_) elif field_ in double_fields: if not isinstance(value_, float): - raise Exception(f'solver option \'{field_}\' must be of type float. You have {type(value_)}.') + raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) else: value_ctypes = c_double(value_) elif field_ in string_fields: if not isinstance(value_, str): - raise Exception(f'solver option \'{field_}\' must be of type str. You have {type(value_)}.') + raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) else: value_ctypes = value_.encode('utf-8') else: - fields = ', '.join(int_fields + double_fields + string_fields) - raise Exception(f'AcadosOcpSolver.options_set() does not support field \'{field_}\'.\n'\ - f' Possible values are {fields}.') + raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) if field_ == 'rti_phase': @@ -1934,44 +1802,6 @@ def options_set(self, field_, value_): return - def set_params_sparse(self, stage_, idx_values_, param_values_): - """ - set parameters of the solvers external function partially: - Pseudo: solver.param[idx_values_] = param_values_; - Parameters: - :param stage_: integer corresponding to shooting node - :param idx_values_: 0 based np array (or iterable) of integers: indices of parameter to be set - :param param_values_: new parameter values as numpy array - """ - - # if not isinstance(idx_values_, np.ndarray) or not issubclass(type(idx_values_[0]), np.integer): - # raise Exception('idx_values_ must be np.array of integers.') - - if not isinstance(param_values_, np.ndarray): - raise Exception('param_values_ must be np.array.') - elif np.float64 != param_values_.dtype: - raise TypeError('param_values_ must be np.array of float64.') - - if param_values_.shape[0] != len(idx_values_): - raise Exception(f'param_values_ and idx_values_ must be of the same size.' + - f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.') - - if any(idx_values_ >= self.acados_ocp.dims.np): - raise Exception(f'idx_values_ contains value >= np = {self.acados_ocp.dims.np}') - - stage = c_int(stage_) - n_update = c_int(len(param_values_)) - - param_data = cast(param_values_.ctypes.data, POINTER(c_double)) - c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc) - idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int)) - - getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \ - [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int] - getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int - getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \ - (self.capsule, stage, idx_data, param_data, n_update) - def __del__(self): if self.solver_created: getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p] @@ -1985,6 +1815,4 @@ def __del__(self): try: self.dlclose(self.shared_lib._handle) except: - print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosOcpSolver {self.model_name}.\n", - "Attempting to create a new one with the same name will likely result in the old one being used!") pass diff --git a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx similarity index 84% rename from third_party/acados/acados_template/acados_ocp_solver_pyx.pyx rename to pyextra/acados_template/acados_ocp_solver_pyx.pyx index bc03ba086fed1a..fe7fa8425afcc6 100644 --- a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -1,5 +1,9 @@ +# -*- coding: future_fstrings -*- # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -59,6 +63,7 @@ cdef class AcadosOcpSolverCython: cdef acados_solver_common.ocp_nlp_in *nlp_in cdef acados_solver_common.ocp_nlp_solver *nlp_solver + cdef int status cdef bint solver_created cdef str model_name @@ -83,6 +88,7 @@ cdef class AcadosOcpSolverCython: # get pointers solver self.__get_pointers_solver() + self.status = 0 def __get_pointers_solver(self): @@ -99,24 +105,6 @@ cdef class AcadosOcpSolverCython: self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule) - def solve_for_x0(self, x0_bar): - """ - Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0. - """ - self.set(0, "lbx", x0_bar) - self.set(0, "ubx", x0_bar) - - status = self.solve() - - if status == 2: - print("Warning: acados_ocp_solver reached maximum iterations.") - elif status != 0: - raise Exception(f'acados acados_ocp_solver returned status {status}') - - u0 = self.get(0, "u") - return u0 - - def solve(self): """ Solve the ocp with current input. @@ -124,24 +112,11 @@ cdef class AcadosOcpSolverCython: return acados_solver.acados_solve(self.capsule) - def reset(self, reset_qp_solver_mem=1): + def reset(self): """ Sets current iterate to all zeros. """ - return acados_solver.acados_reset(self.capsule, reset_qp_solver_mem) - - - def custom_update(self, data_): - """ - A custom function that can be implemented by a user to be called between solver calls. - By default this does nothing. - The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C, - in a function that is compiled into the solver library and can be conveniently used in the Python environment. - """ - data_len = len(data_) - cdef cnp.ndarray[cnp.float64_t, ndim=1] data = np.ascontiguousarray(data_, dtype=np.float64) - - return acados_solver.acados_custom_update(self.capsule, data.data, data_len) + return acados_solver.acados_reset(self.capsule) def set_new_time_steps(self, new_time_steps): @@ -278,7 +253,7 @@ cdef class AcadosOcpSolverCython: if field_ not in out_fields: raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\ - \n Possible values are {}.'.format(field_, out_fields)) + \n Possible values are {}. Exiting.'.format(field_, out_fields)) if stage < 0 or stage > self.N: raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N)) @@ -335,22 +310,16 @@ cdef class AcadosOcpSolverCython: # get iterate: solution = dict() - lN = len(str(self.N+1)) for i in range(self.N+1): - i_string = f'{i:0{lN}d}' - solution['x_'+i_string] = self.get(i,'x') - solution['u_'+i_string] = self.get(i,'u') - solution['z_'+i_string] = self.get(i,'z') - solution['lam_'+i_string] = self.get(i,'lam') - solution['t_'+i_string] = self.get(i, 't') - solution['sl_'+i_string] = self.get(i, 'sl') - solution['su_'+i_string] = self.get(i, 'su') - if i < self.N: - solution['pi_'+i_string] = self.get(i,'pi') - - for k in list(solution.keys()): - if len(solution[k]) == 0: - del solution[k] + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') # save with open(filename, 'w') as f: @@ -539,8 +508,6 @@ cdef class AcadosOcpSolverCython: sl: slack variables of soft lower inequality constraints \n su: slack variables of soft upper inequality constraints \n """ - if not isinstance(value_, np.ndarray): - raise Exception(f"set: value must be numpy array, got {type(value_)}.") cost_fields = ['y_ref', 'yref'] constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su'] @@ -556,7 +523,7 @@ cdef class AcadosOcpSolverCython: else: if field_ not in constraints_fields + cost_fields + out_fields: raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\ - \nPossible values are {}.".format(field, \ + \nPossible values are {}. Exiting.".format(field, \ constraints_fields + cost_fields + out_fields + ['p'])) dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, @@ -580,11 +547,6 @@ cdef class AcadosOcpSolverCython: acados_solver_common.ocp_nlp_set(self.nlp_config, \ self.nlp_solver, stage, field, value.data) - if field_ == 'z': - field = 'z_guess'.encode('utf-8') - acados_solver_common.ocp_nlp_set(self.nlp_config, \ - self.nlp_solver, stage, field, value.data) - return def cost_set(self, int stage, str field_, value_): """ @@ -594,8 +556,6 @@ cdef class AcadosOcpSolverCython: :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' :param value: of appropriate size """ - if not isinstance(value_, np.ndarray): - raise Exception(f"cost_set: value must be numpy array, got {type(value_)}.") field = field_.encode('utf-8') cdef int dims[2] @@ -629,9 +589,6 @@ cdef class AcadosOcpSolverCython: :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] :param value: of appropriate size """ - if not isinstance(value_, np.ndarray): - raise Exception(f"constraints_set: value must be numpy array, got {type(value_)}.") - field = field_.encode('utf-8') cdef int dims[2] @@ -649,7 +606,7 @@ cdef class AcadosOcpSolverCython: # Get elements in column major order value = np.asfortranarray(value_) - if value_shape != tuple(dims): + if value_shape[0] != dims[0] or value_shape[1] != dims[1]: raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') @@ -659,7 +616,7 @@ cdef class AcadosOcpSolverCython: return - def get_from_qp_in(self, int stage, str field_): + def dynamics_get(self, int stage, str field_): """ Get numerical data from the dynamics module of the solver: @@ -670,7 +627,7 @@ cdef class AcadosOcpSolverCython: # get dims cdef int[2] dims - acados_solver_common.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0]) # create output data cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F') @@ -744,50 +701,6 @@ cdef class AcadosOcpSolverCython: '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) - def set_params_sparse(self, int stage, idx_values_, param_values_): - """ - set parameters of the solvers external function partially: - Pseudo: solver.param[idx_values_] = param_values_; - Parameters: - :param stage_: integer corresponding to shooting node - :param idx_values_: 0 based integer array corresponding to parameter indices to be set - :param param_values_: new parameter values as numpy array - """ - - if not isinstance(param_values_, np.ndarray): - raise Exception('param_values_ must be np.array.') - - if param_values_.shape[0] != len(idx_values_): - raise Exception(f'param_values_ and idx_values_ must be of the same size.' + - f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.') - - # n_update = c_int(len(param_values_)) - - # param_data = cast(param_values_.ctypes.data, POINTER(c_double)) - # c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc) - # idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int)) - - # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \ - # [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int] - # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int - # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \ - # (self.capsule, stage, idx_data, param_data, n_update) - - cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(param_values_, dtype=np.float64) - # cdef cnp.ndarray[cnp.intc, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc) - - # NOTE: this does throw an error somehow: - # ValueError: Buffer dtype mismatch, expected 'int object' but got 'int' - # cdef cnp.ndarray[cnp.int, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc) - - cdef cnp.ndarray[cnp.int32_t, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.int32) - cdef int n_update = value.shape[0] - # print(f"in set_params_sparse Cython n_update {n_update}") - - assert acados_solver.acados_update_params_sparse(self.capsule, stage, idx.data, value.data, n_update) == 0 - return - - def __del__(self): if self.solver_created: acados_solver.acados_free(self.capsule) diff --git a/third_party/acados/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py similarity index 86% rename from third_party/acados/acados_template/acados_sim.py rename to pyextra/acados_template/acados_sim.py index 7faa49fb125447..93d5f298db4698 100644 --- a/third_party/acados/acados_template/acados_sim.py +++ b/pyextra/acados_template/acados_sim.py @@ -1,5 +1,9 @@ +# -*- coding: future_fstrings -*- # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -29,9 +33,10 @@ # import numpy as np +import casadi as ca import os from .acados_model import AcadosModel -from .utils import get_acados_path, get_lib_ext +from .utils import get_acados_path class AcadosSimDims: """ @@ -68,28 +73,28 @@ def nx(self, nx): if isinstance(nx, int) and nx > 0: self.__nx = nx else: - raise Exception('Invalid nx value, expected positive integer.') + raise Exception('Invalid nx value, expected positive integer. Exiting.') @nz.setter def nz(self, nz): if isinstance(nz, int) and nz > -1: self.__nz = nz else: - raise Exception('Invalid nz value, expected nonnegative integer.') + raise Exception('Invalid nz value, expected nonnegative integer. Exiting.') @nu.setter def nu(self, nu): if isinstance(nu, int) and nu > -1: self.__nu = nu else: - raise Exception('Invalid nu value, expected nonnegative integer.') + raise Exception('Invalid nu value, expected nonnegative integer. Exiting.') @np.setter def np(self, np): if isinstance(np, int) and np > -1: self.__np = np else: - raise Exception('Invalid np value, expected nonnegative integer.') + raise Exception('Invalid np value, expected nonnegative integer. Exiting.') def set(self, attr, value): setattr(self, attr, value) @@ -107,16 +112,13 @@ def __init__(self): self.__sim_method_num_stages = 1 self.__sim_method_num_steps = 1 self.__sim_method_newton_iter = 3 - # doubles - self.__sim_method_newton_tol = 0.0 # bools self.__sens_forw = True self.__sens_adj = False self.__sens_algebraic = False self.__sens_hess = False - self.__output_z = True + self.__output_z = False self.__sim_method_jac_reuse = 0 - self.__ext_fun_compile_flags = '-O2' @property def integrator_type(self): @@ -138,15 +140,6 @@ def newton_iter(self): """Number of Newton iterations in simulation method. Default: 3""" return self.__sim_method_newton_iter - @property - def newton_tol(self): - """ - Tolerance for Newton system solved in implicit integrator (IRK, GNSF). - 0.0 means this is not used and exactly newton_iter iterations are carried out. - Default: 0.0 - """ - return self.__sim_method_newton_tol - @property def sens_forw(self): """Boolean determining if forward sensitivities are computed. Default: True""" @@ -169,7 +162,7 @@ def sens_hess(self): @property def output_z(self): - """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True""" + """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: False""" return self.__output_z @property @@ -191,21 +184,6 @@ def collocation_type(self): """ return self.__collocation_type - @property - def ext_fun_compile_flags(self): - """ - String with compiler flags for external function compilation. - Default: '-O2'. - """ - return self.__ext_fun_compile_flags - - @ext_fun_compile_flags.setter - def ext_fun_compile_flags(self, ext_fun_compile_flags): - if isinstance(ext_fun_compile_flags, str): - self.__ext_fun_compile_flags = ext_fun_compile_flags - else: - raise Exception('Invalid ext_fun_compile_flags, expected a string.\n') - @integrator_type.setter def integrator_type(self, integrator_type): integrator_types = ('ERK', 'IRK', 'GNSF') @@ -213,7 +191,7 @@ def integrator_type(self, integrator_type): self.__integrator_type = integrator_type else: raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ - + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n') + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') @collocation_type.setter def collocation_type(self, collocation_type): @@ -222,7 +200,7 @@ def collocation_type(self, collocation_type): self.__collocation_type = collocation_type else: raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ - + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n') + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') @T.setter def T(self, T): @@ -249,13 +227,6 @@ def newton_iter(self, newton_iter): else: raise Exception('Invalid newton_iter value. newton_iter must be an integer.') - @newton_tol.setter - def newton_tol(self, newton_tol): - if isinstance(newton_tol, float): - self.__sim_method_newton_tol = newton_tol - else: - raise Exception('Invalid newton_tol value. newton_tol must be an float.') - @sens_forw.setter def sens_forw(self, sens_forw): if sens_forw in (True, False): @@ -309,7 +280,6 @@ class AcadosSim: - :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts` - :py:attr:`acados_include_path` (set automatically) - - :py:attr:`shared_lib_ext` (set automatically) - :py:attr:`acados_lib_path` (set automatically) - :py:attr:`parameter_values` - used to initialize the parameters (can be changed) @@ -331,14 +301,9 @@ def __init__(self, acados_path=''): self.code_export_directory = 'c_generated_code' """Path to where code will be exported. Default: `c_generated_code`.""" - self.shared_lib_ext = get_lib_ext() - - # get cython paths - from sysconfig import get_paths - self.cython_include_dirs = [np.get_include(), get_paths()['include']] + self.cython_include_dirs = '' self.__parameter_values = np.array([]) - self.__problem_class = 'SIM' @property def parameter_values(self): diff --git a/third_party/acados/acados_template/acados_sim_layout.json b/pyextra/acados_template/acados_sim_layout.json similarity index 85% rename from third_party/acados/acados_template/acados_sim_layout.json rename to pyextra/acados_template/acados_sim_layout.json index e3ca4b575b43d2..25b149613b387f 100644 --- a/third_party/acados/acados_template/acados_sim_layout.json +++ b/pyextra/acados_template/acados_sim_layout.json @@ -42,12 +42,6 @@ ], "sim_method_newton_iter": [ "int" - ], - "sim_method_newton_tol": [ - "float" - ], - "ext_fun_compile_flags": [ - "str" ] } } diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py new file mode 100644 index 00000000000000..3588dd38cd26ac --- /dev/null +++ b/pyextra/acados_template/acados_sim_solver.py @@ -0,0 +1,454 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import sys, os, json + +import numpy as np + +from ctypes import * +from copy import deepcopy + +from .generate_c_code_explicit_ode import generate_c_code_explicit_ode +from .generate_c_code_implicit_ode import generate_c_code_implicit_ode +from .generate_c_code_gnsf import generate_c_code_gnsf +from .acados_sim import AcadosSim +from .acados_ocp import AcadosOcp +from .acados_model import acados_model_strip_casadi_symbolics +from .utils import is_column, render_template, format_class_dict, np_array_to_list,\ + make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path +from .builders import CMakeBuilder + + +def make_sim_dims_consistent(acados_sim): + dims = acados_sim.dims + model = acados_sim.model + # nx + if is_column(model.x): + dims.nx = model.x.shape[0] + else: + raise Exception("model.x should be column vector!") + + # nu + if is_column(model.u): + dims.nu = model.u.shape[0] + elif model.u == None or model.u == []: + dims.nu = 0 + else: + raise Exception("model.u should be column vector or None!") + + # nz + if is_column(model.z): + dims.nz = model.z.shape[0] + elif model.z == None or model.z == []: + dims.nz = 0 + else: + raise Exception("model.z should be column vector or None!") + + # np + if is_column(model.p): + dims.np = model.p.shape[0] + elif model.p == None or model.p == []: + dims.np = 0 + else: + raise Exception("model.p should be column vector or None!") + + +def get_sim_layout(): + python_interface_path = get_python_interface_path() + abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json') + with open(abs_path, 'r') as f: + sim_layout = json.load(f) + return sim_layout + + +def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'): + # Load acados_sim structure description + sim_layout = get_sim_layout() + + # Copy input sim object dictionary + sim_dict = dict(deepcopy(acados_sim).__dict__) + + for key, v in sim_layout.items(): + # skip non dict attributes + if not isinstance(v, dict): continue + # Copy sim object attributes dictionaries + sim_dict[key]=dict(getattr(acados_sim, key).__dict__) + + sim_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model']) + sim_json = format_class_dict(sim_dict) + + with open(json_file, 'w') as f: + json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True) + + +def sim_get_default_cmake_builder() -> CMakeBuilder: + """ + If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting. + :return: default :py:class:`~acados_template.builders.CMakeBuilder` + """ + cmake_builder = CMakeBuilder() + cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB'] + return cmake_builder + + +def sim_render_templates(json_file, model_name, code_export_dir, cmake_options: CMakeBuilder = None): + # setting up loader and environment + json_path = os.path.join(os.getcwd(), json_file) + + if not os.path.exists(json_path): + raise Exception(f"{json_path} not found!") + + template_dir = code_export_dir + + ## Render templates + in_file = 'acados_sim_solver.in.c' + out_file = f'acados_sim_solver_{model_name}.c' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'acados_sim_solver.in.h' + out_file = f'acados_sim_solver_{model_name}.h' + render_template(in_file, out_file, template_dir, json_path) + + # Builder + if cmake_options is not None: + in_file = 'CMakeLists.in.txt' + out_file = 'CMakeLists.txt' + render_template(in_file, out_file, template_dir, json_path) + else: + in_file = 'Makefile.in' + out_file = 'Makefile' + render_template(in_file, out_file, template_dir, json_path) + + in_file = 'main_sim.in.c' + out_file = f'main_sim_{model_name}.c' + render_template(in_file, out_file, template_dir, json_path) + + ## folder model + template_dir = os.path.join(code_export_dir, model_name + '_model') + + in_file = 'model.in.h' + out_file = f'{model_name}_model.h' + render_template(in_file, out_file, template_dir, json_path) + + +def sim_generate_casadi_functions(acados_sim): + model = acados_sim.model + model = make_model_consistent(model) + + integrator_type = acados_sim.solver_options.integrator_type + + opts = dict(generate_hess = acados_sim.solver_options.sens_hess, + code_export_directory = acados_sim.code_export_directory) + # generate external functions + if integrator_type == 'ERK': + generate_c_code_explicit_ode(model, opts) + elif integrator_type == 'IRK': + generate_c_code_implicit_ode(model, opts) + elif integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + + +class AcadosSimSolver: + """ + Class to interact with the acados integrator C object. + + :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim` + :param json_file: Default: 'acados_sim.json' + :param build: Default: True + :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` + """ + def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None): + + self.solver_created = False + + if isinstance(acados_sim_, AcadosOcp): + # set up acados_sim_ + acados_sim = AcadosSim() + acados_sim.model = acados_sim_.model + acados_sim.dims.nx = acados_sim_.dims.nx + acados_sim.dims.nu = acados_sim_.dims.nu + acados_sim.dims.nz = acados_sim_.dims.nz + acados_sim.dims.np = acados_sim_.dims.np + acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type + acados_sim.code_export_directory = acados_sim_.code_export_directory + + elif isinstance(acados_sim_, AcadosSim): + acados_sim = acados_sim_ + + acados_sim.__problem_class = 'SIM' + + model_name = acados_sim.model.name + make_sim_dims_consistent(acados_sim) + + # reuse existing json and casadi functions, when creating integrator from ocp + if isinstance(acados_sim_, AcadosSim): + if acados_sim.solver_options.integrator_type == 'GNSF': + set_up_imported_gnsf_model(acados_sim) + + sim_generate_casadi_functions(acados_sim) + sim_formulation_json_dump(acados_sim, json_file) + + code_export_dir = acados_sim.code_export_directory + if build: + # render templates + sim_render_templates(json_file, model_name, code_export_dir, cmake_builder) + + # Compile solver + cwd = os.getcwd() + code_export_dir = os.path.abspath(code_export_dir) + os.chdir(code_export_dir) + if cmake_builder is not None: + cmake_builder.exec(code_export_dir) + else: + os.system('make sim_shared_lib') + os.chdir(cwd) + + self.sim_struct = acados_sim + model_name = self.sim_struct.model.name + self.model_name = model_name + + # Load acados library to avoid unloading the library. + # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed. + # Unloading a library which uses OpenMP results in a segfault (on any platform?). + # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp] + # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html] + libacados_name = 'libacados.so' + libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name) + self.__acados_lib = CDLL(libacados_filepath) + # find out if acados was compiled with OpenMP + try: + self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None + except AttributeError as e: + self.__acados_lib_uses_omp = False + if self.__acados_lib_uses_omp: + print('acados was compiled with OpenMP.') + else: + print('acados was compiled without OpenMP.') + + # Ctypes + lib_prefix = 'lib' + lib_ext = '.so' + if os.name == 'nt': + lib_prefix = '' + lib_ext = '' + self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}') + print(f'self.shared_lib_name = "{self.shared_lib_name}"') + + self.shared_lib = CDLL(self.shared_lib_name) + + + # create capsule + getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int + assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0 + self.solver_created = True + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p + self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p + self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p + self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p + self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p + self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p + self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule) + + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + nz = self.sim_struct.dims.nz + self.gettable = { + 'x': nx, + 'xn': nx, + 'u': nu, + 'z': nz, + 'S_forw': nx*(nx+nu), + 'Sx': nx*nx, + 'Su': nx*nu, + 'S_adj': nx+nu, + 'S_hess': (nx+nu)*(nx+nu), + 'S_algebraic': (nz)*(nx+nu), + } + + self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw + + + def solve(self): + """ + Solve the simulation problem with current input. + """ + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int + + status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule) + return status + + + def get(self, field_): + """ + Get the last solution of the solver. + + :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic'] + """ + field = field_ + field = field.encode('utf-8') + + if field_ in self.gettable.keys(): + + # allocate array + dims = self.gettable[field_] + out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data) + + if field_ == 'S_forw': + nu = self.sim_struct.dims.nu + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx+nu, order='F') + elif field_ == 'Sx': + nx = self.sim_struct.dims.nx + out = out.reshape(nx, nx, order='F') + elif field_ == 'Su': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx, nu, order='F') + elif field_ == 'S_hess': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + out = out.reshape(nx+nu, nx+nu, order='F') + elif field_ == 'S_algebraic': + nx = self.sim_struct.dims.nx + nu = self.sim_struct.dims.nu + nz = self.sim_struct.dims.nz + out = out.reshape(nz, nx+nu, order='F') + else: + raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ + f' available fields are {", ".join(self.gettable.keys())}') + + return out + + + def set(self, field_, value_): + """ + Set numerical data inside the solver. + + :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z'] + :param value: the value with appropriate size. + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + + value_ = value_.astype(float) + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + field = field_ + field = field.encode('utf-8') + + # treat parameters separately + if field_ == 'p': + model_name = self.sim_struct.model.name + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int] + value_data = cast(value_.ctypes.data, POINTER(c_double)) + getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0]) + return + else: + # dimension check + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] + self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) + + value_ = np.ravel(value_, order='F') + + value_shape = value_.shape + if len(value_shape) == 1: + value_shape = (value_shape[0], 0) + + if value_shape != tuple(dims): + raise Exception('AcadosSimSolver.set(): mismatching dimension' \ + ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + + # set + if field_ in ['xdot', 'z']: + self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p) + elif field_ in self.settable: + self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] + self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p) + else: + raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \ + f' available fields are {", ".join(self.settable)}') + + return + + + def __del__(self): + + if self.solver_created: + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule) + + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule) + + try: + self.dlclose(self.shared_lib._handle) + except: + pass diff --git a/third_party/acados/acados_template/acados_solver_common.pxd b/pyextra/acados_template/acados_solver_common.pxd similarity index 90% rename from third_party/acados/acados_template/acados_solver_common.pxd rename to pyextra/acados_template/acados_solver_common.pxd index 75d021626f32b0..fedd7190d97314 100644 --- a/third_party/acados/acados_template/acados_solver_common.pxd +++ b/pyextra/acados_template/acados_solver_common.pxd @@ -1,5 +1,9 @@ +# -*- coding: future_fstrings -*- # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -83,7 +87,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h": int stage, const char *field, int *dims_out) void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out) - void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out) # opts diff --git a/third_party/acados/acados_template/builders.py b/pyextra/acados_template/builders.py similarity index 88% rename from third_party/acados/acados_template/builders.py rename to pyextra/acados_template/builders.py index 8acc05b5287b19..f595033ceb1697 100644 --- a/third_party/acados/acados_template/builders.py +++ b/pyextra/acados_template/builders.py @@ -1,3 +1,4 @@ +# -*- coding: future_fstrings -*- # # Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, # Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, @@ -33,7 +34,7 @@ import os import sys -from subprocess import DEVNULL, call, STDOUT +from subprocess import call class CMakeBuilder: @@ -77,7 +78,7 @@ def get_cmd2_build(self): def get_cmd3_install(self): return f'cmake --install "{self._build_dir}"' - def exec(self, code_export_directory, verbose=True): + def exec(self, code_export_directory): """ Execute the compilation using `CMake` with the given settings. :param code_export_directory: must be the absolute path to the directory where the code was exported to @@ -95,32 +96,17 @@ def exec(self, code_export_directory, verbose=True): os.chdir(self._build_dir) cmd_str = self.get_cmd1_cmake() print(f'call("{cmd_str})"') - retcode = call( - cmd_str, - shell=True, - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) + retcode = call(cmd_str, shell=True) if retcode != 0: raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}') cmd_str = self.get_cmd2_build() print(f'call("{cmd_str}")') - retcode = call( - cmd_str, - shell=True, - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) + retcode = call(cmd_str, shell=True) if retcode != 0: raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}') cmd_str = self.get_cmd3_install() print(f'call("{cmd_str}")') - retcode = call( - cmd_str, - shell=True, - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) + retcode = call(cmd_str, shell=True) if retcode != 0: raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}') except OSError as e: diff --git a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt b/pyextra/acados_template/c_templates_tera/CMakeLists.in.txt similarity index 88% rename from third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt rename to pyextra/acados_template/c_templates_tera/CMakeLists.in.txt index 99bc26f750c0b7..3d6483b5d230ad 100644 --- a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/pyextra/acados_template/c_templates_tera/CMakeLists.in.txt @@ -1,5 +1,8 @@ # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -118,14 +121,12 @@ {%- set openmp_flag = " " %} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} {%- set link_libs = "-lqpOASES_e" %} - {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} - {%- set link_libs = "-ldaqp" %} {%- else %} {%- set link_libs = "" %} {%- endif %} {%- endif %} -cmake_minimum_required(VERSION 3.13) +cmake_minimum_required(VERSION 3.10) project({{ model.name }}) @@ -138,14 +139,6 @@ option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name } option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF) {%- endif %} - -if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows") - # MinGW, change to .lib such that mex recognizes it - set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib") - set(CMAKE_SHARED_LIBRARY_PREFIX "") -endif() - - # object target names set(MODEL_OBJ model_{{ model.name }}) set(OCP_OBJ ocp_{{ model.name }}) @@ -153,11 +146,9 @@ set(SIM_OBJ sim_{{ model.name }}) # model set(MODEL_SRC - {%- if model.dyn_ext_fun_type == "casadi" %} {%- if solver_options.integrator_type == "ERK" %} {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c - {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c {%- if hessian_approx == "EXACT" %} {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c {%- endif %} @@ -185,15 +176,16 @@ set(MODEL_SRC {%- endif %} {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c {%- elif solver_options.integrator_type == "DISCRETE" %} + {%- if model.dyn_ext_fun_type == "casadi" %} {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c {%- if hessian_approx == "EXACT" %} {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c {%- endif %} -{%- endif -%} {%- else %} - {{ model.name }}_model/{{ model.dyn_generic_source }} + {{ model.name }}_model/{{ model.dyn_source_discrete }} {%- endif %} +{%- endif -%} ) add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) @@ -227,9 +219,6 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c -{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c - {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c {%- elif cost_type_0 == "EXTERNAL" %} {%- if cost.cost_ext_fun_type_0 == "casadi" %} {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c @@ -243,9 +232,6 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c -{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %} - {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c - {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c {%- elif cost_type == "EXTERNAL" %} {%- if cost.cost_ext_fun_type == "casadi" %} {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c @@ -259,9 +245,6 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c -{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %} - {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c - {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c {%- elif cost_type_e == "EXTERNAL" %} {%- if cost.cost_ext_fun_type_e == "casadi" %} {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c @@ -292,7 +275,7 @@ set(EX_SRC main_{{ model.name }}.c) set(EX_EXE main_{{ model.name }}) {%- if model_external_shared_lib_dir and model_external_shared_lib_name %} -set(EXTERNAL_DIR {{ model_external_shared_lib_dir | replace(from="\", to="/") }}) +set(EXTERNAL_DIR {{ model_external_shared_lib_dir }}) set(EXTERNAL_LIB {{ model_external_shared_lib_name }}) {%- else %} set(EXTERNAL_DIR) @@ -300,26 +283,23 @@ set(EXTERNAL_LIB) {%- endif %} # set some search paths for preprocessor and linker -set(ACADOS_INCLUDE_PATH {{ acados_include_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the include directory for acados.") -set(ACADOS_LIB_PATH {{ acados_lib_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the lib directory for acados.") +set(ACADOS_INCLUDE_PATH {{ acados_include_path }} CACHE PATH "Define the path which contains the include directory for acados.") +set(ACADOS_LIB_PATH {{ acados_lib_path }} CACHE PATH "Define the path which contains the lib directory for acados.") # c-compiler flags for debugging set(CMAKE_C_FLAGS_DEBUG "-O0 -ggdb") -set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} +set(CMAKE_C_FLAGS " {%- if qp_solver == "FULL_CONDENSING_QPOASES" -%} - -DACADOS_WITH_QPOASES -{%- endif -%} -{%- if qp_solver == "FULL_CONDENSING_DAQP" -%} - -DACADOS_WITH_DAQP + -DACADOS_WITH_QPOASES {%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%} - -DACADOS_WITH_OSQP + -DACADOS_WITH_OSQP {%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%} - -DACADOS_WITH_QPDUNES + -DACADOS_WITH_QPDUNES {%- endif -%} -") + -fPIC -std=c99 {{ openmp_flag }}") #-fno-diagnostics-show-line-numbers -g include_directories( @@ -330,9 +310,6 @@ include_directories( {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} ${ACADOS_INCLUDE_PATH}/qpOASES_e/ {%- endif %} -{%- if qp_solver == "FULL_CONDENSING_DAQP" %} - ${ACADOS_INCLUDE_PATH}/daqp/include -{%- endif %} ) # linker flags diff --git a/pyextra/acados_template/c_templates_tera/CPPLINT.cfg b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg new file mode 100644 index 00000000000000..bbd1caf0571825 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg @@ -0,0 +1 @@ +exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? diff --git a/third_party/acados/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in similarity index 78% rename from third_party/acados/acados_template/c_templates_tera/Makefile.in rename to pyextra/acados_template/c_templates_tera/Makefile.in index fbefc08e380d9c..d45be0a9c7a932 100644 --- a/third_party/acados/acados_template/c_templates_tera/Makefile.in +++ b/pyextra/acados_template/c_templates_tera/Makefile.in @@ -1,5 +1,8 @@ # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -117,8 +120,6 @@ {%- set openmp_flag = " " %} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} {%- set link_libs = "-lqpOASES_e" %} - {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} - {%- set link_libs = "-ldaqp" %} {%- else %} {%- set link_libs = "" %} {%- endif %} @@ -128,11 +129,9 @@ # model MODEL_SRC= - {%- if model.dyn_ext_fun_type == "casadi" %} -{%- if solver_options.integrator_type == "ERK" %} +{%- if solver_options.integrator_type == "ERK" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c -MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c {%- if hessian_approx == "EXACT" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c {%- endif %} @@ -140,9 +139,9 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c - {%- if hessian_approx == "EXACT" %} + {%- if hessian_approx == "EXACT" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c - {%- endif %} + {%- endif %} {%- elif solver_options.integrator_type == "LIFTED_IRK" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c @@ -160,15 +159,16 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c {%- endif %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c {%- elif solver_options.integrator_type == "DISCRETE" %} + {%- if model.dyn_ext_fun_type == "casadi" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c {%- if hessian_approx == "EXACT" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c {%- endif %} -{%- endif %} {%- else %} -MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }} +MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }} {%- endif %} +{%- endif %} MODEL_OBJ := $(MODEL_SRC:.c=.o) # optimal control problem - mostly CasADi exports @@ -200,9 +200,6 @@ OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_z OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c -{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %} -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c {%- elif cost_type_0 == "EXTERNAL" %} {%- if cost.cost_ext_fun_type_0 == "casadi" %} OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c @@ -216,9 +213,6 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }} OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c -{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %} -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c {%- elif cost_type == "EXTERNAL" %} {%- if cost.cost_ext_fun_type == "casadi" %} OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c @@ -232,9 +226,6 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }} OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c -{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %} -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c -OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c {%- elif cost_type_e == "EXTERNAL" %} {%- if cost.cost_ext_fun_type_e == "casadi" %} OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c @@ -244,12 +235,6 @@ OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }} {%- endif %} {%- endif %} -{%- if solver_options.custom_update_filename %} - {%- if solver_options.custom_update_filename != "" %} -OCP_SRC+= {{ solver_options.custom_update_filename }} - {%- endif %} -{%- endif %} - OCP_SRC+= acados_solver_{{ model.name }}.c OCP_OBJ := $(OCP_SRC:.c=.o) @@ -290,9 +275,6 @@ LIB_PATH = {{ acados_lib_path }} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} CPPFLAGS += -DACADOS_WITH_QPOASES {%- endif %} -{%- if qp_solver == "FULL_CONDENSING_DAQP" %} -CPPFLAGS += -DACADOS_WITH_DAQP -{%- endif %} {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %} CPPFLAGS += -DACADOS_WITH_OSQP {%- endif %} @@ -306,13 +288,10 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/ {%- endif %} - {%- if qp_solver == "FULL_CONDENSING_DAQP" %} -CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include - {%- endif %} {# c-compiler flags #} # define the c-compiler flags for make's implicit rules -CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g +CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g # # Debugging # CFLAGS += -g3 @@ -327,9 +306,9 @@ LDLIBS+= -lm LDLIBS+= {{ link_libs }} # libraries -LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }} -LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} -LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }}) +LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so +LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so +LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) # virtual targets .PHONY : all clean @@ -375,73 +354,38 @@ ocp_cython_o: ocp_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ -fPIC \ -o acados_ocp_solver_pyx.o \ + -I /usr/include/python3.8 \ -I $(INCLUDE_PATH)/blasfeo/include/ \ -I $(INCLUDE_PATH)/hpipm/include/ \ -I $(INCLUDE_PATH) \ - {%- for path in cython_include_dirs %} - -I {{ path }} \ - {%- endfor %} + -I {{ cython_include_dirs }} \ acados_ocp_solver_pyx.c \ ocp_cython: ocp_cython_o $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_ocp_solver_pyx{{ shared_lib_ext }} \ + -o acados_ocp_solver_pyx.so \ -Wl,-rpath=$(LIB_PATH) \ acados_ocp_solver_pyx.o \ - $(abspath .)/libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} \ - $(LDFLAGS) $(LDLIBS) - -# Sim Cython targets -sim_cython_c: sim_shared_lib - cython \ - -o acados_sim_solver_pyx.c \ - -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ - $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ - -I {{ code_export_directory }} \ - -sim_cython_o: sim_cython_c - $(CC) $(ACADOS_FLAGS) -c -O2 \ - -fPIC \ - -o acados_sim_solver_pyx.o \ - -I $(INCLUDE_PATH)/blasfeo/include/ \ - -I $(INCLUDE_PATH)/hpipm/include/ \ - -I $(INCLUDE_PATH) \ - {%- for path in cython_include_dirs %} - -I {{ path }} \ - {%- endfor %} - acados_sim_solver_pyx.c \ - -sim_cython: sim_cython_o - $(CC) $(ACADOS_FLAGS) -shared \ - -o acados_sim_solver_pyx{{ shared_lib_ext }} \ - -Wl,-rpath=$(LIB_PATH) \ - acados_sim_solver_pyx.o \ - $(abspath .)/libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} \ + $(abspath .)/libacados_ocp_solver_{{ model.name }}.so \ $(LDFLAGS) $(LDLIBS) {%- if os and os == "pc" %} clean: del \Q *.o 2>nul - del \Q *{{ shared_lib_ext }} 2>nul + del \Q *.so 2>nul del \Q main_{{ model.name }} 2>nul clean_ocp_shared_lib: - del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul + del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul del \Q acados_solver_{{ model.name }}.o 2>nul clean_ocp_cython: - del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul + del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul del \Q acados_solver_{{ model.name }}.o 2>nul - del \Q acados_ocp_solver_pyx{{ shared_lib_ext }} 2>nul + del \Q acados_ocp_solver_pyx.so 2>nul del \Q acados_ocp_solver_pyx.o 2>nul -clean_sim_cython: - del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul - del \Q acados_sim_solver_{{ model.name }}.o 2>nul - del \Q acados_sim_solver_pyx{{ shared_lib_ext }} 2>nul - del \Q acados_sim_solver_pyx.o 2>nul - {%- else %} clean: @@ -454,15 +398,9 @@ clean_ocp_shared_lib: $(RM) $(OCP_OBJ) clean_ocp_cython: - $(RM) libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} + $(RM) libacados_ocp_solver_{{ model.name }}.so $(RM) acados_solver_{{ model.name }}.o - $(RM) acados_ocp_solver_pyx{{ shared_lib_ext }} + $(RM) acados_ocp_solver_pyx.so $(RM) acados_ocp_solver_pyx.o -clean_sim_cython: - $(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} - $(RM) acados_sim_solver_{{ model.name }}.o - $(RM) acados_sim_solver_pyx{{ shared_lib_ext }} - $(RM) acados_sim_solver_pyx.o - {%- endif %} diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c similarity index 98% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c rename to pyextra/acados_template/c_templates_tera/acados_mex_create.in.c index 24ae94ac2cc96d..e67a51567a8bb6 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c similarity index 88% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c rename to pyextra/acados_template/c_templates_tera/acados_mex_free.in.c index bd457969b27408..560adb0b986955 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c similarity index 76% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c rename to pyextra/acados_template/c_templates_tera/acados_mex_set.in.c index 78a308df495b36..f8e1e5e445a24c 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -56,6 +59,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) /* RHS */ int min_nrhs = 6; + char *ext_fun_type = mxArrayToString( prhs[0] ); + char *ext_fun_type_e = mxArrayToString( prhs[1] ); + // C ocp const mxArray *C_ocp = prhs[2]; // capsule @@ -372,63 +378,45 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // } // } // initializations - else if (!strcmp(field, "init_x") || !strcmp(field, "x")) + else if (!strcmp(field, "init_x")) { - if (nrhs == min_nrhs) - { - acados_size = (N+1) * nx; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; ii<=N; ii++) - { - ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx); - } - } - else // (nrhs == min_nrhs + 1) + if (nrhs!=min_nrhs) + MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) + + acados_size = (N+1) * nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (int ii=0; ii<=N; ii++) { - acados_size = nx; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "x", value); + ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx); } } - else if (!strcmp(field, "init_u") || !strcmp(field, "u")) + else if (!strcmp(field, "init_u")) { - if (nrhs==min_nrhs) - { - acados_size = N*nu; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_solver_plan[0]; sim_solver_t type = sim_plan.sim_solver; if (type == IRK) { int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z"); - if (nrhs==min_nrhs) - { - acados_size = N*nz; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_solver_plan[0]; sim_solver_t type = sim_plan.sim_solver; if (type == IRK) { int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x"); - if (nrhs==min_nrhs) - { - acados_size = N*nx; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_solver_plan[0]; sim_solver_t type = sim_plan.sim_solver; @@ -472,95 +454,51 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi"); - if (nrhs==min_nrhs) - { - acados_size = N*nout; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); + capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - {%- if model.dyn_ext_fun_type == "casadi" %} // external functions (implicit model) - capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun; + capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun; capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work; capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in; capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out; capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in; capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun, np); + external_function_param_casadi_create(capsule->sim_impl_dae_fun, np); capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z; capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work; @@ -105,39 +107,33 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out; capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in; capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np); + external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np); - // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z; + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in; capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np); - {%- else %} - capsule->sim_impl_dae_fun->fun = &{{ model.dyn_impl_dae_fun }}; - capsule->sim_impl_dae_fun_jac_x_xdot_z->fun = &{{ model.dyn_impl_dae_fun_jac }}; - capsule->sim_impl_dae_jac_x_xdot_u_z->fun = &{{ model.dyn_impl_dae_jac }}; - {%- endif %} + external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np); {%- if hessian_approx == "EXACT" %} - capsule->sim_impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess; capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work; capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in; capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out; capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in; capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_hess, np); + external_function_param_casadi_create(capsule->sim_impl_dae_hess, np); {%- endif %} {% elif solver_options.integrator_type == "ERK" %} // explicit ode - capsule->sim_forw_vde_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_vde_adj_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_expl_ode_fun_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); + capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw; capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in; @@ -145,15 +141,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in; capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out; capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_forw_vde_casadi, np); - - capsule->sim_vde_adj_casadi->casadi_fun = &{{ model.name }}_expl_vde_adj; - capsule->sim_vde_adj_casadi->casadi_n_in = &{{ model.name }}_expl_vde_adj_n_in; - capsule->sim_vde_adj_casadi->casadi_n_out = &{{ model.name }}_expl_vde_adj_n_out; - capsule->sim_vde_adj_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_adj_sparsity_in; - capsule->sim_vde_adj_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_adj_sparsity_out; - capsule->sim_vde_adj_casadi->casadi_work = &{{ model.name }}_expl_vde_adj_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_vde_adj_casadi, np); + external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np); capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun; capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in; @@ -161,30 +149,30 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in; capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out; capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_fun_casadi, np); + external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np); {%- if hessian_approx == "EXACT" %} - capsule->sim_expl_ode_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z; + capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + // external_function_param_casadi impl_dae_jac_x_xdot_u_z; capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess; capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work; capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in; capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out; capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in; capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_hess, np); + external_function_param_casadi_create(capsule->sim_expl_ode_hess, np); {%- endif %} {% elif solver_options.integrator_type == "GNSF" -%} {% if model.gnsf.purely_linear != 1 %} - capsule->sim_gnsf_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); - capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); + capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); {% if model.gnsf.nontrivial_f_LO == 1 %} - capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); + capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); {%- endif %} {%- endif %} - capsule->sim_gnsf_get_matrices_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})); + capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); {% if model.gnsf.purely_linear != 1 %} capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun; @@ -193,7 +181,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in; capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out; capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun, np); + external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np); capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y; capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in; @@ -201,7 +189,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in; capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out; capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun_jac_y, np); + external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np); capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat; capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in; @@ -209,7 +197,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in; capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out; capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_jac_y_uhat, np); + external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np); {% if model.gnsf.nontrivial_f_LO == 1 %} capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz; @@ -218,7 +206,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in; capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out; capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np); + external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np); {%- endif %} {%- endif %} @@ -228,7 +216,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in; capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out; capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work; - external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_get_matrices_fun, np); + external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np); {% endif %} // sim plan & config @@ -264,8 +252,6 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) capsule->acados_sim_opts = {{ model.name }}_sim_opts; int tmp_int = {{ solver_options.sim_method_newton_iter }}; sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int); - double tmp_double = {{ solver_options.sim_method_newton_tol }}; - sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_tol", &tmp_double); sim_collocation_type collocation_type = {{ solver_options.collocation_type }}; sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type); @@ -321,9 +307,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) {%- elif solver_options.integrator_type == "ERK" %} {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, - "expl_vde_forw", capsule->sim_forw_vde_casadi); - {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, - "expl_vde_adj", capsule->sim_vde_adj_casadi); + "expl_vde_for", capsule->sim_forw_vde_casadi); {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); {%- if hessian_approx == "EXACT" %} @@ -424,29 +408,28 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule) // free external function {%- if solver_options.integrator_type == "IRK" %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_jac_x_xdot_u_z); + external_function_param_casadi_free(capsule->sim_impl_dae_fun); + external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); {%- if hessian_approx == "EXACT" %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_hess); + external_function_param_casadi_free(capsule->sim_impl_dae_hess); {%- endif %} {%- elif solver_options.integrator_type == "ERK" %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_forw_vde_casadi); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_vde_adj_casadi); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_fun_casadi); + external_function_param_casadi_free(capsule->sim_forw_vde_casadi); + external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); {%- if hessian_approx == "EXACT" %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_hess); + external_function_param_casadi_free(capsule->sim_expl_ode_hess); {%- endif %} {%- elif solver_options.integrator_type == "GNSF" %} {% if model.gnsf.purely_linear != 1 %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun_jac_y); - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_jac_y_uhat); + external_function_param_casadi_free(capsule->sim_gnsf_phi_fun); + external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y); + external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat); {% if model.gnsf.nontrivial_f_LO == 1 %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); + external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); {%- endif %} {%- endif %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_get_matrices_fun); + external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun); {% endif %} return 0; @@ -466,7 +449,6 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl {%- if solver_options.integrator_type == "ERK" %} capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p); - capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p); capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); {%- if hessian_approx == "EXACT" %} capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p); diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h similarity index 75% rename from third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h rename to pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h index 59aee62f49fd3f..7306491bafc0a2 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -57,23 +60,22 @@ typedef struct sim_solver_capsule /* external functions */ // ERK - external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_fun_casadi; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_hess; + external_function_param_casadi * sim_forw_vde_casadi; + external_function_param_casadi * sim_expl_ode_fun_casadi; + external_function_param_casadi * sim_expl_ode_hess; // IRK - external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_hess; + external_function_param_casadi * sim_impl_dae_fun; + external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; + external_function_param_casadi * sim_impl_dae_hess; // GNSF - external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_get_matrices_fun; + external_function_param_casadi * sim_gnsf_phi_fun; + external_function_param_casadi * sim_gnsf_phi_fun_jac_y; + external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; + external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi * sim_gnsf_get_matrices_fun; } sim_solver_capsule; diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c similarity index 95% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c rename to pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c index bd73ff69a45e99..68a6a3f80f54fa 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c similarity index 84% rename from third_party/acados/acados_template/c_templates_tera/acados_solver.in.c rename to pyextra/acados_template/c_templates_tera/acados_solver.in.c index 5e36a53d1001d3..0af81277093eda 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -39,27 +42,34 @@ // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" -#include "{{ model.name }}_constraints/{{ model.name }}_constraints.h" - -{%- if cost.cost_type != "LINEAR_LS" or cost.cost_type_e != "LINEAR_LS" or cost.cost_type_0 != "LINEAR_LS" %} -#include "{{ model.name }}_cost/{{ model.name }}_cost.h" -{%- endif %} - -{%- if not solver_options.custom_update_filename %} - {%- set custom_update_filename = "" %} -{% else %} - {%- set custom_update_filename = solver_options.custom_update_filename %} +{% if constraints.constr_type == "BGP" and dims.nphi %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h" +{% endif %} +{% if constraints.constr_type == "BGH" and dims.nh > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h" +{% endif %} +{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} +#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h" +{% endif %} +{%- if cost.cost_type == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h" +{%- elif cost.cost_type == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h" {%- endif %} -{%- if not solver_options.custom_update_header_filename %} - {%- set custom_update_header_filename = "" %} -{% else %} - {%- set custom_update_header_filename = solver_options.custom_update_header_filename %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h" +{%- elif cost.cost_type_0 == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h" {%- endif %} -{%- if custom_update_header_filename != "" %} -#include "{{ custom_update_header_filename }}" +{%- if cost.cost_type_e == "NONLINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h" +{%- elif cost.cost_type_e == "EXTERNAL" %} +#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h" {%- endif %} - #include "acados_solver_{{ model.name }}.h" #define NX {{ model.name | upper }}_NX @@ -195,6 +205,7 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, nlp_solver_plan->nlp_constraints[N] = BGH; {%- endif %} +{%- if solver_options.hessian_approx == "EXACT" %} {%- if solver_options.regularize_method == "NO_REGULARIZE" %} nlp_solver_plan->regularization = NO_REGULARIZE; {%- elif solver_options.regularize_method == "MIRROR" %} @@ -206,6 +217,7 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, {%- elif solver_options.regularize_method == "CONVEXIFY" %} nlp_solver_plan->regularization = CONVEXIFY; {%- endif %} +{%- endif %} } @@ -312,11 +324,11 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); } -{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR"%} +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); {%- endif %} -{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR"%} +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} for (int i = 1; i < N; i++) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} @@ -341,7 +353,7 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]); {%- endif %} -{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR"%} +{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} free(intNp1mem); @@ -376,6 +388,7 @@ return nlp_dims; void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_solver_capsule* capsule) { const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; /************************************************ * external functions @@ -455,50 +468,35 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ {% elif solver_options.integrator_type == "IRK" %} // implicit dae - capsule->impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); - {%- else %} - capsule->impl_dae_fun[i].fun = &{{ model.dyn_impl_dae_fun }}; - external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun[i], {{ dims.np }}); - {%- endif %} } - capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); - {%- else %} - capsule->impl_dae_fun_jac_x_xdot_z[i].fun = &{{ model.dyn_impl_dae_fun_jac }}; - external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }}); - {%- endif %} } - capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); - {%- else %} - capsule->impl_dae_jac_x_xdot_u_z[i].fun = &{{ model.dyn_impl_dae_jac }}; - external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }}); - {%- endif %} } {%- if solver_options.hessian_approx == "EXACT" %} - capsule->impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); } {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} // external functions (implicit model) - capsule->impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); } - capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); + capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); } @@ -576,11 +574,6 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); -{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - // convex-over-nonlinear cost - MAP_CASADI_FNC(conl_cost_0_fun, {{ model.name }}_conl_cost_0_fun); - MAP_CASADI_FNC(conl_cost_0_fun_jac_hess, {{ model.name }}_conl_cost_0_fun_jac_hess); - {%- elif cost.cost_type_0 == "EXTERNAL" %} // external cost {%- if cost.cost_ext_fun_type_0 == "casadi" %} @@ -626,20 +619,6 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ { MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); } - -{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - // convex-over-nonlinear cost - capsule->conl_cost_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(conl_cost_fun[i], {{ model.name }}_conl_cost_fun); - } - capsule->conl_cost_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(conl_cost_fun_jac_hess[i], {{ model.name }}_conl_cost_fun_jac_hess); - } - {%- elif cost.cost_type == "EXTERNAL" %} // external cost capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); @@ -681,12 +660,6 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ MAP_CASADI_FNC(cost_y_e_fun, {{ model.name }}_cost_y_e_fun); MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, {{ model.name }}_cost_y_e_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_e_hess, {{ model.name }}_cost_y_e_hess); - -{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - // convex-over-nonlinear cost - MAP_CASADI_FNC(conl_cost_e_fun, {{ model.name }}_conl_cost_e_fun); - MAP_CASADI_FNC(conl_cost_e_fun_jac_hess, {{ model.name }}_conl_cost_e_fun_jac_hess); - {%- elif cost.cost_type_e == "EXTERNAL" %} // external cost - function {%- if cost.cost_ext_fun_type_e == "casadi" %} @@ -835,55 +808,9 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } /**** Cost ****/ - -{%- if dims.ny_0 == 0 %} -{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - double* yref_0 = calloc(NY0, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_0) %} - {%- if cost.yref_0[j] != 0 %} - yref_0[{{ j }}] = {{ cost.yref_0[j] }}; - {%- endif %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); - free(yref_0); -{%- endif %} - - -{%- if dims.ny == 0 %} -{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - double* yref = calloc(NY, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny) %} - {%- if cost.yref[j] != 0 %} - yref[{{ j }}] = {{ cost.yref[j] }}; - {%- endif %} - {%- endfor %} - - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); - } - free(yref); -{%- endif %} - - -{%- if dims.ny_e == 0 %} -{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - double* yref_e = calloc(NYN, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_e) %} - {%- if cost.yref_e[j] != 0 %} - yref_e[{{ j }}] = {{ cost.yref_e[j] }}; - {%- endif %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); - free(yref_e); -{%- endif %} - -{%- if dims.ny_0 == 0 %} -{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} - double* W_0 = calloc(NY0*NY0, sizeof(double)); +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} + {%- if dims.ny_0 > 0 %} + double* W_0 = calloc(NY0*NY0, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.ny_0) %} @@ -894,10 +821,21 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); free(W_0); + + double* yref_0 = calloc(NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- if cost.yref_0[j] != 0 %} + yref_0[{{ j }}] = {{ cost.yref_0[j] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); + free(yref_0); + {%- endif %} {%- endif %} -{%- if dims.ny == 0 %} -{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} + {%- if dims.ny > 0 %} double* W = calloc(NY*NY, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} @@ -908,30 +846,25 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endfor %} {%- endfor %} + double* yref = calloc(NY, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny) %} + {%- if cost.yref[j] != 0 %} + yref[{{ j }}] = {{ cost.yref[j] }}; + {%- endif %} + {%- endfor %} + for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); } free(W); + free(yref); + {%- endif %} {%- endif %} -{%- if dims.ny_e == 0 %} -{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} - double* W_e = calloc(NYN*NYN, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_e) %} - {%- for k in range(end=dims.ny_e) %} - {%- if cost.W_e[j][k] != 0 %} - W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); - free(W_e); -{%- endif %} - -{%- if dims.ny_0 == 0 %} -{%- elif cost.cost_type_0 == "LINEAR_LS" %} +{%- if cost.cost_type_0 == "LINEAR_LS" %} double* Vx_0 = calloc(NY0*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} @@ -944,7 +877,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); free(Vx_0); - {%- if dims.nu > 0 %} + {%- if dims.ny_0 > 0 and dims.nu > 0 %} double* Vu_0 = calloc(NY0*NU, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} @@ -958,7 +891,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(Vu_0); {%- endif %} - {%- if dims.nz > 0 %} + {%- if dims.ny_0 > 0 and dims.nz > 0 %} double* Vz_0 = calloc(NY0*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} @@ -971,10 +904,10 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); free(Vz_0); {%- endif %} -{%- endif %} +{%- endif %}{# LINEAR LS #} -{%- if dims.ny == 0 %} -{%- elif cost.cost_type == "LINEAR_LS" %} + +{%- if cost.cost_type == "LINEAR_LS" %} double* Vx = calloc(NY*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} @@ -990,7 +923,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } free(Vx); - {% if dims.nu > 0 %} + {% if dims.ny > 0 and dims.nu > 0 %} double* Vu = calloc(NY*NU, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} @@ -1008,7 +941,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(Vu); {%- endif %} - {%- if dims.nz > 0 %} + {%- if dims.ny > 0 and dims.nz > 0 %} double* Vz = calloc(NY*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} @@ -1027,28 +960,11 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {%- endif %}{# LINEAR LS #} -{%- if dims.ny_e == 0 %} -{%- elif cost.cost_type_e == "LINEAR_LS" %} - double* Vx_e = calloc(NYN*NX, sizeof(double)); - // change only the non-zero elements: - {% for j in range(end=dims.ny_e) %} - {%- for k in range(end=dims.nx) %} - {%- if cost.Vx_e[j][k] != 0 %} - Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); - free(Vx_e); -{%- endif %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess); -{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun", &capsule->conl_cost_0_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun_jac_hess", &capsule->conl_cost_0_fun_jac_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac); @@ -1062,12 +978,6 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]); } -{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - for (int i = 1; i < N; i++) - { - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun", &capsule->conl_cost_fun[i-1]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun_jac_hess", &capsule->conl_cost_fun_jac_hess[i-1]); - } {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 1; i < N; i++) { @@ -1077,25 +987,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } {%- endif %} -{%- if cost.cost_type_e == "NONLINEAR_LS" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); - -{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun", &capsule->conl_cost_e_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun_jac_hess", &capsule->conl_cost_e_fun_jac_hess); - -{%- elif cost.cost_type_e == "EXTERNAL" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); -{%- endif %} - - {%- if dims.ns > 0 %} - // slacks double* zlumem = calloc(4*NS, sizeof(double)); double* Zl = zlumem+NS*0; double* Zu = zlumem+NS*1; @@ -1136,8 +1028,59 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(zlumem); {%- endif %} + // terminal cost +{%- if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} + {%- if dims.ny_e > 0 %} + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- if cost.yref_e[j] != 0 %} + yref_e[{{ j }}] = {{ cost.yref_e[j] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); + + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.ny_e) %} + {%- if cost.W_e[j][k] != 0 %} + W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); + + {%- if cost.cost_type_e == "LINEAR_LS" %} + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_e[j][k] != 0 %} + Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); + {%- endif %} + + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); + {%- endif %} + {%- endif %}{# ny_e > 0 #} + +{%- elif cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + {% if dims.ns_e > 0 %} - // slacks terminal double* zluemem = calloc(4*NSN, sizeof(double)); double* Zl_e = zluemem+NSN*0; double* Zu_e = zluemem+NSN*1; @@ -1242,7 +1185,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endfor %} for (int i = 1; i < N; i++) - { + { ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); @@ -1420,7 +1363,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {% if dims.ng > 0 %} - // set up general constraints for stage 0 to N-1 + // set up general constraints for stage 0 to N-1 double* D = calloc(NG*NU, sizeof(double)); double* C = calloc(NG*NX, sizeof(double)); double* lug = calloc(2*NG, sizeof(double)); @@ -1484,7 +1427,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule uh[{{ i }}] = {{ constraints.uh[i] }}; {%- endif %} {%- endfor %} - + for (int i = 0; i < N; i++) { // nonlinear constraints for stages 0 to N-1 @@ -1656,16 +1599,16 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {% endif %} {% if dims.ng_e > 0 %} - // set up general constraints for last stage + // set up general constraints for last stage double* C_e = calloc(NGN*NX, sizeof(double)); double* lug_e = calloc(2*NGN, sizeof(double)); double* lg_e = lug_e; double* ug_e = lug_e + NGN; - {% for j in range(end=dims.ng_e) %} + {% for j in range(end=dims.ng) %} {%- for k in range(end=dims.nx) %} {%- if constraints.C_e[j][k] != 0 %} - C_e[{{ j }}+NGN * {{ k }}] = {{ constraints.C_e[j][k] }}; + C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} @@ -1715,7 +1658,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} - // set up convex-over-nonlinear constraints for last stage + // set up convex-over-nonlinear constraints for last stage double* luphi_e = calloc(2*NPHIN, sizeof(double)); double* lphi_e = luphi_e; double* uphi_e = luphi_e + NPHIN; @@ -1744,6 +1687,7 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* { const int N = capsule->nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; void *nlp_opts = capsule->nlp_opts; /************************************************ @@ -1751,9 +1695,12 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* ************************************************/ {% if solver_options.hessian_approx == "EXACT" %} - int nlp_solver_exact_hessian = 1; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); - + bool nlp_solver_exact_hessian = true; + // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate! + if (nlp_solver_exact_hessian) + { + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); + } int exact_hess_dyn = {{ solver_options.exact_hess_dyn }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn); @@ -1914,8 +1861,6 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); {%- endif %} - int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); {%- if solver_options.qp_solver is containing("HPIPM") %} // set HPIPM mode: should be done before setting other QP solver options @@ -1975,15 +1920,6 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* int print_level = {{ solver_options.print_level }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); -{%- if solver_options.qp_solver is containing('PARTIAL_CONDENSING') %} - int qp_solver_cond_ric_alg = {{ solver_options.qp_solver_cond_ric_alg }}; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg); -{% endif %} - -{%- if solver_options.qp_solver == 'PARTIAL_CONDENSING_HPIPM' %} - int qp_solver_ric_alg = {{ solver_options.qp_solver_ric_alg }}; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg); -{% endif %} int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; {%- if cost.cost_type == "EXTERNAL" %} @@ -2106,12 +2042,6 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c // 9) do precomputations int status = {{ model.name }}_acados_create_9_precompute(capsule); - - {%- if custom_update_filename != "" %} - // Initialize custom update function - custom_update_init_function(capsule); - {%- endif %} - return status; } @@ -2146,7 +2076,7 @@ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_caps } -int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem) +int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule) { // set initialization to all zeros @@ -2158,6 +2088,8 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int ocp_nlp_in* nlp_in = capsule->nlp_in; ocp_nlp_solver* nlp_solver = capsule->nlp_solver; + int nx, nu, nv, ns, nz, ni, dim; + double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double)); for(int i=0; i reset memory int qp_status; ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status); - if (reset_qp_solver_mem || (qp_status == 3)) + if (qp_status == 3) { // printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status); ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out); @@ -2212,6 +2144,7 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu exit(1); } +{%- if dims.np > 0 %} const int N = capsule->nlp_solver_plan->N; if (stage < N && stage >= 0) { @@ -2268,9 +2201,6 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p); capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p); capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p); - {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_0_fun.set_param(&capsule->conl_cost_0_fun, p); - capsule->conl_cost_0_fun_jac_hess.set_param(&capsule->conl_cost_0_fun_jac_hess, p); {%- elif cost.cost_type_0 == "EXTERNAL" %} capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p); capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p); @@ -2283,9 +2213,6 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p); capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p); capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p); - {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_fun[stage-1].set_param(capsule->conl_cost_fun+stage-1, p); - capsule->conl_cost_fun_jac_hess[stage-1].set_param(capsule->conl_cost_fun_jac_hess+stage-1, p); {%- elif cost.cost_type == "EXTERNAL" %} capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p); capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p); @@ -2302,9 +2229,6 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p); capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p); capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); - {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_e_fun.set_param(&capsule->conl_cost_e_fun, p); - capsule->conl_cost_e_fun_jac_hess.set_param(&capsule->conl_cost_e_fun_jac_hess, p); {%- elif cost.cost_type_e == "EXTERNAL" %} capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p); capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p); @@ -2321,149 +2245,16 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu {%- endif %} {% endif %} } +{% endif %}{# if dims.np #} return solver_status; } -int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) -{ - int solver_status = 0; - - int casadi_np = {{ dims.np }}; - if (casadi_np < n_update) { - printf("{{ model.name }}_acados_update_params_sparse: trying to set %d parameters for external functions." - " External function has %d parameters. Exiting.\n", n_update, casadi_np); - exit(1); - } - // for (int i = 0; i < n_update; i++) - // { - // if (idx[i] > casadi_np) { - // printf("{{ model.name }}_acados_update_params_sparse: attempt to set parameters with index %d, while" - // " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np); - // exit(1); - // } - // printf("param %d value %e\n", idx[i], p[i]); - // } - -{%- if dims.np > 0 %} - const int N = capsule->nlp_solver_plan->N; - if (stage < N && stage >= 0) - { - {%- if solver_options.integrator_type == "IRK" %} - capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p); - capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_z+stage, n_update, idx, p); - capsule->impl_dae_jac_x_xdot_u_z[stage].set_param_sparse(capsule->impl_dae_jac_x_xdot_u_z+stage, n_update, idx, p); - - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->impl_dae_hess[stage].set_param_sparse(capsule->impl_dae_hess+stage, n_update, idx, p); - {%- endif %} - {% elif solver_options.integrator_type == "LIFTED_IRK" %} - capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p); - capsule->impl_dae_fun_jac_x_xdot_u[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_u+stage, n_update, idx, p); - {% elif solver_options.integrator_type == "ERK" %} - capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p); - capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p); - - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->hess_vde_casadi[stage].set_param_sparse(capsule->hess_vde_casadi+stage, n_update, idx, p); - {%- endif %} - {% elif solver_options.integrator_type == "GNSF" %} - {% if model.gnsf.purely_linear != 1 %} - capsule->gnsf_phi_fun[stage].set_param_sparse(capsule->gnsf_phi_fun+stage, n_update, idx, p); - capsule->gnsf_phi_fun_jac_y[stage].set_param_sparse(capsule->gnsf_phi_fun_jac_y+stage, n_update, idx, p); - capsule->gnsf_phi_jac_y_uhat[stage].set_param_sparse(capsule->gnsf_phi_jac_y_uhat+stage, n_update, idx, p); - {% if model.gnsf.nontrivial_f_LO == 1 %} - capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param_sparse(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, n_update, idx, p); - {%- endif %} - {%- endif %} - {% elif solver_options.integrator_type == "DISCRETE" %} - capsule->discr_dyn_phi_fun[stage].set_param_sparse(capsule->discr_dyn_phi_fun+stage, n_update, idx, p); - capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, n_update, idx, p); - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, n_update, idx, p); - {% endif %} - {%- endif %}{# integrator_type #} - - // constraints - {% if constraints.constr_type == "BGP" %} - capsule->phi_constraint[stage].set_param_sparse(capsule->phi_constraint+stage, n_update, idx, p); - {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} - capsule->nl_constr_h_fun_jac[stage].set_param_sparse(capsule->nl_constr_h_fun_jac+stage, n_update, idx, p); - capsule->nl_constr_h_fun[stage].set_param_sparse(capsule->nl_constr_h_fun+stage, n_update, idx, p); - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->nl_constr_h_fun_jac_hess[stage].set_param_sparse(capsule->nl_constr_h_fun_jac_hess+stage, n_update, idx, p); - {%- endif %} - {%- endif %} - - // cost - if (stage == 0) - { - {%- if cost.cost_type_0 == "NONLINEAR_LS" %} - capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p); - capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p); - capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p); - {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_0_fun.set_param_sparse(&capsule->conl_cost_0_fun, n_update, idx, p); - capsule->conl_cost_0_fun_jac_hess.set_param_sparse(&capsule->conl_cost_0_fun_jac_hess, n_update, idx, p); - {%- elif cost.cost_type_0 == "EXTERNAL" %} - capsule->ext_cost_0_fun.set_param_sparse(&capsule->ext_cost_0_fun, n_update, idx, p); - capsule->ext_cost_0_fun_jac.set_param_sparse(&capsule->ext_cost_0_fun_jac, n_update, idx, p); - capsule->ext_cost_0_fun_jac_hess.set_param_sparse(&capsule->ext_cost_0_fun_jac_hess, n_update, idx, p); - {% endif %} - } - else // 0 < stage < N - { - {%- if cost.cost_type == "NONLINEAR_LS" %} - capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p); - capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p); - capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p); - {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_fun[stage-1].set_param_sparse(capsule->conl_cost_fun+stage-1, n_update, idx, p); - capsule->conl_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->conl_cost_fun_jac_hess+stage-1, n_update, idx, p); - {%- elif cost.cost_type == "EXTERNAL" %} - capsule->ext_cost_fun[stage-1].set_param_sparse(capsule->ext_cost_fun+stage-1, n_update, idx, p); - capsule->ext_cost_fun_jac[stage-1].set_param_sparse(capsule->ext_cost_fun_jac+stage-1, n_update, idx, p); - capsule->ext_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->ext_cost_fun_jac_hess+stage-1, n_update, idx, p); - {%- endif %} - } - } - - else // stage == N - { - // terminal shooting node has no dynamics - // cost - {%- if cost.cost_type_e == "NONLINEAR_LS" %} - capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p); - capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p); - capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p); - {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - capsule->conl_cost_e_fun.set_param_sparse(&capsule->conl_cost_e_fun, n_update, idx, p); - capsule->conl_cost_e_fun_jac_hess.set_param_sparse(&capsule->conl_cost_e_fun_jac_hess, n_update, idx, p); - {%- elif cost.cost_type_e == "EXTERNAL" %} - capsule->ext_cost_e_fun.set_param_sparse(&capsule->ext_cost_e_fun, n_update, idx, p); - capsule->ext_cost_e_fun_jac.set_param_sparse(&capsule->ext_cost_e_fun_jac, n_update, idx, p); - capsule->ext_cost_e_fun_jac_hess.set_param_sparse(&capsule->ext_cost_e_fun_jac_hess, n_update, idx, p); - {% endif %} - // constraints - {% if constraints.constr_type_e == "BGP" %} - capsule->phi_e_constraint.set_param_sparse(&capsule->phi_e_constraint, n_update, idx, p); - {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} - capsule->nl_constr_h_e_fun_jac.set_param_sparse(&capsule->nl_constr_h_e_fun_jac, n_update, idx, p); - capsule->nl_constr_h_e_fun.set_param_sparse(&capsule->nl_constr_h_e_fun, n_update, idx, p); - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->nl_constr_h_e_fun_jac_hess.set_param_sparse(&capsule->nl_constr_h_e_fun_jac_hess, n_update, idx, p); - {%- endif %} - {% endif %} - } -{% endif %}{# if dims.np #} - - return solver_status; -} int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule) { - // solve NLP + // solve NLP int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); return solver_status; @@ -2474,9 +2265,6 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) { // before destroying, keep some info const int N = capsule->nlp_solver_plan->N; - {%- if custom_update_filename != "" %} - custom_update_terminate_function(capsule); - {%- endif %} // free memory ocp_nlp_solver_opts_destroy(capsule->nlp_opts); ocp_nlp_in_destroy(capsule->nlp_in); @@ -2492,11 +2280,11 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- if solver_options.integrator_type == "IRK" %} for (int i = 0; i < N; i++) { - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]); - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); {%- if solver_options.hessian_approx == "EXACT" %} - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_hess[i]); + external_function_param_casadi_free(&capsule->impl_dae_hess[i]); {%- endif %} } free(capsule->impl_dae_fun); @@ -2509,8 +2297,8 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- elif solver_options.integrator_type == "LIFTED_IRK" %} for (int i = 0; i < N; i++) { - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]); - external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); } free(capsule->impl_dae_fun); free(capsule->impl_dae_fun_jac_x_xdot_u); @@ -2566,7 +2354,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess); {%- endif %} - + {%- endif %} // cost @@ -2574,9 +2362,6 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_param_casadi_free(&capsule->cost_y_0_fun); external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_0_hess); -{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - external_function_param_casadi_free(&capsule->conl_cost_0_fun); - external_function_param_casadi_free(&capsule->conl_cost_0_fun_jac_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun); external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac); @@ -2592,14 +2377,6 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) free(capsule->cost_y_fun); free(capsule->cost_y_fun_jac_ut_xt); free(capsule->cost_y_hess); -{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - for (int i = 0; i < N - 1; i++) - { - external_function_param_casadi_free(&capsule->conl_cost_fun[i]); - external_function_param_casadi_free(&capsule->conl_cost_fun_jac_hess[i]); - } - free(capsule->conl_cost_fun); - free(capsule->conl_cost_fun_jac_hess); {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 0; i < N - 1; i++) { @@ -2615,9 +2392,6 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_param_casadi_free(&capsule->cost_y_e_fun); external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_e_hess); -{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - external_function_param_casadi_free(&capsule->conl_cost_e_fun); - external_function_param_casadi_free(&capsule->conl_cost_e_fun_jac_hess); {%- elif cost.cost_type_e == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun); external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac); @@ -2664,6 +2438,15 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) return 0; } +ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } +ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } +ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } +ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } +ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } +void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } +ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } + void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule) { @@ -2678,13 +2461,8 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; - printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); - if (stat_n > 8) - printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); - printf("\n"); - {%- if solver_options.nlp_solver_type == "SQP" %} - + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha\n"); for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) @@ -2715,27 +2493,3 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul {%- endif %} } -int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len) -{ -{%- if custom_update_filename == "" %} - (void)capsule; - (void)data; - (void)data_len; - printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); - printf("nothing set yet..\n"); - return 1; -{% else %} - custom_update_function(capsule, data, data_len); -{%- endif %} -} - - - -ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } -ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } -ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } -ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } -ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } -void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } -ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } -ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h similarity index 84% rename from third_party/acados/acados_template/c_templates_tera/acados_solver.in.h rename to pyextra/acados_template/c_templates_tera/acados_solver.in.h index 5cf38aa8c8ff3b..1c82ef3ba0e3da 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -71,12 +74,6 @@ extern "C" { #endif -{%- if not solver_options.custom_update_filename %} - {%- set custom_update_filename = "" %} -{% else %} - {%- set custom_update_filename = solver_options.custom_update_filename %} -{%- endif %} - // ** capsule for solver data ** typedef struct {{ model.name }}_solver_capsule { @@ -102,15 +99,15 @@ typedef struct {{ model.name }}_solver_capsule external_function_param_casadi *hess_vde_casadi; {%- endif %} {% elif solver_options.integrator_type == "IRK" %} - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun; - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_z; - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_jac_x_xdot_u_z; + external_function_param_casadi *impl_dae_fun; + external_function_param_casadi *impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi *impl_dae_jac_x_xdot_u_z; {% if solver_options.hessian_approx == "EXACT" %} - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess; + external_function_param_casadi *impl_dae_hess; {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun; - external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_u; + external_function_param_casadi *impl_dae_fun; + external_function_param_casadi *impl_dae_fun_jac_x_xdot_u; {% elif solver_options.integrator_type == "GNSF" %} external_function_param_casadi *gnsf_phi_fun; external_function_param_casadi *gnsf_phi_fun_jac_y; @@ -131,9 +128,6 @@ typedef struct {{ model.name }}_solver_capsule external_function_param_casadi *cost_y_fun; external_function_param_casadi *cost_y_fun_jac_ut_xt; external_function_param_casadi *cost_y_hess; -{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - external_function_param_casadi *conl_cost_fun; - external_function_param_casadi *conl_cost_fun_jac_hess; {%- elif cost.cost_type == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun; external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac; @@ -144,9 +138,6 @@ typedef struct {{ model.name }}_solver_capsule external_function_param_casadi cost_y_0_fun; external_function_param_casadi cost_y_0_fun_jac_ut_xt; external_function_param_casadi cost_y_0_hess; -{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - external_function_param_casadi conl_cost_0_fun; - external_function_param_casadi conl_cost_0_fun_jac_hess; {% elif cost.cost_type_0 == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun; external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac; @@ -157,9 +148,6 @@ typedef struct {{ model.name }}_solver_capsule external_function_param_casadi cost_y_e_fun; external_function_param_casadi cost_y_e_fun_jac_ut_xt; external_function_param_casadi cost_y_e_hess; -{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - external_function_param_casadi conl_cost_e_fun; - external_function_param_casadi conl_cost_e_fun_jac_hess; {% elif cost.cost_type_e == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun; external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac; @@ -172,10 +160,8 @@ typedef struct {{ model.name }}_solver_capsule {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} external_function_param_casadi *nl_constr_h_fun_jac; external_function_param_casadi *nl_constr_h_fun; -{%- if solver_options.hessian_approx == "EXACT" %} external_function_param_casadi *nl_constr_h_fun_jac_hess; {%- endif %} -{%- endif %} {% if constraints.constr_type_e == "BGP" %} @@ -183,14 +169,8 @@ typedef struct {{ model.name }}_solver_capsule {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} external_function_param_casadi nl_constr_h_e_fun_jac; external_function_param_casadi nl_constr_h_e_fun; -{%- if solver_options.hessian_approx == "EXACT" %} external_function_param_casadi nl_constr_h_e_fun_jac_hess; {%- endif %} -{%- endif %} - -{%- if custom_update_filename != "" %} - void * custom_update_memory; -{%- endif %} } {{ model.name }}_solver_capsule; @@ -199,7 +179,7 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_s ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule); /** * Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than @@ -217,14 +197,10 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name */ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N); ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np); -ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); - ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule); -ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len); - - + ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule); diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd similarity index 90% rename from third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd rename to pyextra/acados_template/c_templates_tera/acados_solver.in.pxd index 233e3f79da5198..09f8755cbeed53 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd @@ -1,5 +1,8 @@ # -# Copyright (c) The acados authors. +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl # # This file is part of acados. # @@ -44,14 +47,11 @@ cdef extern from "acados_solver_{{ model.name }}.h": int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) - int acados_update_params_sparse "{{ model.name }}_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule) - int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem) + int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule) int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule) void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule) - int acados_custom_update "{{ model.name }}_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len) - acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule) acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule) acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule) diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c similarity index 85% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c rename to pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c index 3dd248037aeedd..96e0983de6cd4e 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -58,8 +61,6 @@ static void mdlInitializeSizes (SimStruct *S) ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); - int N = {{ model.name | upper }}_N; - {%- for key, val in simulink_opts.inputs -%} {%- if val != 0 and val != 1 -%} {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }} @@ -116,12 +117,6 @@ static void mdlInitializeSizes (SimStruct *S) {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} {%- set n_inputs = n_inputs + 1 -%} {%- endif -%} - {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} - {%- set n_inputs = n_inputs + 1 -%} - {%- endif -%} - {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} - {%- set n_inputs = n_inputs + 1 -%} - {%- endif -%} {%- for key, val in simulink_opts.inputs -%} {%- if val != 0 and val != 1 -%} @@ -182,7 +177,7 @@ static void mdlInitializeSizes (SimStruct *S) {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} {%- set i_input = i_input + 1 %} // parameters - ssSetInputPortVectorDimension(S, {{ i_input }}, (N+1) * {{ dims.np }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }}); {%- endif %} {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %} @@ -240,34 +235,23 @@ static void mdlInitializeSizes (SimStruct *S) {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} {%- set i_input = i_input + 1 %} // lg - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); {%- endif -%} {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} {%- set i_input = i_input + 1 %} // ug - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); {%- endif -%} {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} {%- set i_input = i_input + 1 %} // lh - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); {%- endif -%} {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} {%- set i_input = i_input + 1 %} // uh - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }}); - {%- endif -%} - - {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} - {%- set i_input = i_input + 1 %} - // lh_e - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }}); - {%- endif -%} - {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} - {%- set i_input = i_input + 1 %} - // uh_e - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }}); {%- endif -%} {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} @@ -328,21 +312,11 @@ static void mdlInitializeSizes (SimStruct *S) ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); {%- endif %} - {%- if simulink_opts.outputs.cost_value == 1 %} - {%- set i_output = i_output + 1 %} - ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); - {%- endif %} - {%- if simulink_opts.outputs.KKT_residual == 1 %} {%- set i_output = i_output + 1 %} ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 ); {%- endif %} - {%- if simulink_opts.outputs.KKT_residuals == 1 %} - {%- set i_output = i_output + 1 %} - ssSetOutputPortVectorDimension(S, {{ i_output }}, 4 ); - {%- endif %} - {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} {%- set i_output = i_output + 1 %} ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1 @@ -430,9 +404,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) InputRealPtrsType in_sign; - int N = {{ model.name | upper }}_N; - - {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbx_e, dims.nbu, dims.ng, dims.nh, dims.ng_e, dims.nh_e] -%} + {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%} {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #} {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %} @@ -485,7 +457,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); // update value of parameters - for (int ii = 0; ii <= N; ii++) + for (int ii = 0; ii <= {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.np }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]); @@ -509,7 +481,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int ii = 1; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.ny }}; jj++) buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]); @@ -525,14 +497,14 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.ny_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", (void *) buffer); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer); {%- endif %} {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} // lbx {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int ii = 1; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.nbx }}; jj++) buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); @@ -543,7 +515,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // ubx {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < N; ii++) + for (int ii = 1; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.nbx }}; jj++) buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]); @@ -559,7 +531,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer); {%- endif %} {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} // ubx_e @@ -568,7 +540,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer); {%- endif %} @@ -576,7 +548,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // lbu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int ii = 0; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); @@ -587,7 +559,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // ubu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int ii = 0; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); @@ -600,67 +572,44 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) - { - for (int jj = 0; jj < {{ dims.ng }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", (void *) buffer); - } + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer); {%- endif -%} {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} // ug {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) - { - for (int jj = 0; jj < {{ dims.ng }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", (void *) buffer); - } - {%- endif -%} + for (int i = 0; i < {{ dims.ng }}; i++) + buffer[i] = (double)(*in_sign[i]); + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer); + {%- endif -%} {%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} // lh {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) - { - for (int jj = 0; jj < {{ dims.nh }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", (void *) buffer); - } + for (int i = 0; i < {{ dims.nh }}; i++) + buffer[i] = (double)(*in_sign[i]); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer); {%- endif -%} {%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} // uh {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) - { - for (int jj = 0; jj < {{ dims.nh }}; jj++) - buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", (void *) buffer); - } - {%- endif -%} - - - {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} - // lh_e - {%- set i_input = i_input + 1 %} - in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int i = 0; i < {{ dims.nh_e }}; i++) - buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer); - {%- endif -%} - {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} - // uh_e - {%- set i_input = i_input + 1 %} - in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int i = 0; i < {{ dims.nh_e }}; i++) + for (int i = 0; i < {{ dims.nh }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer); + + for (int ii = 0; ii < {{ dims.N }}; ii++) + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer); {%- endif -%} {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} @@ -680,7 +629,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.ny * dims.ny }}; i++) buffer[i] = (double)(*in_sign[i]); - for (int ii = 1; ii < N; ii++) + for (int ii = 1; ii < {{ dims.N }}; ii++) ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer); {%- endif %} @@ -691,7 +640,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", buffer); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer); {%- endif %} {%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} @@ -701,7 +650,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) double reset = (double)(*in_sign[0]); if (reset) { - {{ model.name }}_acados_reset(capsule, 1); + {{ model.name }}_acados_reset(capsule); } {%- endif %} @@ -721,7 +670,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // u_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < N; ii++) + for (int ii = 0; ii < {{ dims.N }}; ii++) { for (int jj = 0; jj < {{ dims.nu }}; jj++) buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]); @@ -737,7 +686,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* set outputs */ // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_KKT_residuals, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin, *out_cost_value; + real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; int tmp_int; {%- set i_output = -1 -%}{# note here i_output is 0-based #} @@ -750,7 +699,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- if simulink_opts.outputs.utraj == 1 %} {%- set i_output = i_output + 1 %} out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < N; ii++) + for (int ii = 0; ii < {{ dims.N }}; ii++) ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) (out_utraj + ii * {{ dims.nu }})); {%- endif %} @@ -770,32 +719,12 @@ static void mdlOutputs(SimStruct *S, int_T tid) *out_status = (real_t) acados_status; {%- endif %} - {%- if simulink_opts.outputs.cost_value == 1 %} - {%- set i_output = i_output + 1 %} - out_cost_value = ssGetOutputPortRealSignal(S, {{ i_output }}); - ocp_nlp_eval_cost(capsule->nlp_solver, nlp_in, nlp_out); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_cost_value); - {%- endif %} - {%- if simulink_opts.outputs.KKT_residual == 1 %} {%- set i_output = i_output + 1 %} out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }}); *out_KKT_res = (real_t) nlp_out->inf_norm_res; {%- endif %} - {%- if simulink_opts.outputs.KKT_residuals == 1 %} - {%- set i_output = i_output + 1 %} - out_KKT_residuals = ssGetOutputPortRealSignal(S, {{ i_output }}); - - {%- if solver_options.nlp_solver_type == "SQP_RTI" %} - ocp_nlp_eval_residuals(capsule->nlp_solver, nlp_in, nlp_out); - {%- endif %} - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_KKT_residuals[0]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_KKT_residuals[1]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_KKT_residuals[2]); - ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_KKT_residuals[3]); - {%- endif %} - {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} {%- set i_output = i_output + 1 %} out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }}); diff --git a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h new file mode 100644 index 00000000000000..347446e3f4e895 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_0_COST +#define {{ model.name }}_Y_0_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_0 == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_n_in(void); +int {{ model.name }}_cost_y_0_fun_n_out(void); + +int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); +int {{ model.name }}_cost_y_0_hess_n_in(void); +int {{ model.name }}_cost_y_0_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_0_COST diff --git a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h new file mode 100644 index 00000000000000..acc99009fef648 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_E_COST +#define {{ model.name }}_Y_E_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type_e == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_n_in(void); +int {{ model.name }}_cost_y_e_fun_n_out(void); + +int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); +int {{ model.name }}_cost_y_e_hess_n_in(void); +int {{ model.name }}_cost_y_e_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_E_COST diff --git a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h new file mode 100644 index 00000000000000..1e03780cc1b59b --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h @@ -0,0 +1,69 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_Y_COST +#define {{ model.name }}_Y_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_type == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_sparsity_out(int); +int {{ model.name }}_cost_y_fun_n_in(void); +int {{ model.name }}_cost_y_fun_n_out(void); + +int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_hess_sparsity_out(int); +int {{ model.name }}_cost_y_hess_n_in(void); +int {{ model.name }}_cost_y_hess_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_Y_COST diff --git a/pyextra/acados_template/c_templates_tera/external_cost.in.h b/pyextra/acados_template/c_templates_tera/external_cost.in.h new file mode 100644 index 00000000000000..d200dba45e318c --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST +#define {{ model.name }}_EXT_COST + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type == "casadi" %} +{% if cost.cost_type == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST diff --git a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h new file mode 100644 index 00000000000000..8152447e5f8abe --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h @@ -0,0 +1,75 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_0 +#define {{ model.name }}_EXT_COST_0 + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_0 == "casadi" %} + +{% if cost.cost_type_0 == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_0 diff --git a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h new file mode 100644 index 00000000000000..56485db4c73ed1 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h @@ -0,0 +1,74 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_EXT_COST_E +#define {{ model.name }}_EXT_COST_E + +#ifdef __cplusplus +extern "C" { +#endif + +{% if cost.cost_ext_fun_type_e == "casadi" %} +{% if cost.cost_type_e == "EXTERNAL" %} +int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); +{% endif %} + +{% else %} +int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_EXT_COST_E diff --git a/pyextra/acados_template/c_templates_tera/h_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_constraint.in.h new file mode 100644 index 00000000000000..e49176c6efea1e --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/h_constraint.in.h @@ -0,0 +1,70 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_H_CONSTRAINT +#define {{ model.name }}_H_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh > 0 %} +int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_sparsity_out(int); +int {{ model.name }}_constr_h_fun_n_in(void); +int {{ model.name }}_constr_h_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_CONSTRAINT diff --git a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h new file mode 100644 index 00000000000000..a5dd7116415252 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h @@ -0,0 +1,71 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_H_E_CONSTRAINT +#define {{ model.name }}_H_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nh_e > 0 %} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_n_in(void); +int {{ model.name }}_constr_h_e_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_H_E_CONSTRAINT diff --git a/third_party/acados/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c similarity index 94% rename from third_party/acados/acados_template/c_templates_tera/main.in.c rename to pyextra/acados_template/c_templates_tera/main.in.c index 92a8b33eac2d61..99c4f13be1398c 100644 --- a/third_party/acados/acados_template/c_templates_tera/main.in.c +++ b/pyextra/acados_template/c_templates_tera/main.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -28,11 +31,6 @@ * POSSIBILITY OF SUCH DAMAGE.; */ -{%- if not solver_options.custom_update_filename %} - {%- set custom_update_filename = "" %} -{% else %} - {%- set custom_update_filename = solver_options.custom_update_filename %} -{%- endif %} // standard #include @@ -44,9 +42,6 @@ #include "acados_c/external_function_interface.h" #include "acados_solver_{{ model.name }}.h" -// blasfeo -#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" - #define NX {{ model.name | upper }}_NX #define NZ {{ model.name | upper }}_NZ #define NU {{ model.name | upper }}_NU @@ -196,11 +191,6 @@ int main() printf("{{ model.name }}_acados_solve() failed with status %d.\n", status); } - -{%- if custom_update_filename != "" %} - {{ model.name }}_acados_custom_update(acados_ocp_capsule, xtraj, NX*(N+1)); -{%- endif %} - // get solution ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter); diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c b/pyextra/acados_template/c_templates_tera/main_mex.in.c similarity index 95% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c rename to pyextra/acados_template/c_templates_tera/main_mex.in.c index 851a3cc04f6fd3..8da5db29a0aaa7 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c +++ b/pyextra/acados_template/c_templates_tera/main_mex.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c b/pyextra/acados_template/c_templates_tera/main_sim.in.c similarity index 93% rename from third_party/acados/acados_template/c_templates_tera/main_sim.in.c rename to pyextra/acados_template/c_templates_tera/main_sim.in.c index 8960aa00354bde..743e81d593d798 100644 --- a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c +++ b/pyextra/acados_template/c_templates_tera/main_sim.in.c @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m similarity index 93% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m rename to pyextra/acados_template/c_templates_tera/make_main_mex.in.m index d217948456b898..9188686a0d4763 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m +++ b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m @@ -1,5 +1,8 @@ % -% Copyright (c) The acados authors. +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl % % This file is part of acados. % @@ -26,7 +29,6 @@ % CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE.; - % function make_main_mex_{{ model.name }}() diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m b/pyextra/acados_template/c_templates_tera/make_mex.in.m similarity index 81% rename from third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m rename to pyextra/acados_template/c_templates_tera/make_mex.in.m index 5e358271374ea2..cde30f6f413c63 100644 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m +++ b/pyextra/acados_template/c_templates_tera/make_mex.in.m @@ -1,5 +1,8 @@ % -% Copyright (c) The acados authors. +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl % % This file is part of acados. % @@ -26,7 +29,6 @@ % CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE.; - % function make_mex_{{ model.name }}() @@ -46,23 +48,6 @@ blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')]; hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')]; - % load linking information of compiled acados - link_libs_core_filename = fullfile(acados_folder, 'lib', 'link_libs.json'); - addpath(fullfile(acados_folder, 'external', 'jsonlab')); - link_libs = loadjson(link_libs_core_filename); - - % add necessary link instructs - acados_lib_extra = {}; - lib_names = fieldnames(link_libs); - for idx = 1 : numel(lib_names) - lib_name = lib_names{idx}; - link_arg = link_libs.(lib_name); - if ~isempty(link_arg) - acados_lib_extra = [acados_lib_extra, link_arg]; - end - end - - mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')]; mex_names = { ... @@ -109,8 +94,7 @@ if is_octave() % mkoctfile -p CFLAGS mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... - template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',... - acados_lib_extra{:}, mex_files{ii}) + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) else if ismac() FLAGS = 'CFLAGS=$CFLAGS -std=c99'; @@ -118,8 +102,7 @@ FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; end mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... - template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',... - acados_lib_extra{:}, mex_files{ii}) + acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) end end diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m new file mode 100644 index 00000000000000..77603a78fa0228 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m @@ -0,0 +1,344 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = { ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',... + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',... + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',... + {% if model.gnsf.nontrivial_f_LO == 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',... + {%- endif %} + {%- endif %} + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',... + {%- elif solver_options.integrator_type == "DISCRETE" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',... + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',... + {%- if solver_options.hessian_approx == "EXACT" %} + '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',... + {%- endif %} + {%- endif %} + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',... + {%- elif cost.cost_type_0 == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',... + {%- endif %} + + {%- if cost.cost_type == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',... + {%- elif cost.cost_type == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',... + {%- endif %} + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',... + {%- elif cost.cost_type_e == "EXTERNAL" %} + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',... + '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',... + {%- endif %} + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ... + {%- endif %} + {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ... + '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ... + {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} + '{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ... + {%- endif %} + 'acados_solver_sfunction_{{ model.name }}.c', ... + 'acados_solver_{{ model.name }}.c' + }; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... + ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... + ['-I', fullfile(INC_PATH, 'acados')], ... + ['-I', fullfile(INC_PATH)]}; + +{% if solver_options.qp_solver is containing("QPOASES") %} +INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')]; +{% endif %} + +CFLAGS = 'CFLAGS=$CFLAGS'; +LDFLAGS = 'LDFLAGS=$LDFLAGS'; +COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; +COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; + +{% if solver_options.qp_solver is containing("QPOASES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ]; +{%- elif solver_options.qp_solver is containing("OSQP") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ]; +{%- elif solver_options.qp_solver is containing("QPDUNES") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; +{%- elif solver_options.qp_solver is containing("HPMPC") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ]; +{% endif %} + +LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')]; + +LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; + +% acados linking libraries and flags +{%- if acados_link_libs and os and os == "pc" %} +LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}']; +COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}']; +LIBS{end+1} = '{{ acados_link_libs.qpoases }}'; +LIBS{end+1} = '{{ acados_link_libs.hpmpc }}'; +LIBS{end+1} = '{{ acados_link_libs.osqp }}'; +{%- else %} + {% if solver_options.qp_solver is containing("QPOASES") %} +LIBS{end+1} = '-lqpOASES_e'; + {% endif %} +{%- endif %} + +mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... + LIB_PATH, LIBS{:}, SOURCES{:}, ... + '-output', 'acados_solver_sfunction_{{ model.name }}' ); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n'; +i_in = 1; + + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} +input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} +input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... + ' size [{{ dims.nbx_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} +input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... + ' size [{{ (dims.N+1)*dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} +input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... + ' size [{{ (dims.N-1) * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} +input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} +input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} +input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} +input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} +input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} +input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} +{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} +input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); +i_in = i_in + 1; +{%- endif -%} + +{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} +input_note = strcat(input_note, num2str(i_in), ') lg, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} +input_note = strcat(input_note, num2str(i_in), ') ug, size [{{ dims.ng }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} +input_note = strcat(input_note, num2str(i_in), ') lh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} +{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} +input_note = strcat(input_note, num2str(i_in), ') uh, size [{{ dims.nh }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} +input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} +input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} +input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.x_init %} {#- x_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if simulink_opts.inputs.u_init %} {#- u_init #} +input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n '); +i_in = i_in + 1; +{%- endif %} + +fprintf(input_note) + +disp(' ') + +output_note = 'Outputs are:\n'; +i_out = 0; + +{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.utraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.xtraj == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n '); +{%- endif %} + +{%- if simulink_opts.outputs.solver_status == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); +{%- endif %} + +{%- if simulink_opts.outputs.KKT_residual == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); +{%- endif %} + +{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_sim == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_qp == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_lin == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n '); +{%- endif %} + +{%- if simulink_opts.outputs.sqp_iter == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); +{%- endif %} + +fprintf(output_note) diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m new file mode 100644 index 00000000000000..a0c503e41abaa2 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m @@ -0,0 +1,103 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ... + 'acados_sim_solver_{{ model.name }}.c ', ... + {%- if solver_options.integrator_type == 'ERK' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "IRK" %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ... + '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ... + {%- if solver_options.hessian_approx == 'EXACT' %} + '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',... + {%- endif %} + {%- elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c ' + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c ' + '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ' + {% if model.gnsf.nontrivial_f_LO == 1 %} + '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ' + {%- endif %} + {%- endif %} + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ' + {%- endif %} + ]; + +INC_PATH = '{{ acados_include_path }}'; + +INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ... + ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ... + ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' ']; + +CFLAGS = ' -O'; + +LIB_PATH = '{{ acados_lib_path }}'; + +LIBS = '-lacados -lblasfeo -lhpipm'; + +eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... + CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); + +fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ... + eval('mexext')] ); + + +%% print note on usage of s-function +fprintf('\n\nNote: Usage of Sfunction is as follows:\n') +input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n '; +i_in = 2; +{%- if dims.nu > 0 %} +input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n '); +i_in = i_in + 1; +{%- endif %} + +{%- if dims.np > 0 %} +input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n '); +i_in = i_in + 1; +{%- endif %} + + +fprintf(input_note) + +disp(' ') + +output_note = strcat('Outputs are:\n', ... + '1) x1 - simulated state, size [{{ dims.nx }}]\n'); + +fprintf(output_note) diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m new file mode 100644 index 00000000000000..278e0a605c28b0 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/mex_solver.in.m @@ -0,0 +1,166 @@ +% +% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; +% + +classdef {{ model.name }}_mex_solver < handle + + properties + C_ocp + C_ocp_ext_fun + cost_ext_fun_type + cost_ext_fun_type_e + end % properties + + + + methods + + % constructor + function obj = {{ model.name }}_mex_solver() + make_mex_{{ model.name }}(); + [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}(); + % to have path to destructor when changing directory + addpath('.') + obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}'; + obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}'; + end + + % destructor + function delete(obj) + if ~isempty(obj.C_ocp) + acados_mex_free_{{ model.name }}(obj.C_ocp); + end + end + + % solve + function solve(obj) + acados_mex_solve_{{ model.name }}(obj.C_ocp); + end + + % set -- borrowed from MEX interface + function set(varargin) + obj = varargin{1}; + field = varargin{2}; + value = varargin{3}; + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + if nargin==3 + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value); + elseif nargin==4 + stage = varargin{4}; + acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage); + else + disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)'); + end + end + + function value = get_cost(obj) + value = ocp_get_cost(obj.C_ocp); + end + + % get -- borrowed from MEX interface + function value = get(varargin) + % usage: + % obj.get(field, value, [stage]) + obj = varargin{1}; + field = varargin{2}; + if any(strfind('sens', field)) + error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') + end + if ~isa(field, 'char') + error('field must be a char vector, use '' '''); + end + + if nargin==2 + value = ocp_get(obj.C_ocp, field); + elseif nargin==3 + stage = varargin{3}; + value = ocp_get(obj.C_ocp, field, stage); + else + disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); + end + end + + + % print + function print(varargin) + if nargin < 2 + field = 'stat'; + else + field = varargin{2}; + end + + obj = varargin{1}; + + if strcmp(field, 'stat') + stat = obj.get('stat'); + {%- if solver_options.nlp_solver_type == "SQP" %} + fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha'); + if size(stat,2)>8 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8)); + if size(stat,2)>8 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12)); + end + fprintf('\n'); + end + fprintf('\n'); + {%- else %} + fprintf('\niter\tqp_status\tqp_iter'); + if size(stat,2)>3 + fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); + end + fprintf('\n'); + for jj=1:size(stat,1) + fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7)); + end + fprintf('\n'); + end + {% endif %} + + else + fprintf('unsupported field in function print of acados_ocp.print, got %s', field); + keyboard + end + + end + + end % methods + +end % class + diff --git a/third_party/acados/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h similarity index 94% rename from third_party/acados/acados_template/c_templates_tera/model.in.h rename to pyextra/acados_template/c_templates_tera/model.in.h index e5059df9ffa90b..918e2bc29e5b68 100644 --- a/third_party/acados/acados_template/c_templates_tera/model.in.h +++ b/pyextra/acados_template/c_templates_tera/model.in.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -44,8 +47,7 @@ extern "C" { {%- endif %} {% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %} - {% if model.dyn_ext_fun_type == "casadi" %} -// implicit ODE: function +// implicit ODE int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_fun_sparsity_in(int); @@ -53,7 +55,7 @@ const int *{{ model.name }}_impl_dae_fun_sparsity_out(int); int {{ model.name }}_impl_dae_fun_n_in(void); int {{ model.name }}_impl_dae_fun_n_out(void); -// implicit ODE: function + jacobians +// implicit ODE int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); @@ -61,7 +63,7 @@ const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void); int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void); -// implicit ODE: jacobians only +// implicit ODE int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int); @@ -77,23 +79,14 @@ const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void); int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void); - {%- if hessian_approx == "EXACT" %} -// implicit ODE - hessian +{%- if hessian_approx == "EXACT" %} int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_hess_sparsity_in(int); const int *{{ model.name }}_impl_dae_hess_sparsity_out(int); int {{ model.name }}_impl_dae_hess_n_in(void); int {{ model.name }}_impl_dae_hess_n_out(void); - {% endif %} - {% else %}{# ext_fun_type #} - {%- if hessian_approx == "EXACT" %} -int {{ model.dyn_impl_dae_hess }}(void **, void **, void *); - {% endif %} -int {{ model.dyn_impl_dae_fun_jac }}(void **, void **, void *); -int {{ model.dyn_impl_dae_jac }}(void **, void **, void *); -int {{ model.dyn_impl_dae_fun }}(void **, void **, void *); - {% endif %}{# ext_fun_type #} +{%- endif %} {% elif solver_options.integrator_type == "GNSF" %} /* GNSF Functions */ diff --git a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h new file mode 100644 index 00000000000000..283ed7f88953b8 --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h @@ -0,0 +1,55 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_PHI_CONSTRAINT +#define {{ model.name }}_PHI_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi > 0 %} +// implicit ODE +int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_constraint_sparsity_out(int); +int {{ model.name }}_phi_constraint_n_in(void); +int {{ model.name }}_phi_constraint_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_CONSTRAINT diff --git a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h new file mode 100644 index 00000000000000..dc8e293ad7cdfa --- /dev/null +++ b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h @@ -0,0 +1,21 @@ +#ifndef {{ model.name }}_PHI_E_CONSTRAINT +#define {{ model.name }}_PHI_E_CONSTRAINT + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi_e > 0 %} +int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); +int {{ model.name }}_phi_e_constraint_n_in(void); +int {{ model.name }}_phi_e_constraint_n_out(void); +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_PHI_E_CONSTRAINT diff --git a/pyextra/acados_template/generate_c_code_constraint.py b/pyextra/acados_template/generate_c_code_constraint.py new file mode 100644 index 00000000000000..c79ddc129a57eb --- /dev/null +++ b/pyextra/acados_template/generate_c_code_constraint.py @@ -0,0 +1,180 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_constraint( model, con_name, is_terminal, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load constraint variables and expression + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if is_terminal: + con_h_expr = model.con_h_expr_e + con_phi_expr = model.con_phi_expr_e + # create dummy u, z + u = symbol('u', 0, 0) + z = symbol('z', 0, 0) + else: + con_h_expr = model.con_h_expr + con_phi_expr = model.con_phi_expr + u = model.u + z = model.z + + if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): + raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") + + if not (is_empty(con_h_expr) and is_empty(con_phi_expr)): + if is_empty(con_h_expr): + constr_type = 'BGP' + else: + constr_type = 'BGH' + + if is_empty(p): + p = symbol('p', 0, 0) + + if is_empty(z): + z = symbol('z', 0, 0) + + if not (is_empty(con_h_expr)) and opts['generate_hess']: + # multipliers for hessian + nh = casadi_length(con_h_expr) + lam_h = symbol('lam_h', nh, 1) + + # set up & change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = con_name + '_constraints' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = os.path.join('.', gen_dir) + os.chdir(gen_dir_location) + + # export casadi functions + if constr_type == 'BGH': + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt' + + jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x))) + jac_z_t = jacobian(con_h_expr, z) + constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \ + [con_h_expr, jac_ux_t, jac_z_t]) + + constraint_fun_jac_tran.generate(fun_name, casadi_opts) + if opts['generate_hess']: + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' + else: + fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' + + # adjoint + adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True) + # hessian + hess_ux = jacobian(adj_ux, vertcat(u, x)) + + adj_z = jtimes(con_h_expr, z, lam_h, True) + hess_z = jacobian(adj_z, z) + + # set up functions + constraint_fun_jac_tran_hess = \ + Function(fun_name, [x, u, lam_h, z, p], \ + [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) + + # generate C code + constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts) + + if is_terminal: + fun_name = con_name + '_constr_h_e_fun' + else: + fun_name = con_name + '_constr_h_fun' + h_fun = Function(fun_name, [x, u, z, p], [con_h_expr]) + h_fun.generate(fun_name, casadi_opts) + + else: # BGP constraint + if is_terminal: + fun_name = con_name + '_phi_e_constraint' + r = model.con_r_in_phi_e + con_r_expr = model.con_r_expr_e + else: + fun_name = con_name + '_phi_constraint' + r = model.con_r_in_phi + con_r_expr = model.con_r_expr + + nphi = casadi_length(con_phi_expr) + con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr) + phi_jac_u = jacobian(con_phi_expr_x_u_z, u) + phi_jac_x = jacobian(con_phi_expr_x_u_z, x) + phi_jac_z = jacobian(con_phi_expr_x_u_z, z) + + hess = hessian(con_phi_expr[0], r)[0] + for i in range(1, nphi): + hess = vertcat(hess, hessian(con_phi_expr[i], r)[0]) + + r_jac_u = jacobian(con_r_expr, u) + r_jac_x = jacobian(con_r_expr, x) + + constraint_phi = \ + Function(fun_name, [x, u, z, p], \ + [con_phi_expr_x_u_z, \ + vertcat(transpose(phi_jac_u), \ + transpose(phi_jac_x)), \ + transpose(phi_jac_z), \ + hess, vertcat(transpose(r_jac_u), \ + transpose(r_jac_x))]) + + constraint_phi.generate(fun_name, casadi_opts) + + # change directory back + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_discrete_dynamics.py b/pyextra/acados_template/generate_c_code_discrete_dynamics.py new file mode 100644 index 00000000000000..c6a245ff814b0e --- /dev/null +++ b/pyextra/acados_template/generate_c_code_discrete_dynamics.py @@ -0,0 +1,98 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +import os +import casadi as ca +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_discrete_dynamics( model, opts ): + + casadi_version = ca.CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + # load model + x = model.x + u = model.u + p = model.p + phi = model.disc_dyn_expr + model_name = model.name + nx = casadi_length(x) + + if isinstance(phi, ca.MX): + symbol = ca.MX.sym + elif isinstance(phi, ca.SX): + symbol = ca.SX.sym + else: + Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi))) + + # assume nx1 = nx !!! + lam = symbol('lam', nx, 1) + + # generate jacobians + ux = ca.vertcat(u,x) + jac_ux = ca.jacobian(phi, ux) + # generate adjoint + adj_ux = ca.jtimes(phi, ux, lam, True) + # generate hessian + hess_ux = ca.jacobian(adj_ux, ux) + + ## change directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + # set up & generate Functions + fun_name = model_name + '_dyn_disc_phi_fun' + phi_fun = ca.Function(fun_name, [x, u, p], [phi]) + phi_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac' + phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) + phi_fun_jac_ut_xt.generate(fun_name, casadi_opts) + + fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' + phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) + phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/pyextra/acados_template/generate_c_code_explicit_ode.py b/pyextra/acados_template/generate_c_code_explicit_ode.py new file mode 100644 index 00000000000000..76e64005352a65 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_explicit_ode.py @@ -0,0 +1,124 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_explicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + # load model + x = model.x + u = model.u + p = model.p + f_expl = model.f_expl_expr + model_name = model.name + + ## get model dimensions + nx = x.size()[0] + nu = u.size()[0] + + if isinstance(f_expl, casadi.MX): + symbol = MX.sym + elif isinstance(f_expl, casadi.SX): + symbol = SX.sym + else: + raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.") + ## set up functions to be exported + Sx = symbol('Sx', nx, nx) + Sp = symbol('Sp', nx, nu) + lambdaX = symbol('lambdaX', nx, 1) + + fun_name = model_name + '_expl_ode_fun' + + ## Set up functions + expl_ode_fun = Function(fun_name, [x, u, p], [f_expl]) + + vdeX = jtimes(f_expl,x,Sx) + vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp) + + fun_name = model_name + '_expl_vde_forw' + + expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) + + adj = jtimes(f_expl, vertcat(x, u), lambdaX, True) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj]) + + if generate_hess: + S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu))) + hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw)) + hess2 = [] + for j in range(nx+nu): + for i in range(j,nx+nu): + hess2 = vertcat(hess2, hess[i,j]) + + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) + + ## generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + fun_name = model_name + '_expl_ode_fun' + expl_ode_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_forw' + expl_vde_forw.generate(fun_name, casadi_opts) + + fun_name = model_name + '_expl_vde_adj' + expl_vde_adj.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_expl_ode_hess' + expl_ode_hess.generate(fun_name, casadi_opts) + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_external_cost.py b/pyextra/acados_template/generate_c_code_external_cost.py new file mode 100644 index 00000000000000..8396522619c138 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_external_cost.py @@ -0,0 +1,116 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta +from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning + + +def generate_c_code_external_cost(model, stage_type, opts): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + suffix_name = "_cost_ext_cost_e_fun" + suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_e_fun_jac" + u = symbol("u", 0, 0) + ext_cost = model.cost_expr_ext_cost_e + custom_hess = model.cost_expr_ext_cost_custom_hess_e + + elif stage_type == 'path': + suffix_name = "_cost_ext_cost_fun" + suffix_name_hess = "_cost_ext_cost_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost + custom_hess = model.cost_expr_ext_cost_custom_hess + + elif stage_type == 'initial': + suffix_name = "_cost_ext_cost_0_fun" + suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" + suffix_name_jac = "_cost_ext_cost_0_fun_jac" + u = model.u + ext_cost = model.cost_expr_ext_cost_0 + custom_hess = model.cost_expr_ext_cost_custom_hess_0 + + # set up functions to be exported + fun_name = model.name + suffix_name + fun_name_hess = model.name + suffix_name_hess + fun_name_jac = model.name + suffix_name_jac + + # generate expression for full gradient and Hessian + full_hess, grad = hessian(ext_cost, vertcat(u, x)) + + if custom_hess is not None: + full_hess = custom_hess + + ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) + ext_cost_fun_jac_hess = Function( + fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] + ) + ext_cost_fun_jac = Function( + fun_name_jac, [x, u, p], [ext_cost, grad] + ) + + # generate C code + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = model.name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = "./" + gen_dir + os.chdir(gen_dir_location) + + ext_cost_fun.generate(fun_name, casadi_opts) + ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) + ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) + + os.chdir(cwd) + return diff --git a/pyextra/acados_template/generate_c_code_gnsf.py b/pyextra/acados_template/generate_c_code_gnsf.py new file mode 100644 index 00000000000000..97acb8e330f817 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_gnsf.py @@ -0,0 +1,131 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning + +def generate_c_code_gnsf( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + model_name = model.name + code_export_dir = opts["code_export_directory"] + + # set up directory + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + # obtain gnsf dimensions + get_matrices_fun = model.get_matrices_fun + phi_fun = model.phi_fun + + size_gnsf_A = get_matrices_fun.size_out(0) + gnsf_nx1 = size_gnsf_A[1] + gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + gnsf_nuhat = max(phi_fun.size_in(1)) + gnsf_ny = max(phi_fun.size_in(0)) + gnsf_nout = max(phi_fun.size_out(0)) + + # set up expressions + # if the model uses MX because of cost/constraints + # the DAE can be exported as SX -> detect GNSF in Matlab + # -> evaluated SX GNSF functions with MX. + u = model.u + + if isinstance(u, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + y = symbol("y", gnsf_ny, 1) + uhat = symbol("uhat", gnsf_nuhat, 1) + p = model.p + x1 = symbol("gnsf_x1", gnsf_nx1, 1) + x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) + z1 = symbol("gnsf_z1", gnsf_nz1, 1) + dummy = symbol("gnsf_dummy", 1, 1) + empty_var = symbol("gnsf_empty_var", 0, 0) + + ## generate C code + fun_name = model_name + '_gnsf_phi_fun' + phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) + phi_fun_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_fun_jac_y' + phi_fun_jac_y = model.phi_fun_jac_y + phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) + phi_fun_jac_y_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_phi_jac_y_uhat' + phi_jac_y_uhat = model.phi_jac_y_uhat + phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) + phi_jac_y_uhat_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' + f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz + f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) + + # avoid codegeneration issue + if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): + f_lo_fun_jac_x1k1uz_eval = [empty_var] + + f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p], + f_lo_fun_jac_x1k1uz_eval) + f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts) + + fun_name = model_name + '_gnsf_get_matrices_fun' + get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1)) + get_matrices_fun_.generate(fun_name, casadi_opts) + + # remove fields for json dump + del model.phi_fun + del model.phi_fun_jac_y + del model.phi_jac_y_uhat + del model.f_lo_fun_jac_x1k1uz + del model.get_matrices_fun + + os.chdir(cwd) + + return diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py new file mode 100644 index 00000000000000..f2d50b43abbb7a --- /dev/null +++ b/pyextra/acados_template/generate_c_code_implicit_ode.py @@ -0,0 +1,139 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning + +def generate_c_code_implicit_ode( model, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + generate_hess = opts["generate_hess"] + code_export_dir = opts["code_export_directory"] + + ## load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + f_impl = model.f_impl_expr + model_name = model.name + + ## get model dimensions + nx = casadi_length(x) + nu = casadi_length(u) + nz = casadi_length(z) + + ## generate jacobians + jac_x = jacobian(f_impl, x) + jac_xdot = jacobian(f_impl, xdot) + jac_u = jacobian(f_impl, u) + jac_z = jacobian(f_impl, z) + + ## generate hessian + x_xdot_z_u = vertcat(x, xdot, z, u) + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + multiplier = symbol('multiplier', nx + nz) + + ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True) + HESS = jacobian(ADJ, x_xdot_z_u) + + ## Set up functions + p = model.p + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) + + # fun_name = model_name + '_impl_dae_jac_x_xdot_u' + # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) + + + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) + + # generate C code + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + model_dir = model_name + '_model' + if not os.path.exists(model_dir): + os.mkdir(model_dir) + model_dir_location = os.path.join('.', model_dir) + os.chdir(model_dir_location) + + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' + impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' + impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' + impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts) + + if generate_hess: + fun_name = model_name + '_impl_dae_hess' + impl_dae_hess.generate(fun_name, casadi_opts) + + os.chdir(cwd) diff --git a/pyextra/acados_template/generate_c_code_nls_cost.py b/pyextra/acados_template/generate_c_code_nls_cost.py new file mode 100644 index 00000000000000..ffcd78ca7bf785 --- /dev/null +++ b/pyextra/acados_template/generate_c_code_nls_cost.py @@ -0,0 +1,113 @@ +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os +from casadi import * +from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning + +def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): + + casadi_version = CasadiMeta.version() + casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') + + if casadi_version not in (ALLOWED_CASADI_VERSIONS): + casadi_version_warning(casadi_version) + + x = model.x + p = model.p + + if isinstance(x, casadi.MX): + symbol = MX.sym + else: + symbol = SX.sym + + if stage_type == 'terminal': + middle_name = '_cost_y_e' + u = symbol('u', 0, 0) + cost_expr = model.cost_y_expr_e + + elif stage_type == 'initial': + middle_name = '_cost_y_0' + u = model.u + cost_expr = model.cost_y_expr_0 + + elif stage_type == 'path': + middle_name = '_cost_y' + u = model.u + cost_expr = model.cost_y_expr + + # set up directory + code_export_dir = opts["code_export_directory"] + if not os.path.exists(code_export_dir): + os.makedirs(code_export_dir) + + cwd = os.getcwd() + os.chdir(code_export_dir) + gen_dir = cost_name + '_cost' + if not os.path.exists(gen_dir): + os.mkdir(gen_dir) + gen_dir_location = os.path.join('.', gen_dir) + os.chdir(gen_dir_location) + + # set up expressions + cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x))) + + ny = casadi_length(cost_expr) + + y = symbol('y', ny, 1) + + y_adj = jtimes(cost_expr, vertcat(u, x), y, True) + y_hess = jacobian(y_adj, vertcat(u, x)) + + ## generate C code + suffix_name = '_fun' + fun_name = cost_name + middle_name + suffix_name + y_fun = Function( fun_name, [x, u, p], \ + [ cost_expr ]) + y_fun.generate( fun_name, casadi_opts ) + + suffix_name = '_fun_jac_ut_xt' + fun_name = cost_name + middle_name + suffix_name + y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \ + [ cost_expr, cost_jac_expr ]) + y_fun_jac_ut_xt.generate( fun_name, casadi_opts ) + + suffix_name = '_hess' + fun_name = cost_name + middle_name + suffix_name + y_hess = Function(fun_name, [x, u, y, p], [ y_hess ]) + y_hess.generate( fun_name, casadi_opts ) + + os.chdir(cwd) + + return + diff --git a/third_party/acados/acados_template/simulink_default_opts.json b/pyextra/acados_template/simulink_default_opts.json similarity index 89% rename from third_party/acados/acados_template/simulink_default_opts.json rename to pyextra/acados_template/simulink_default_opts.json index 5d178fef85309c..70c939cb889e01 100644 --- a/third_party/acados/acados_template/simulink_default_opts.json +++ b/pyextra/acados_template/simulink_default_opts.json @@ -4,9 +4,7 @@ "utraj": 0, "xtraj": 0, "solver_status": 1, - "cost_value": 0, "KKT_residual": 1, - "KKT_residuals": 0, "x1": 1, "CPU_time": 1, "CPU_time_sim": 0, @@ -31,8 +29,6 @@ "ug": 1, "lh": 1, "uh": 1, - "lh_e": 1, - "uh_e": 1, "cost_W_0": 0, "cost_W": 0, "cost_W_e": 0, diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py new file mode 100644 index 00000000000000..8f44f61e7e503e --- /dev/null +++ b/pyextra/acados_template/utils.py @@ -0,0 +1,440 @@ +# -*- coding: future_fstrings -*- +# +# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, +# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, +# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, +# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import os, sys, json +import urllib.request +import shutil +import numpy as np +from casadi import SX, MX, DM, Function, CasadiMeta + +ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0') + +TERA_VERSION = "0.0.34" + +def get_acados_path(): + ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR') + if not ACADOS_PATH: + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + acados_path = os.path.join(acados_template_path, '..','..','..') + ACADOS_PATH = os.path.realpath(acados_path) + msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, ' + msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH) + msg += 'Please export ACADOS_SOURCE_DIR to avoid this warning.' + print(msg) + return ACADOS_PATH + + +def get_python_interface_path(): + ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH') + if not ACADOS_PYTHON_INTERFACE_PATH: + acados_path = get_acados_path() + ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template') + return ACADOS_PYTHON_INTERFACE_PATH + + +def get_tera_exec_path(): + TERA_PATH = os.environ.get('TERA_PATH') + if not TERA_PATH: + TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') + if os.name == 'nt': + TERA_PATH += '.exe' + return TERA_PATH + + +platform2tera = { + "linux": "linux", + "darwin": "osx", + "win32": "windows" +} + + +def casadi_version_warning(casadi_version): + msg = 'Warning: Please note that the following versions of CasADi are ' + msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) + msg += 'If there is an incompatibility with the CasADi generated code, ' + msg += 'please consider changing your CasADi version.\n' + msg += 'Version {} currently in use.'.format(casadi_version) + print(msg) + + +def is_column(x): + if isinstance(x, np.ndarray): + if x.ndim == 1: + return True + elif x.ndim == 2 and x.shape[1] == 1: + return True + else: + return False + elif isinstance(x, (MX, SX, DM)): + if x.shape[1] == 1: + return True + elif x.shape[0] == 0 and x.shape[1] == 0: + return True + else: + return False + elif x == None or x == []: + return False + else: + raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def is_empty(x): + if isinstance(x, (MX, SX, DM)): + return x.is_empty() + elif isinstance(x, np.ndarray): + if np.prod(x.shape) == 0: + return True + else: + return False + elif x == None or x == []: + return True + else: + raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, " + + "None, numpy array empty list. Got: " + str(type(x))) + + +def casadi_length(x): + if isinstance(x, (MX, SX, DM)): + return int(np.prod(x.shape)) + else: + raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX." + + " Got: " + str(type(x))) + + +def make_model_consistent(model): + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + if isinstance(x, MX): + symbol = MX.sym + elif isinstance(x, SX): + symbol = SX.sym + else: + raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x))) + + if is_empty(p): + model.p = symbol('p', 0, 0) + + if is_empty(z): + model.z = symbol('z', 0, 0) + + return model + + +def get_tera(): + tera_path = get_tera_exec_path() + acados_path = get_acados_path() + + if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): + return tera_path + + repo_url = "https://github.com/acados/tera_renderer/releases" + url = "{}/download/v{}/t_renderer-v{}-{}".format( + repo_url, TERA_VERSION, TERA_VERSION, platform2tera[sys.platform]) + + manual_install = 'For manual installation follow these instructions:\n' + manual_install += '1 Download binaries from {}\n'.format(url) + manual_install += '2 Copy them in {}/bin\n'.format(acados_path) + manual_install += '3 Strip the version and platform from the binaries: ' + manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n' + manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' + manual_install += '"chmod +x {}"\n\n'.format(tera_path) + + msg = "\n" + msg += 'Tera template render executable not found, ' + msg += 'while looking in path:\n{}\n'.format(tera_path) + msg += 'In order to be able to render the templates, ' + msg += 'you need to download the tera renderer binaries from:\n' + msg += '{}\n\n'.format(repo_url) + msg += 'Do you wish to set up Tera renderer automatically?\n' + msg += 'y/N? (press y to download tera or any key for manual installation)\n' + + if input(msg) == 'y': + print("Dowloading {}".format(url)) + with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: + shutil.copyfileobj(response, out_file) + print("Successfully downloaded t_renderer.") + os.chmod(tera_path, 0o755) + return tera_path + + msg_cancel = "\nYou cancelled automatic download.\n\n" + msg_cancel += manual_install + msg_cancel += "Once installed re-run your script.\n\n" + print(msg_cancel) + + sys.exit(1) + + +def render_template(in_file, out_file, template_dir, json_path): + cwd = os.getcwd() + if not os.path.exists(template_dir): + os.mkdir(template_dir) + os.chdir(template_dir) + + tera_path = get_tera() + + # setting up loader and environment + acados_path = os.path.dirname(os.path.abspath(__file__)) + template_glob = os.path.join(acados_path, 'c_templates_tera', '*') + + # call tera as system cmd + os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'" + # Windows cmd.exe can not cope with '...', so use "..." instead: + if os.name == 'nt': + os_cmd = os_cmd.replace('\'', '\"') + + status = os.system(os_cmd) + if (status != 0): + raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\nExiting.\n') + + os.chdir(cwd) + + +## Conversion functions +def np_array_to_list(np_array): + if isinstance(np_array, (np.ndarray)): + return np_array.tolist() + elif isinstance(np_array, (SX)): + return DM(np_array).full() + elif isinstance(np_array, (DM)): + return np_array.full() + else: + raise(Exception(f"Cannot convert to list type {type(np_array)}")) + + +def format_class_dict(d): + """ + removes the __ artifact from class to dict conversion + """ + out = {} + for k, v in d.items(): + if isinstance(v, dict): + v = format_class_dict(v) + + out_key = k.split('__', 1)[-1] + out[k.replace(k, out_key)] = v + return out + + +def get_ocp_nlp_layout(): + python_interface_path = get_python_interface_path() + abs_path = os.path.join(python_interface_path, 'acados_layout.json') + with open(abs_path, 'r') as f: + ocp_nlp_layout = json.load(f) + return ocp_nlp_layout + + +def ocp_check_against_layout(ocp_nlp, ocp_dims): + """ + Check dimensions against layout + Parameters + --------- + ocp_nlp : dict + dictionary loaded from JSON to be post-processed. + + ocp_dims : instance of AcadosOcpDims + """ + + ocp_nlp_layout = get_ocp_nlp_layout() + + ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout) + return + + +def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout): + + for key, item in ocp_nlp.items(): + + try: + layout_of_key = layout[key] + except KeyError: + raise Exception("ocp_check_against_layout_recursion: field" \ + " '{0}' is not in layout but in OCP description.".format(key)) + + if isinstance(item, dict): + ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key) + + if 'ndarray' in layout_of_key: + if isinstance(item, int) or isinstance(item, float): + item = np.array([item]) + if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'): + dim_layout = [] + dim_names = layout_of_key[1] + + for dim_name in dim_names: + dim_layout.append(ocp_dims[dim_name]) + + dims = tuple(dim_layout) + + item = np.array(item) + item_dims = item.shape + if len(item_dims) != len(dims): + raise Exception('Mismatching dimensions for field {0}. ' \ + 'Expected {1} dimensional array, got {2} dimensional array.' \ + .format(key, len(dims), len(item_dims))) + + if np.prod(item_dims) != 0 or np.prod(dims) != 0: + if dims != item_dims: + raise Exception('acados -- mismatching dimensions for field {0}. ' \ + 'Provided data has dimensions {1}, ' \ + 'while associated dimensions {2} are {3}' \ + .format(key, item_dims, dim_names, dims)) + return + + +def J_to_idx(J): + nrows = J.shape[0] + idx = np.zeros((nrows, )) + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) != 1: + raise Exception('Invalid J matrix structure detected, ' \ + 'must contain one nonzero element per row. Exiting.') + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J matrices can only contain 1s. Exiting.') + idx[i] = this_idx[0] + return idx + + +def J_to_idx_slack(J): + nrows = J.shape[0] + ncol = J.shape[1] + idx = np.zeros((ncol, )) + i_idx = 0 + for i in range(nrows): + this_idx = np.nonzero(J[i,:])[0] + if len(this_idx) == 1: + idx[i_idx] = i + i_idx = i_idx + 1 + elif len(this_idx) > 1: + raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \ + 'Found more than one nonzero in row ' + str(i)) + if this_idx.size > 0 and J[i,this_idx[0]] != 1: + raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \ + 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) + if not i_idx == ncol: + raise Exception('J_to_idx_slack: J must contain a 1 in every column!') + return idx + + +def acados_dae_model_json_dump(model): + + # load model + x = model.x + xdot = model.xdot + u = model.u + z = model.z + p = model.p + + f_impl = model.f_impl_expr + model_name = model.name + + # create struct with impl_dae_fun, casadi_version + fun_name = model_name + '_impl_dae_fun' + impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) + + casadi_version = CasadiMeta.version() + str_impl_dae_fun = impl_dae_fun.serialize() + + dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version} + + # dump + json_file = model_name + '_acados_dae.json' + with open(json_file, 'w') as f: + json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + print("dumped ", model_name, " dae to file:", json_file, "\n") + + +def set_up_imported_gnsf_model(acados_formulation): + + gnsf = acados_formulation.gnsf_model + + # check CasADi version + # dump_casadi_version = gnsf['casadi_version'] + # casadi_version = CasadiMeta.version() + + # if not casadi_version == dump_casadi_version: + # print("WARNING: GNSF model was dumped with another CasADi version.\n" + # + "This might yield errors. Please use the same version for compatibility, serialize version: " + # + dump_casadi_version + " current Python CasADi verison: " + casadi_version) + # input("Press any key to attempt to continue...") + + # load model + phi_fun = Function.deserialize(gnsf['phi_fun']) + phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y']) + phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat']) + get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun']) + + # obtain gnsf dimensions + size_gnsf_A = get_matrices_fun.size_out(0) + acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1] + acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] + acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1)) + acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0)) + acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0)) + + # save gnsf functions in model + acados_formulation.model.phi_fun = phi_fun + acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y + acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat + acados_formulation.model.get_matrices_fun = get_matrices_fun + + # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},... + # {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,... + # nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO}); + get_matrices_out = get_matrices_fun(0) + acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12]) + acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13]) + + if "f_lo_fun_jac_x1k1uz" in gnsf: + f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz']) + acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz + else: + dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1) + dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1) + dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1) + dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu) + dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np) + empty_var = SX.sym('empty_var', 0, 0) + + empty_fun = Function('empty_fun', \ + [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p], + [empty_var]) + acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun + + del acados_formulation.gnsf_model diff --git a/pyproject.toml b/pyproject.toml deleted file mode 100644 index 2400364c70653b..00000000000000 --- a/pyproject.toml +++ /dev/null @@ -1,254 +0,0 @@ -[project] -name = "openpilot" -requires-python = ">= 3.12.3, < 3.13" -license = {text = "MIT License"} -version = "0.1.0" -description = "an open source driver assistance system" -authors = [ - {name = "Vehicle Researcher", email="user@comma.ai"} -] - -dependencies = [ - # multiple users - "sounddevice", # micd + soundd - "pyserial", # pigeond + qcomgpsd - "requests", # many one-off uses - "sympy", # rednose + friends - "crcmod-plus", # cars + qcomgpsd - "tqdm", # cars (fw_versions.py) on start + many one-off uses - - # core - "cffi", - "scons", - "pycapnp==2.1.0", - "Cython", - "setuptools", - "numpy >=2.0", - - # body / webrtcd - "aiohttp", - "aiortc", - # aiortc does not put an upper bound on pyopenssl and is now incompatible - # with the latest release - "pyopenssl < 24.3.0", - "pyaudio", - - # panda - "libusb1", - "spidev; platform_system == 'Linux'", - - # modeld - "onnx >= 1.14.0", - - # logging - "pyzmq", - "sentry-sdk", - "xattr", # used in place of 'os.getxattr' for macOS compatibility - - # athena - "PyJWT", - "json-rpc", - "websocket_client", - - # acados deps - "casadi >=3.6.6", # 3.12 fixed in 3.6.6 - - # joystickd - "inputs", - - # these should be removed - "psutil", - "pycryptodome", # used in updated/casync, panda, body, and a test - "setproctitle", - - # logreader - "zstandard", - - # ui - "raylib > 5.5.0.3", - "qrcode", - "mapbox-earcut", - "jeepney", -] - -[project.optional-dependencies] -docs = [ - "Jinja2", - "mkdocs", -] - -testing = [ - "coverage", - "hypothesis ==6.47.*", - "ty", - "pytest", - "pytest-cpp", - "pytest-subtests", - # https://github.com/pytest-dev/pytest-xdist/pull/1229 - "pytest-xdist @ git+https://github.com/sshane/pytest-xdist@2b4372bd62699fb412c4fe2f95bf9f01bd2018da", - "pytest-timeout", - "pytest-asyncio", - "pytest-mock", - "pytest-repeat", - "ruff", - "codespell", - "pre-commit-hooks", -] - -dev = [ - "av", - "azure-identity", - "azure-storage-blob", - "dictdiffer", - "matplotlib", - "opencv-python-headless", - "parameterized >=0.8, <0.9", - "pyautogui", - "pywinctl", - "tabulate", -] - -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; (sys_platform != 'linux' or platform_machine != 'aarch64')", # not vended for linux aarch64 -] - -[project.urls] -Homepage = "https://github.com/commaai/openpilot" - -[build-system] -requires = ["hatchling"] -build-backend = "hatchling.build" - -[tool.hatch.build.targets.wheel] -packages = [ "." ] - -[tool.hatch.metadata] -allow-direct-references = true - -[tool.pytest.ini_options] -minversion = "6.0" -addopts = "--ignore=openpilot/ --ignore=opendbc/ --ignore=panda/ --ignore=rednose_repo/ --ignore=tinygrad_repo/ --ignore=teleoprtc_repo/ --ignore=msgq/ -Werror --strict-config --strict-markers --durations=10 -n auto --dist=loadgroup" -cpp_files = "test_*" -cpp_harness = "selfdrive/test/cpp_harness.py" -python_files = "test_*.py" -asyncio_default_fixture_loop_scope = "function" -#timeout = "30" # you get this long by default -markers = [ - "slow: tests that take awhile to run and can be skipped with -m 'not slow'", - "tici: tests that are only meant to run on the C3/C3X", - "skip_tici_setup: mark test to skip tici setup fixture" -] -testpaths = [ - "common", - "selfdrive", - "system", - "tools", - "cereal", -] - -[tool.codespell] -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/*, *.po, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*, tools/plotjuggler/layouts/*, selfdrive/assets/offroad/mici_fcc.html" - -# https://docs.astral.sh/ruff/configuration/#using-pyprojecttoml -[tool.ruff] -indent-width = 2 -lint.select = [ - "E", "F", "W", "PIE", "C4", "ISC", "A", "B", - "NPY", # numpy - "UP", # pyupgrade - "TRY203", "TRY400", "TRY401", # try/excepts - "RUF008", "RUF100", - "TID251", - "PLE", "PLR1704", -] -lint.ignore = [ - "E741", - "E402", - "C408", - "ISC003", - "B027", - "B024", - "NPY002", # new numpy random syntax is worse - "UP045", "UP007", # these don't play nice with raylib atm -] -line-length = 160 -target-version ="py311" -exclude = [ - "body", - "cereal", - "panda", - "opendbc", - "opendbc_repo", - "rednose_repo", - "tinygrad_repo", - "teleoprtc", - "teleoprtc_repo", - "third_party", - "*.ipynb", - "generated", -] -lint.flake8-implicit-str-concat.allow-multiline = false - -[tool.ruff.lint.flake8-tidy-imports.banned-api] -"selfdrive".msg = "Use openpilot.selfdrive" -"common".msg = "Use openpilot.common" -"system".msg = "Use openpilot.system" -"third_party".msg = "Use openpilot.third_party" -"tools".msg = "Use openpilot.tools" -"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!" -"unittest".msg = "Use pytest" -"time.time".msg = "Use time.monotonic" - -# raylib banned APIs -"pyray.measure_text_ex".msg = "Use openpilot.system.ui.lib.text_measure" -"pyray.is_mouse_button_pressed".msg = "This can miss events. Use Widget._handle_mouse_press" -"pyray.is_mouse_button_released".msg = "This can miss events. Use Widget._handle_mouse_release" -"pyray.draw_text".msg = "Use a function (such as rl.draw_font_ex) that takes font as an argument" - -[tool.ruff.format] -quote-style = "preserve" - -[tool.ty.src] -exclude = [ - "cereal/", - "msgq/", - "msgq_repo/", - "opendbc/", - "opendbc_repo/", - "panda/", - "rednose/", - "rednose_repo/", - "tinygrad/", - "tinygrad_repo/", - "teleoprtc/", - "teleoprtc_repo/", - "third_party/", -] - -[tool.ty.rules] -# Ignore unresolved imports for Cython-compiled modules (.pyx) -unresolved-import = "ignore" -# Ignore unresolved attributes - many from capnp and Cython modules -unresolved-attribute = "ignore" -# Ignore invalid method overrides - signature variance issues -invalid-method-override = "ignore" -# Ignore possibly-missing-attribute - too many false positives -possibly-missing-attribute = "ignore" -# Ignore invalid assignment - often intentional monkey-patching -invalid-assignment = "ignore" -# Ignore no-matching-overload - numpy/ctypes overload matching issues -no-matching-overload = "ignore" -# Ignore invalid-argument-type - many false positives from raylib, ctypes, numpy -invalid-argument-type = "ignore" -# Ignore call-non-callable - false positives from dynamic types -call-non-callable = "ignore" -# Ignore unsupported-operator - false positives from dynamic types -unsupported-operator = "ignore" -# Ignore not-subscriptable - false positives from dynamic types -not-subscriptable = "ignore" -# not-iterable errors are now fixed diff --git a/rednose_repo b/rednose_repo index 7fddc8e6d49def..3b6bd703b7a766 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 7fddc8e6d49def83c952a78673179bdc62789214 +Subproject commit 3b6bd703b7a7667e4f82d0b81ef9a454819b94bd diff --git a/release/README.md b/release/README.md deleted file mode 100644 index 7aeea9fe4aff6d..00000000000000 --- a/release/README.md +++ /dev/null @@ -1,35 +0,0 @@ -# openpilot releases - -``` -## release checklist - -### Go to staging -- [ ] make a GitHub issue to track release with this checklist -- [ ] create release master branch - - [ ] create a branch from upstream master named `zerotentwo` for release `v0.10.2` - - [ ] revert risky commits (double check with autonomy team) - - [ ] push the new branch -- [ ] push to staging: - - [ ] make sure you are on the newly created release master branch (`zerotentwo`) - - [ ] run `BRANCH=devel-staging release/build_stripped.sh`. Jenkins will then automatically build staging on device, run `test_onroad` and update the staging branch -- [ ] bump version on master: `common/version.h` and `RELEASES.md` -- [ ] post on Discord, tag `@release crew` - -### Go to release -- [ ] before going to release, test the following: - - [ ] update from previous release -> new release - - [ ] update from new release -> previous release - - [ ] fresh install with `openpilot-test.comma.ai` - - [ ] drive on fresh install - - [ ] no submodules or LFS - - [ ] check sentry, MTBF, etc. - - [ ] stress test passes in production -- [ ] publish the blog post -- [ ] `git reset --hard origin/release-mici-staging` -- [ ] tag the release: `git tag v0.X.X && git push origin v0.X.X` -- [ ] create GitHub release -- [ ] final test install on `openpilot.comma.ai` -- [ ] update factory provisioning -- [ ] close out milestone and issue -- [ ] post on Discord, X, etc. -``` diff --git a/release/build_devel.sh b/release/build_devel.sh new file mode 100755 index 00000000000000..f06e3102c82093 --- /dev/null +++ b/release/build_devel.sh @@ -0,0 +1,86 @@ +#!/usr/bin/bash + +set -ex + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + +SOURCE_DIR="$(git -C $DIR rev-parse --show-toplevel)" +if [ -z "$TARGET_DIR" ]; then + TARGET_DIR="$(mktemp -d)" +fi + +# set git identity +source $DIR/identity.sh + +echo "[-] Setting up repo T=$SECONDS" + +cd $SOURCE_DIR +git fetch origin + +rm -rf $TARGET_DIR +mkdir -p $TARGET_DIR +cd $TARGET_DIR +cp -r $SOURCE_DIR/.git $TARGET_DIR +pre-commit uninstall || true + +echo "[-] bringing master-ci and devel in sync T=$SECONDS" +cd $TARGET_DIR +git fetch origin master-ci +git fetch origin devel + +git checkout -f --track origin/master-ci +git reset --hard master-ci +git checkout master-ci +git reset --hard origin/devel +git clean -xdf +git lfs uninstall + +# remove everything except .git +echo "[-] erasing old openpilot T=$SECONDS" +find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; + +# reset source tree +cd $SOURCE_DIR +git clean -xdf + +# do the files copy +echo "[-] copying files T=$SECONDS" +cd $SOURCE_DIR +cp -pR --parents $(cat release/files_*) $TARGET_DIR/ +if [ ! -z "$EXTRA_FILES" ]; then + cp -pR --parents $EXTRA_FILES $TARGET_DIR/ +fi + +# in the directory +cd $TARGET_DIR +rm -f panda/board/obj/panda.bin.signed + +# include source commit hash and build date in commit +GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse HEAD) +DATETIME=$(date '+%Y-%m-%dT%H:%M:%S') +VERSION=$(cat $SOURCE_DIR/common/version.h | awk -F\" '{print $2}') + +echo "[-] committing version $VERSION T=$SECONDS" +git add -f . +git status +git commit -a -m "openpilot v$VERSION release + +date: $DATETIME +master commit: $GIT_HASH +" + +# ensure files are within GitHub's limit +BIG_FILES="$(find . -type f -not -path './.git/*' -size +95M)" +if [ ! -z "$BIG_FILES" ]; then + printf '\n\n\n' + echo "Found files exceeding GitHub's 100MB limit:" + echo "$BIG_FILES" + exit 1 +fi + +if [ ! -z "$BRANCH" ]; then + echo "[-] Pushing to $BRANCH T=$SECONDS" + git push -f origin master-ci:$BRANCH +fi + +echo "[-] done T=$SECONDS, ready at $TARGET_DIR" diff --git a/release/build_release.sh b/release/build_release.sh index 220da05c17d430..80106eefb2336f 100755 --- a/release/build_release.sh +++ b/release/build_release.sh @@ -1,6 +1,4 @@ -#!/usr/bin/env bash -set -e -set -x +#!/usr/bin/bash -e # git diff --name-status origin/release3-staging | grep "^A" | less @@ -11,14 +9,17 @@ cd $DIR BUILD_DIR=/data/openpilot SOURCE_DIR="$(git rev-parse --show-toplevel)" -if [ -z "$RELEASE_BRANCH" ]; then - echo "RELEASE_BRANCH is not set" - exit 1 +if [ -f /TICI ]; then + FILES_SRC="release/files_tici" + RELEASE_BRANCH=release3-staging + DASHCAM_BRANCH=dashcam3-staging +else + exit 0 fi - # set git identity source $DIR/identity.sh +export GIT_SSH_COMMAND="ssh -i /data/gitkey" echo "[-] Setting up repo T=$SECONDS" rm -rf $BUILD_DIR @@ -26,12 +27,14 @@ mkdir -p $BUILD_DIR cd $BUILD_DIR git init git remote add origin git@github.com:commaai/openpilot.git +git fetch origin $RELEASE_BRANCH git checkout --orphan $RELEASE_BRANCH # do the files copy echo "[-] copying files T=$SECONDS" cd $SOURCE_DIR -cp -pR --parents $(./release/release_files.py) $BUILD_DIR/ +cp -pR --parents $(cat release/files_common) $BUILD_DIR/ +cp -pR --parents $(cat $FILES_SRC) $BUILD_DIR/ # in the directory cd $BUILD_DIR @@ -40,21 +43,23 @@ rm -f panda/board/obj/panda.bin.signed rm -f panda/board/obj/panda_h7.bin.signed VERSION=$(cat common/version.h | awk -F[\"-] '{print $2}') +echo "#define COMMA_VERSION \"$VERSION-release\"" > common/version.h + echo "[-] committing version $VERSION T=$SECONDS" git add -f . git commit -a -m "openpilot v$VERSION release" +git branch --set-upstream-to=origin/$RELEASE_BRANCH + +# Build panda firmware +pushd panda/ +CERT=/data/pandaextra/certs/release RELEASE=1 scons -u . +mv board/obj/panda.bin.signed /tmp/panda.bin.signed +mv board/obj/panda_h7.bin.signed /tmp/panda_h7.bin.signed +popd # Build export PYTHONPATH="$BUILD_DIR" -scons -j$(nproc) --minimal - -if [ -z "$PANDA_DEBUG_BUILD" ]; then - # release panda fw - CERT=/data/pandaextra/certs/release RELEASE=1 scons -j$(nproc) panda/ -else - # build with ALLOW_DEBUG=1 to enable features like experimental longitudinal - scons -j$(nproc) panda/ -fi +scons -j$(nproc) # Ensure no submodules in release if test "$(git submodule--helper list | wc -l)" -gt "0"; then @@ -71,13 +76,14 @@ find . -name '*.os' -delete find . -name '*.pyc' -delete find . -name 'moc_*' -delete find . -name '__pycache__' -delete +rm -rf panda/board panda/certs panda/crypto rm -rf .sconsign.dblite Jenkinsfile release/ -rm selfdrive/modeld/models/driving_vision.onnx -rm selfdrive/modeld/models/driving_policy.onnx - -find third_party/ -name '*x86*' -exec rm -r {} + -find third_party/ -name '*Darwin*' -exec rm -r {} + +rm selfdrive/modeld/models/supercombo.onnx +# Move back signed panda fw +mkdir -p panda/board/obj +mv /tmp/panda.bin.signed panda/board/obj/panda.bin.signed +mv /tmp/panda_h7.bin.signed panda/board/obj/panda_h7.bin.signed # Restore third_party git checkout third_party/ @@ -90,13 +96,23 @@ git add -f . git commit --amend -m "openpilot v$VERSION" # Run tests +TEST_FILES="tools/" +cd $SOURCE_DIR +cp -pR -n --parents $TEST_FILES $BUILD_DIR/ cd $BUILD_DIR -RELEASE=1 pytest -n0 -s selfdrive/test/test_onroad.py -#pytest selfdrive/car/tests/test_car_interfaces.py - -if [ ! -z "$RELEASE_BRANCH" ]; then - echo "[-] pushing release T=$SECONDS" - git push -f origin $RELEASE_BRANCH:$RELEASE_BRANCH +RELEASE=1 selfdrive/test/test_onroad.py +#selfdrive/manager/test/test_manager.py +selfdrive/car/tests/test_car_interfaces.py +rm -rf $TEST_FILES + +if [ ! -z "$PUSH" ]; then + echo "[-] pushing T=$SECONDS" + git push -f origin $RELEASE_BRANCH + + # Create dashcam + git rm selfdrive/car/*/carcontroller.py + git commit -m "create dashcam release from release" + git push -f origin $RELEASE_BRANCH:$DASHCAM_BRANCH fi echo "[-] done T=$SECONDS" diff --git a/release/build_stripped.sh b/release/build_stripped.sh deleted file mode 100755 index 6f1a568c25ebfd..00000000000000 --- a/release/build_stripped.sh +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env bash -set -ex - -DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" - -SOURCE_DIR="$(git -C $DIR rev-parse --show-toplevel)" -if [ -z "$TARGET_DIR" ]; then - TARGET_DIR="$(mktemp -d)" -fi - -# set git identity -source $DIR/identity.sh - -echo "[-] Setting up target repo T=$SECONDS" - -rm -rf $TARGET_DIR -mkdir -p $TARGET_DIR -cd $TARGET_DIR -cp -r $SOURCE_DIR/.git $TARGET_DIR - -echo "[-] setting up stripped branch sync T=$SECONDS" -cd $TARGET_DIR - -# tmp branch -git checkout --orphan tmp - -# remove everything except .git -echo "[-] erasing old openpilot T=$SECONDS" -git submodule deinit -f --all -git rm -rf --cached . -find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; - -# cleanup before the copy -cd $SOURCE_DIR -git clean -xdff -git submodule foreach --recursive git clean -xdff - -# do the files copy -echo "[-] copying files T=$SECONDS" -cd $SOURCE_DIR -cp -pR --parents $(./release/release_files.py) $TARGET_DIR/ - -# in the directory -cd $TARGET_DIR -rm -rf .git/modules/ -rm -f panda/board/obj/panda.bin.signed - -# include source commit hash and build date in commit -GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse HEAD) -GIT_COMMIT_DATE=$(git --git-dir=$SOURCE_DIR/.git show --no-patch --format='%ct %ci' HEAD) -DATETIME=$(date '+%Y-%m-%dT%H:%M:%S') -VERSION=$(cat $SOURCE_DIR/common/version.h | awk -F\" '{print $2}') - -echo -n "$GIT_HASH" > git_src_commit -echo -n "$GIT_COMMIT_DATE" > git_src_commit_date - -echo "[-] committing version $VERSION T=$SECONDS" -git add -f . -git status -git commit -a -m "openpilot v$VERSION release - -date: $DATETIME -master commit: $GIT_HASH -" - -# should be no submodules or LFS files -git submodule status -if [ ! -z "$(git lfs ls-files)" ]; then - echo "LFS files detected!" - exit 1 -fi - -# ensure files are within GitHub's limit -BIG_FILES="$(find . -type f -not -path './.git/*' -size +95M)" -if [ ! -z "$BIG_FILES" ]; then - printf '\n\n\n' - echo "Found files exceeding GitHub's 100MB limit:" - echo "$BIG_FILES" - exit 1 -fi - -if [ ! -z "$BRANCH" ]; then - echo "[-] Pushing to $BRANCH T=$SECONDS" - git push -f origin tmp:$BRANCH -fi - -echo "[-] done T=$SECONDS, ready at $TARGET_DIR" diff --git a/release/check-dirty.sh b/release/check-dirty.sh index ac049970cf06bc..9c6389f3801ac6 100755 --- a/release/check-dirty.sh +++ b/release/check-dirty.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash set -e DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" diff --git a/release/check-submodules.sh b/release/check-submodules.sh index 93869a740347d9..182042e6b4918d 100755 --- a/release/check-submodules.sh +++ b/release/check-submodules.sh @@ -1,11 +1,6 @@ -#!/usr/bin/env bash +#!/bin/bash while read hash submodule ref; do - if [ "$submodule" = "tinygrad_repo" ]; then - echo "Skipping $submodule" - continue - fi - git -C $submodule fetch --depth 100 origin master git -C $submodule branch -r --contains $hash | grep "origin/master" if [ "$?" -eq 0 ]; then diff --git a/release/files_common b/release/files_common new file mode 100644 index 00000000000000..be0a4f0f03ba9e --- /dev/null +++ b/release/files_common @@ -0,0 +1,573 @@ +.gitignore +LICENSE +launch_env.sh +launch_chffrplus.sh +launch_openpilot.sh + +Jenkinsfile +SConstruct + +README.md +RELEASES.md +docs/CARS.md +docs/CONTRIBUTING.md +docs/INTEGRATION.md +docs/LIMITATIONS.md +site_scons/site_tools/cython.py + +common/.gitignore +common/__init__.py +common/conversions.py +common/gpio.py +common/realtime.py +common/clock.pyx +common/timeout.py +common/ffi_wrapper.py +common/file_helpers.py +common/logging_extra.py +common/numpy_fast.py +common/params.py +common/params_pyx.pyx +common/profiler.py +common/basedir.py +common/dict_helpers.py +common/filter_simple.py +common/stat_live.py +common/spinner.py +common/text_window.py + +common/kalman/.gitignore +common/kalman/* + +common/transformations/__init__.py +common/transformations/camera.py +common/transformations/model.py + +common/transformations/SConscript +common/transformations/coordinates.py +common/transformations/coordinates.cc +common/transformations/coordinates.hpp +common/transformations/orientation.py +common/transformations/orientation.cc +common/transformations/orientation.hpp +common/transformations/transformations.pxd +common/transformations/transformations.pyx + +common/api/__init__.py + +release/* + +tools/__init__.py +tools/lib/* +tools/joystick/* +tools/replay/*.cc +tools/replay/*.h + +selfdrive/__init__.py +selfdrive/sentry.py +selfdrive/tombstoned.py +selfdrive/updated.py +selfdrive/rtshield.py +selfdrive/statsd.py + +system/logmessaged.py +system/swaglog.py +system/version.py + +selfdrive/athena/__init__.py +selfdrive/athena/athenad.py +selfdrive/athena/manage_athenad.py +selfdrive/athena/registration.py + +selfdrive/boardd/.gitignore +selfdrive/boardd/SConscript +selfdrive/boardd/__init__.py +selfdrive/boardd/boardd.cc +selfdrive/boardd/boardd.h +selfdrive/boardd/main.cc +selfdrive/boardd/boardd.py +selfdrive/boardd/boardd_api_impl.pyx +selfdrive/boardd/can_list_to_can_capnp.cc +selfdrive/boardd/panda.cc +selfdrive/boardd/panda.h +selfdrive/boardd/set_time.py +selfdrive/boardd/pandad.py + +selfdrive/car/__init__.py +selfdrive/car/docs_definitions.py +selfdrive/car/car_helpers.py +selfdrive/car/fingerprints.py +selfdrive/car/interfaces.py +selfdrive/car/vin.py +selfdrive/car/disable_ecu.py +selfdrive/car/fw_versions.py +selfdrive/car/fw_query_definitions.py +selfdrive/car/ecu_addrs.py +selfdrive/car/isotp_parallel_query.py +selfdrive/car/tests/__init__.py +selfdrive/car/tests/test_car_interfaces.py +selfdrive/car/torque_data/params.yaml +selfdrive/car/torque_data/substitute.yaml +selfdrive/car/torque_data/override.yaml + +selfdrive/car/body/*.py +selfdrive/car/chrysler/*.py +selfdrive/car/ford/*.py +selfdrive/car/gm/*.py +selfdrive/car/honda/*.py +selfdrive/car/hyundai/*.py +selfdrive/car/mazda/*.py +selfdrive/car/mock/*.py +selfdrive/car/nissan/*.py +selfdrive/car/subaru/*.py +selfdrive/car/tesla/*.py +selfdrive/car/toyota/*.py +selfdrive/car/volkswagen/*.py + +system/clocksd/.gitignore +system/clocksd/SConscript +system/clocksd/clocksd.cc + +selfdrive/debug/can_printer.py +selfdrive/debug/check_freq.py +selfdrive/debug/dump.py +selfdrive/debug/filter_log_message.py +selfdrive/debug/get_fingerprint.py +selfdrive/debug/uiview.py + +selfdrive/debug/hyundai_enable_radar_points.py +selfdrive/debug/vw_mqb_config.py + +common/SConscript +common/version.h + +common/swaglog.h +common/swaglog.cc +common/statlog.h +common/statlog.cc +common/util.cc +common/util.h +common/queue.h +common/clutil.cc +common/clutil.h +common/params.h +common/params.cc +common/watchdog.cc +common/watchdog.h + +common/modeldata.h +common/mat.h +common/timing.h + +common/gpio.cc +common/gpio.h +common/i2c.cc +common/i2c.h + +selfdrive/controls/__init__.py +selfdrive/controls/controlsd.py +selfdrive/controls/plannerd.py +selfdrive/controls/radard.py +selfdrive/controls/lib/__init__.py +selfdrive/controls/lib/alertmanager.py +selfdrive/controls/lib/alerts_offroad.json +selfdrive/controls/lib/desire_helper.py +selfdrive/controls/lib/drive_helpers.py +selfdrive/controls/lib/events.py +selfdrive/controls/lib/latcontrol_angle.py +selfdrive/controls/lib/latcontrol_indi.py +selfdrive/controls/lib/latcontrol_torque.py +selfdrive/controls/lib/latcontrol_pid.py +selfdrive/controls/lib/latcontrol.py +selfdrive/controls/lib/lateral_planner.py +selfdrive/controls/lib/longcontrol.py +selfdrive/controls/lib/longitudinal_planner.py +selfdrive/controls/lib/pid.py +selfdrive/controls/lib/radar_helpers.py +selfdrive/controls/lib/vehicle_model.py + +selfdrive/controls/lib/cluster/* + +selfdrive/controls/lib/lateral_mpc_lib/.gitignore +selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore +selfdrive/controls/lib/lateral_mpc_lib/* +selfdrive/controls/lib/longitudinal_mpc_lib/* + +selfdrive/hardware + +system/__init__.py + +system/hardware/__init__.py +system/hardware/base.h +system/hardware/base.py +system/hardware/hw.h +system/hardware/tici/__init__.py +system/hardware/tici/hardware.h +system/hardware/tici/hardware.py +system/hardware/tici/pins.py +system/hardware/tici/agnos.py +system/hardware/tici/casync.py +system/hardware/tici/agnos.json +system/hardware/tici/amplifier.py +system/hardware/tici/updater +system/hardware/tici/iwlist.py +system/hardware/pc/__init__.py +system/hardware/pc/hardware.py + +selfdrive/locationd/__init__.py +selfdrive/locationd/.gitignore +selfdrive/locationd/SConscript +selfdrive/locationd/ubloxd.cc +selfdrive/locationd/ublox_msg.cc +selfdrive/locationd/ublox_msg.h +selfdrive/locationd/generated/ubx.cpp +selfdrive/locationd/generated/ubx.h +selfdrive/locationd/generated/gps.cpp +selfdrive/locationd/generated/gps.h + +selfdrive/locationd/laikad.py +selfdrive/locationd/laikad_helpers.py +selfdrive/locationd/locationd.h +selfdrive/locationd/locationd.cc +selfdrive/locationd/paramsd.py +selfdrive/locationd/models/__init__.py +selfdrive/locationd/models/.gitignore +selfdrive/locationd/models/car_kf.py +selfdrive/locationd/models/gnss_kf.py +selfdrive/locationd/models/live_kf.py +selfdrive/locationd/models/live_kf.h +selfdrive/locationd/models/live_kf.cc +selfdrive/locationd/models/constants.py +selfdrive/locationd/models/gnss_helpers.py + +selfdrive/locationd/calibrationd.py + +system/logcatd/.gitignore +system/logcatd/SConscript +system/logcatd/logcatd_systemd.cc + +system/proclogd/SConscript +system/proclogd/main.cc +system/proclogd/proclog.cc +system/proclogd/proclog.h + +selfdrive/loggerd/.gitignore +selfdrive/loggerd/SConscript +selfdrive/loggerd/encoder/encoder.cc +selfdrive/loggerd/encoder/encoder.h +selfdrive/loggerd/encoder/v4l_encoder.cc +selfdrive/loggerd/encoder/v4l_encoder.h +selfdrive/loggerd/video_writer.cc +selfdrive/loggerd/video_writer.h +selfdrive/loggerd/logger.cc +selfdrive/loggerd/logger.h +selfdrive/loggerd/loggerd.cc +selfdrive/loggerd/loggerd.h +selfdrive/loggerd/encoderd.cc +selfdrive/loggerd/bootlog.cc +selfdrive/loggerd/encoder/ffmpeg_encoder.cc +selfdrive/loggerd/encoder/ffmpeg_encoder.h + +selfdrive/loggerd/__init__.py +selfdrive/loggerd/config.py +selfdrive/loggerd/uploader.py +selfdrive/loggerd/deleter.py +selfdrive/loggerd/xattr_cache.py + +selfdrive/sensord/SConscript +selfdrive/sensord/libdiag.h +selfdrive/sensord/sensors_qcom2.cc +selfdrive/sensord/sensors/*.cc +selfdrive/sensord/sensors/*.h +selfdrive/sensord/sensord +selfdrive/sensord/pigeond.py + +selfdrive/thermald/thermald.py +selfdrive/thermald/power_monitoring.py +selfdrive/thermald/fan_controller.py + +selfdrive/test/__init__.py +selfdrive/test/helpers.py +selfdrive/test/setup_device_ci.sh +selfdrive/test/test_onroad.py + +selfdrive/ui/.gitignore +selfdrive/ui/SConscript +selfdrive/ui/*.cc +selfdrive/ui/*.h +selfdrive/ui/ui +selfdrive/ui/text +selfdrive/ui/spinner +selfdrive/ui/soundd/*.cc +selfdrive/ui/soundd/*.h +selfdrive/ui/soundd/soundd +selfdrive/ui/soundd/.gitignore +selfdrive/ui/translations/*.ts +selfdrive/ui/translations/languages.json + +selfdrive/ui/qt/*.cc +selfdrive/ui/qt/*.h +selfdrive/ui/qt/offroad/*.cc +selfdrive/ui/qt/offroad/*.h +selfdrive/ui/qt/offroad/*.qml +selfdrive/ui/qt/widgets/*.cc +selfdrive/ui/qt/widgets/*.h +selfdrive/ui/qt/maps/*.cc +selfdrive/ui/qt/maps/*.h + +system/camerad/SConscript +system/camerad/main.cc + +system/camerad/snapshot/* +system/camerad/include/* +system/camerad/cameras/camera_common.h +system/camerad/cameras/camera_common.cc +system/camerad/cameras/sensor2_i2c.h + +system/camerad/imgproc/conv.cl +system/camerad/imgproc/pool.cl +system/camerad/imgproc/utils.cc +system/camerad/imgproc/utils.h + +selfdrive/manager/__init__.py +selfdrive/manager/build.py +selfdrive/manager/helpers.py +selfdrive/manager/manager.py +selfdrive/manager/process_config.py +selfdrive/manager/process.py +selfdrive/manager/test/__init__.py +selfdrive/manager/test/test_manager.py + +selfdrive/modeld/__init__.py +selfdrive/modeld/SConscript +selfdrive/modeld/modeld.cc +selfdrive/modeld/dmonitoringmodeld.cc +selfdrive/modeld/constants.py +selfdrive/modeld/modeld +selfdrive/modeld/dmonitoringmodeld + +selfdrive/modeld/models/commonmodel.cc +selfdrive/modeld/models/commonmodel.h +selfdrive/modeld/models/driving.cc +selfdrive/modeld/models/driving.h +selfdrive/modeld/models/dmonitoring.cc +selfdrive/modeld/models/dmonitoring.h +selfdrive/modeld/models/supercombo.onnx +selfdrive/modeld/models/dmonitoring_model_q.dlc + +selfdrive/modeld/transforms/loadyuv.cc +selfdrive/modeld/transforms/loadyuv.h +selfdrive/modeld/transforms/loadyuv.cl +selfdrive/modeld/transforms/transform.cc +selfdrive/modeld/transforms/transform.h +selfdrive/modeld/transforms/transform.cl + +selfdrive/modeld/thneed/*.py +selfdrive/modeld/thneed/thneed.h +selfdrive/modeld/thneed/thneed_common.cc +selfdrive/modeld/thneed/thneed_qcom2.cc +selfdrive/modeld/thneed/serialize.cc +selfdrive/modeld/thneed/include/* + +selfdrive/modeld/runners/snpemodel.cc +selfdrive/modeld/runners/snpemodel.h +selfdrive/modeld/runners/thneedmodel.cc +selfdrive/modeld/runners/thneedmodel.h +selfdrive/modeld/runners/runmodel.h +selfdrive/modeld/runners/run.h + +selfdrive/monitoring/dmonitoringd.py +selfdrive/monitoring/driver_monitor.py + +selfdrive/navd/__init__.py +selfdrive/navd/navd.py +selfdrive/navd/helpers.py + +selfdrive/assets/.gitignore +selfdrive/assets/assets.qrc +selfdrive/assets/*.png +selfdrive/assets/*.svg +selfdrive/assets/body/* +selfdrive/assets/fonts/*.ttf +selfdrive/assets/icons/* +selfdrive/assets/images/* +selfdrive/assets/offroad/* +selfdrive/assets/sounds/* +selfdrive/assets/training/* + +third_party/SConscript + +third_party/linux/** +third_party/opencl/** + +third_party/json11/json11.cpp +third_party/json11/json11.hpp + +third_party/qrcode/*.cc +third_party/qrcode/*.hpp + +third_party/kaitai/*.h +third_party/kaitai/*.cpp + +third_party/libyuv/include/** +third_party/libyuv/lib/** +third_party/libyuv/larch64/** + +third_party/snpe/include/** +third_party/snpe/dsp** + +third_party/acados/x86_64/** +third_party/acados/larch64/** +third_party/acados/include/** + +third_party/qt5/larch64/bin/** + +scripts/update_now.sh +scripts/stop_updater.sh + +pyextra/.gitignore +pyextra/acados_template/** + +rednose/.gitignore +rednose/** +laika/** + +body/.gitignore +body/board/SConscript +body/board/*.h +body/board/*.c +body/board/*.s +body/board/*.ld +body/board/inc/** +body/board/obj/ +body/board/bldc/** +body/board/drivers/** +body/certs/** +body/crypto/** + +cereal/.gitignore +cereal/__init__.py +cereal/car.capnp +cereal/legacy.capnp +cereal/log.capnp +cereal/services.py +cereal/SConscript +cereal/include/** +cereal/logger/logger.h +cereal/messaging/.gitignore +cereal/messaging/__init__.py +cereal/messaging/bridge.cc +cereal/messaging/impl_msgq.cc +cereal/messaging/impl_msgq.h +cereal/messaging/impl_zmq.cc +cereal/messaging/impl_zmq.h +cereal/messaging/messaging.cc +cereal/messaging/messaging.h +cereal/messaging/messaging.pxd +cereal/messaging/messaging_pyx.pyx +cereal/messaging/msgq.cc +cereal/messaging/msgq.h +cereal/messaging/socketmaster.cc +cereal/visionipc/.gitignore +cereal/visionipc/__init__.py +cereal/visionipc/*.cc +cereal/visionipc/*.h +cereal/visionipc/*.pyx +cereal/visionipc/*.pxd + +panda/.gitignore +panda/__init__.py +panda/board/** +panda/certs/** +panda/crypto/** +panda/examples/query_fw_versions.py +panda/python/** + +opendbc/.gitignore +opendbc/__init__.py +opendbc/can/__init__.py +opendbc/can/SConscript +opendbc/can/can_define.py +opendbc/can/common.cc +opendbc/can/common.h +opendbc/can/common.pxd +opendbc/can/common_dbc.h +opendbc/can/dbc.cc +opendbc/can/packer.cc +opendbc/can/packer.py +opendbc/can/packer_pyx.pyx +opendbc/can/parser.cc +opendbc/can/parser.py +opendbc/can/parser_pyx.pyx + +opendbc/comma_body.dbc + +opendbc/chrysler_ram_hd_generated.dbc +opendbc/chrysler_ram_dt_generated.dbc +opendbc/chrysler_pacifica_2017_hybrid_generated.dbc +opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc + +opendbc/gm_global_a_powertrain_generated.dbc +opendbc/gm_global_a_object.dbc +opendbc/gm_global_a_chassis.dbc + +opendbc/FORD_CADS.dbc +opendbc/ford_fusion_2018_adas.dbc +opendbc/ford_lincoln_base_pt.dbc + +opendbc/honda_accord_2018_can_generated.dbc +opendbc/acura_ilx_2016_can_generated.dbc +opendbc/acura_rdx_2018_can_generated.dbc +opendbc/acura_rdx_2020_can_generated.dbc +opendbc/honda_civic_touring_2016_can_generated.dbc +opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +opendbc/honda_crv_touring_2016_can_generated.dbc +opendbc/honda_crv_ex_2017_can_generated.dbc +opendbc/honda_crv_ex_2017_body_generated.dbc +opendbc/honda_crv_executive_2016_can_generated.dbc +opendbc/honda_fit_ex_2018_can_generated.dbc +opendbc/honda_odyssey_exl_2018_generated.dbc +opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc +opendbc/honda_insight_ex_2019_can_generated.dbc +opendbc/acura_ilx_2016_nidec.dbc +opendbc/honda_civic_ex_2022_can_generated.dbc + +opendbc/hyundai_canfd.dbc +opendbc/hyundai_kia_generic.dbc +opendbc/hyundai_kia_mando_front_radar.dbc + +opendbc/mazda_2017.dbc + +opendbc/nissan_x_trail_2017.dbc +opendbc/nissan_leaf_2018.dbc + +opendbc/subaru_global_2017_generated.dbc +opendbc/subaru_outback_2015_generated.dbc +opendbc/subaru_outback_2019_generated.dbc +opendbc/subaru_forester_2017_generated.dbc + +opendbc/toyota_tnga_k_pt_generated.dbc +opendbc/toyota_new_mc_pt_generated.dbc +opendbc/toyota_nodsu_pt_generated.dbc +opendbc/toyota_adas.dbc +opendbc/toyota_tss2_adas.dbc + +opendbc/vw_golf_mk4.dbc +opendbc/vw_mqb_2010.dbc + +opendbc/tesla_can.dbc +opendbc/tesla_radar.dbc +opendbc/tesla_powertrain.dbc + +tinygrad_repo/openpilot/compile.py +tinygrad_repo/accel/opencl/* +tinygrad_repo/extra/onnx.py +tinygrad_repo/extra/utils.py +tinygrad_repo/tinygrad/llops/ops_gpu.py +tinygrad_repo/tinygrad/llops/ops_opencl.py +tinygrad_repo/tinygrad/helpers.py +tinygrad_repo/tinygrad/mlops.py +tinygrad_repo/tinygrad/ops.py +tinygrad_repo/tinygrad/shapetracker.py +tinygrad_repo/tinygrad/tensor.py +tinygrad_repo/tinygrad/nn/__init__.py diff --git a/release/files_pc b/release/files_pc new file mode 100644 index 00000000000000..01ecae4327ebca --- /dev/null +++ b/release/files_pc @@ -0,0 +1,7 @@ +selfdrive/modeld/runners/onnx* + +third_party/mapbox-gl-native-qt/x86_64/*.so + +third_party/libyuv/x64/** +third_party/snpe/x86_64/** +third_party/snpe/x86_64-linux-clang/** diff --git a/release/files_tici b/release/files_tici new file mode 100644 index 00000000000000..c8abd720d5d637 --- /dev/null +++ b/release/files_tici @@ -0,0 +1,19 @@ +third_party/snpe/larch64** +third_party/snpe/aarch64-ubuntu-gcc7.5/* +third_party/mapbox-gl-native-qt/include/* + +system/timezoned.py + +selfdrive/assets/navigation/* +selfdrive/assets/training_wide/* + +system/camerad/cameras/camera_qcom2.cc +system/camerad/cameras/camera_qcom2.h +system/camerad/cameras/camera_util.cc +system/camerad/cameras/camera_util.h +system/camerad/cameras/real_debayer.cl + +selfdrive/sensord/rawgps/* + +selfdrive/ui/qt/spinner_larch64 +selfdrive/ui/qt/text_larch64 diff --git a/release/pack.py b/release/pack.py deleted file mode 100755 index 92ff68fe761041..00000000000000 --- a/release/pack.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python3 - -import importlib -import shutil -import sys -import tempfile -import zipapp -from argparse import ArgumentParser -from pathlib import Path - -from openpilot.common.basedir import BASEDIR - - -DIRS = ['cereal', 'openpilot'] -EXTS = ['.png', '.py', '.ttf', '.capnp', '.json', '.fnt', '.mo'] -INTERPRETER = '/usr/bin/env python3' - - -def copy(src, dest): - if any(src.endswith(ext) for ext in EXTS): - shutil.copy2(src, dest, follow_symlinks=True) - - -if __name__ == '__main__': - parser = ArgumentParser(prog='pack.py', description="package script into a portable executable", epilog='comma.ai') - parser.add_argument('-e', '--entrypoint', help="function to call in module, default is 'main'", default='main') - parser.add_argument('-o', '--output', help='output file') - parser.add_argument('module', help="the module to target, e.g. 'openpilot.system.ui.spinner'") - args = parser.parse_args() - - if not args.output: - args.output = args.module - - try: - mod = importlib.import_module(args.module) - except ModuleNotFoundError: - print(f'{args.module} not found, typo?') - sys.exit(1) - - if not hasattr(mod, args.entrypoint): - print(f'{args.module} does not have a {args.entrypoint}() function, typo?') - sys.exit(1) - - with tempfile.TemporaryDirectory() as tmp: - for directory in DIRS: - shutil.copytree(BASEDIR + '/' + directory, tmp + '/' + directory, symlinks=False, dirs_exist_ok=True, copy_function=copy) - entry = f'{args.module}:{args.entrypoint}' - zipapp.create_archive(tmp, target=args.output, interpreter=INTERPRETER, main=entry) - - print(f'created executable {Path(args.output).resolve()}') diff --git a/release/release_files.py b/release/release_files.py deleted file mode 100755 index 36910293a4c6ca..00000000000000 --- a/release/release_files.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -import os -import re -from pathlib import Path - -HERE = os.path.abspath(os.path.dirname(__file__)) -ROOT = HERE + "/.." - -blacklist = [ - ".git/", - ".github/workflows/", - - "matlab.*.md", - - # no LFS or submodules in release - ".lfsconfig", - ".gitattributes", - ".git$", - ".gitmodules", -] - -# gets you through the blacklist -whitelist: list[str] = [ -] - -if __name__ == "__main__": - for f in Path(ROOT).rglob("**/*"): - if not (f.is_file() or f.is_symlink()): - continue - - rf = str(f.relative_to(ROOT)) - blacklisted = any(re.search(p, rf) for p in blacklist) - whitelisted = any(re.search(p, rf) for p in whitelist) - if blacklisted and not whitelisted: - continue - - print(rf) diff --git a/release/verify.sh b/release/verify.sh new file mode 100755 index 00000000000000..56f21183f1b81e --- /dev/null +++ b/release/verify.sh @@ -0,0 +1,16 @@ +#!/bin/bash + +set -e + +RED="\033[0;31m" +GREEN="\033[0;32m" +CLEAR="\033[0m" + +BRANCHES="devel dashcam3 release3" +for b in $BRANCHES; do + if git diff --quiet origin/$b origin/$b-staging && [ "$(git rev-parse origin/$b)" = "$(git rev-parse origin/$b-staging)" ]; then + printf "%-10s $GREEN ok $CLEAR\n" "$b" + else + printf "%-10s $RED mismatch $CLEAR\n" "$b" + fi +done diff --git a/scripts/apply-pr.sh b/scripts/apply-pr.sh index ad0af46b49a9ac..65805b848506b1 100755 --- a/scripts/apply-pr.sh +++ b/scripts/apply-pr.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/bin/bash if [ $# -eq 0 ]; then echo "usage: $0 " @@ -8,4 +8,4 @@ fi BASE="https://github.com/commaai/openpilot/pull/" PR_NUM="$(echo $1 | grep -o -E '[0-9]+')" -curl -L $BASE/$PR_NUM.patch | git apply -3 +curl -L $BASE/$PR_NUM.patch | git apply diff --git a/scripts/cell.sh b/scripts/cell.sh index 310a9694fd044b..cae701eccc1ee2 100755 --- a/scripts/cell.sh +++ b/scripts/cell.sh @@ -1,3 +1,5 @@ -#!/usr/bin/env bash -nmcli connection modify --temporary esim ipv4.route-metric 1 ipv6.route-metric 1 -nmcli con up esim +#!/usr/bin/bash + +nmcli connection modify --temporary lte ipv4.route-metric 1 +nmcli connection modify --temporary lte ipv6.route-metric 1 +nmcli con up lte diff --git a/scripts/checkout-pr.sh b/scripts/checkout-pr.sh deleted file mode 100755 index eeba816d8898b5..00000000000000 --- a/scripts/checkout-pr.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env bash -set -e - -if [ $# -eq 0 ]; then - echo "usage: $0 " - exit 1 -fi - -BASE="https://github.com/commaai/openpilot/pull/" -PR_NUM="$(echo $1 | grep -o -E '[0-9]+')" -BRANCH=tmp-pr${PR_NUM} - -git branch -D -f $BRANCH || true -git fetch -u -f origin pull/$PR_NUM/head:$BRANCH -git switch $BRANCH -git reset --hard FETCH_HEAD diff --git a/scripts/ci_results.py b/scripts/ci_results.py deleted file mode 100755 index a133541c69ca50..00000000000000 --- a/scripts/ci_results.py +++ /dev/null @@ -1,212 +0,0 @@ -#!/usr/bin/env python3 -"""Fetch CI results from GitHub Actions and Jenkins.""" - -import argparse -import json -import subprocess -import time -import urllib.error -import urllib.request -from datetime import datetime - -JENKINS_URL = "https://jenkins.comma.life" -DEFAULT_TIMEOUT = 1800 # 30 minutes -POLL_INTERVAL = 30 # seconds -LOG_TAIL_LINES = 10 # lines of log to include for failed jobs - - -def get_git_info(): - branch = subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], text=True).strip() - commit = subprocess.check_output(["git", "rev-parse", "HEAD"], text=True).strip() - return branch, commit - - -def get_github_actions_status(commit_sha): - result = subprocess.run( - ["gh", "run", "list", "--commit", commit_sha, "--workflow", "tests.yaml", "--json", "databaseId,status,conclusion"], - capture_output=True, text=True, check=True - ) - runs = json.loads(result.stdout) - if not runs: - return None, None - - run_id = runs[0]["databaseId"] - result = subprocess.run( - ["gh", "run", "view", str(run_id), "--json", "jobs"], - capture_output=True, text=True, check=True - ) - data = json.loads(result.stdout) - jobs = {job["name"]: {"status": job["status"], "conclusion": job["conclusion"], - "duration": format_duration(job) if job["conclusion"] not in ("skipped", None) and job.get("startedAt") else "", - "id": job["databaseId"]} - for job in data.get("jobs", [])} - return jobs, run_id - - -def get_github_job_log(run_id, job_id): - result = subprocess.run( - ["gh", "run", "view", str(run_id), "--job", str(job_id), "--log-failed"], - capture_output=True, text=True - ) - lines = result.stdout.strip().split('\n') - return '\n'.join(lines[-LOG_TAIL_LINES:]) if len(lines) > LOG_TAIL_LINES else result.stdout.strip() - - -def format_duration(job): - start = datetime.fromisoformat(job["startedAt"].replace("Z", "+00:00")) - end = datetime.fromisoformat(job["completedAt"].replace("Z", "+00:00")) - secs = int((end - start).total_seconds()) - return f"{secs // 60}m {secs % 60}s" - - -def get_jenkins_status(branch, commit_sha): - base_url = f"{JENKINS_URL}/job/openpilot/job/{branch}" - try: - # Get list of recent builds - with urllib.request.urlopen(f"{base_url}/api/json?tree=builds[number,url]", timeout=10) as resp: - builds = json.loads(resp.read().decode()).get("builds", []) - - # Find build matching commit - for build in builds[:20]: # check last 20 builds - with urllib.request.urlopen(f"{build['url']}api/json", timeout=10) as resp: - data = json.loads(resp.read().decode()) - for action in data.get("actions", []): - if action.get("_class") == "hudson.plugins.git.util.BuildData": - build_sha = action.get("lastBuiltRevision", {}).get("SHA1", "") - if build_sha.startswith(commit_sha) or commit_sha.startswith(build_sha): - # Get stages info - stages = [] - try: - with urllib.request.urlopen(f"{build['url']}wfapi/describe", timeout=10) as resp2: - wf_data = json.loads(resp2.read().decode()) - stages = [{"name": s["name"], "status": s["status"]} for s in wf_data.get("stages", [])] - except urllib.error.HTTPError: - pass - return { - "number": data["number"], - "in_progress": data.get("inProgress", False), - "result": data.get("result"), - "url": data.get("url", ""), - "stages": stages, - } - return None # no build found for this commit - except urllib.error.HTTPError: - return None # branch doesn't exist on Jenkins - - -def get_jenkins_log(build_url): - url = f"{build_url}consoleText" - with urllib.request.urlopen(url, timeout=30) as resp: - text = resp.read().decode(errors='replace') - lines = text.strip().split('\n') - return '\n'.join(lines[-LOG_TAIL_LINES:]) if len(lines) > LOG_TAIL_LINES else text.strip() - - -def is_complete(gh_status, jenkins_status): - gh_done = gh_status is None or all(j["status"] == "completed" for j in gh_status.values()) - jenkins_done = jenkins_status is None or not jenkins_status.get("in_progress", True) - return gh_done and jenkins_done - - -def status_icon(status, conclusion=None): - if status == "completed": - return ":white_check_mark:" if conclusion == "success" else ":x:" - return ":hourglass:" if status == "in_progress" else ":grey_question:" - - -def format_markdown(gh_status, gh_run_id, jenkins_status, commit_sha, branch): - lines = ["# CI Results", "", - f"**Branch**: {branch}", - f"**Commit**: {commit_sha[:7]}", - f"**Generated**: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}", ""] - - lines.extend(["## GitHub Actions", "", "| Job | Status | Duration |", "|-----|--------|----------|"]) - failed_gh_jobs = [] - if gh_status: - for job_name, job in gh_status.items(): - icon = status_icon(job["status"], job.get("conclusion")) - conclusion = job.get("conclusion") or job["status"] - lines.append(f"| {job_name} | {icon} {conclusion} | {job.get('duration', '')} |") - if job.get("conclusion") == "failure": - failed_gh_jobs.append((job_name, job.get("id"))) - else: - lines.append("| - | No workflow runs found | |") - - lines.extend(["", "## Jenkins", "", "| Stage | Status |", "|-------|--------|"]) - failed_jenkins_stages = [] - if jenkins_status: - stages = jenkins_status.get("stages", []) - if stages: - for stage in stages: - icon = ":white_check_mark:" if stage["status"] == "SUCCESS" else ( - ":x:" if stage["status"] == "FAILED" else ":hourglass:") - lines.append(f"| {stage['name']} | {icon} {stage['status'].lower()} |") - if stage["status"] == "FAILED": - failed_jenkins_stages.append(stage["name"]) - # Show overall build status if still in progress - if jenkins_status["in_progress"]: - lines.append("| (build in progress) | :hourglass: in_progress |") - else: - icon = ":hourglass:" if jenkins_status["in_progress"] else ( - ":white_check_mark:" if jenkins_status["result"] == "SUCCESS" else ":x:") - status = "in progress" if jenkins_status["in_progress"] else (jenkins_status["result"] or "unknown") - lines.append(f"| #{jenkins_status['number']} | {icon} {status.lower()} |") - if jenkins_status.get("url"): - lines.append(f"\n[View build]({jenkins_status['url']})") - else: - lines.append("| - | No builds found for branch |") - - if failed_gh_jobs or failed_jenkins_stages: - lines.extend(["", "## Failure Logs", ""]) - - for job_name, job_id in failed_gh_jobs: - lines.append(f"### GitHub Actions: {job_name}") - log = get_github_job_log(gh_run_id, job_id) - lines.extend(["", "```", log, "```", ""]) - - for stage_name in failed_jenkins_stages: - lines.append(f"### Jenkins: {stage_name}") - log = get_jenkins_log(jenkins_status["url"]) - lines.extend(["", "```", log, "```", ""]) - - return "\n".join(lines) + "\n" - - -def main(): - parser = argparse.ArgumentParser(description="Fetch CI results from GitHub Actions and Jenkins") - parser.add_argument("--wait", action="store_true", help="Wait for CI to complete") - parser.add_argument("--timeout", type=int, default=DEFAULT_TIMEOUT, help="Timeout in seconds (default: 1800)") - parser.add_argument("-o", "--output", default="ci_results.md", help="Output file (default: ci_results.md)") - parser.add_argument("--branch", help="Branch to check (default: current branch)") - parser.add_argument("--commit", help="Commit SHA to check (default: HEAD)") - args = parser.parse_args() - - branch, commit = get_git_info() - branch = args.branch or branch - commit = args.commit or commit - print(f"Fetching CI results for {branch} @ {commit[:7]}") - - start_time = time.monotonic() - while True: - gh_status, gh_run_id = get_github_actions_status(commit) - jenkins_status = get_jenkins_status(branch, commit) if branch != "HEAD" else None - - if not args.wait or is_complete(gh_status, jenkins_status): - break - - elapsed = time.monotonic() - start_time - if elapsed >= args.timeout: - print(f"Timeout after {int(elapsed)}s") - break - - print(f"CI still running, waiting {POLL_INTERVAL}s... ({int(elapsed)}s elapsed)") - time.sleep(POLL_INTERVAL) - - content = format_markdown(gh_status, gh_run_id, jenkins_status, commit, branch) - with open(args.output, "w") as f: - f.write(content) - print(f"Results written to {args.output}") - - -if __name__ == "__main__": - main() diff --git a/scripts/code_stats.py b/scripts/code_stats.py new file mode 100755 index 00000000000000..6c6db4335a6a71 --- /dev/null +++ b/scripts/code_stats.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import os +import ast +import stat +import subprocess + +fouts = {x.decode('utf-8') for x in subprocess.check_output(['git', 'ls-files']).strip().split()} + +pyf = [] +for d in ["cereal", "common", "scripts", "selfdrive", "tools"]: + for root, dirs, files in os.walk(d): + for f in files: + if f.endswith(".py"): + pyf.append(os.path.join(root, f)) + +imps = set() + +class Analyzer(ast.NodeVisitor): + def visit_Import(self, node): + for alias in node.names: + imps.add(alias.name) + self.generic_visit(node) + + def visit_ImportFrom(self, node): + imps.add(node.module) + self.generic_visit(node) + +tlns = 0 +carlns = 0 +scriptlns = 0 +testlns = 0 +for f in sorted(pyf): + if f not in fouts: + continue + xbit = bool(os.stat(f)[stat.ST_MODE] & stat.S_IXUSR) + src = open(f).read() + lns = len(src.split("\n")) + tree = ast.parse(src) + Analyzer().visit(tree) + print("%5d %s %s" % (lns, f, xbit)) + if 'test' in f: + testlns += lns + elif f.startswith('tools/') or f.startswith('scripts/') or f.startswith('selfdrive/debug'): + scriptlns += lns + elif f.startswith('selfdrive/car'): + carlns += lns + else: + tlns += lns + +print("%d lines of openpilot python" % tlns) +print("%d lines of car ports" % carlns) +print("%d lines of tools/scripts/debug" % scriptlns) +print("%d lines of tests" % testlns) +#print(sorted(list(imps))) diff --git a/scripts/count_cars.py b/scripts/count_cars.py new file mode 100755 index 00000000000000..25bad2c9b4debe --- /dev/null +++ b/scripts/count_cars.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +from collections import Counter +from pprint import pprint + +from selfdrive.car.docs import get_all_car_info + +if __name__ == "__main__": + cars = get_all_car_info() + make_count = Counter(l.make for l in cars) + print("\n", "*" * 20, len(cars), "total", "*" * 20, "\n") + pprint(make_count) diff --git a/scripts/disable-powersave.py b/scripts/disable-powersave.py index 367b4108b0d28b..93688504f38c1d 100755 --- a/scripts/disable-powersave.py +++ b/scripts/disable-powersave.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from openpilot.system.hardware import HARDWARE +from system.hardware import HARDWARE if __name__ == "__main__": HARDWARE.set_power_save(False) diff --git a/scripts/dump_pll.c b/scripts/dump_pll.c new file mode 100644 index 00000000000000..325ee2b4c02327 --- /dev/null +++ b/scripts/dump_pll.c @@ -0,0 +1,59 @@ +#include +#include +#include + +void hexdump(uint32_t *d, int l) { + for (int i = 0; i < l; i++) { + if (i%0x10 == 0 && i != 0) printf("\n"); + printf("%8x ", d[i]); + } + printf("\n"); +} + +/* Power cluster primary PLL */ +#define C0_PLL_MODE 0x0 +#define C0_PLL_L_VAL 0x4 +#define C0_PLL_ALPHA 0x8 +#define C0_PLL_USER_CTL 0x10 +#define C0_PLL_CONFIG_CTL 0x18 +#define C0_PLL_CONFIG_CTL_HI 0x1C +#define C0_PLL_STATUS 0x28 +#define C0_PLL_TEST_CTL_LO 0x20 +#define C0_PLL_TEST_CTL_HI 0x24 + +/* Power cluster alt PLL */ +#define C0_PLLA_MODE 0x100 +#define C0_PLLA_L_VAL 0x104 +#define C0_PLLA_ALPHA 0x108 +#define C0_PLLA_USER_CTL 0x110 +#define C0_PLLA_CONFIG_CTL 0x118 +#define C0_PLLA_STATUS 0x128 +#define C0_PLLA_TEST_CTL_LO 0x120 + +#define APC_DIAG_OFFSET 0x48 +#define CLK_CTL_OFFSET 0x44 +#define MUX_OFFSET 0x40 +#define MDD_DROOP_CODE 0x7c +#define SSSCTL_OFFSET 0x160 +#define PSCTL_OFFSET 0x164 + +int main() { + int fd = open("/dev/mem", O_RDWR); + volatile uint32_t *mb = (uint32_t*)mmap(0,0x1000,PROT_READ | PROT_WRITE,MAP_SHARED,fd,0x06400000); + volatile uint32_t *mc = (uint32_t*)mmap(0,0x1000,PROT_READ | PROT_WRITE,MAP_SHARED,fd,0x06480000); + volatile uint32_t *md = (uint32_t*)mmap(0,0x1000,PROT_READ | PROT_WRITE,MAP_SHARED,fd,0x09A20000); + while (1) { + printf("PLL MODE:%x L_VAL:%x ALPHA:%x USER_CTL:%x CONFIG_CTL:%x CONFIG_CTL_HI:%x STATUS:%x TEST_CTL_LO:%x TEST_CTL_HI:%x\n", + mb[C0_PLL_MODE/4], mb[C0_PLL_L_VAL/4], mb[C0_PLL_ALPHA/4], + mb[C0_PLL_USER_CTL/4], mb[C0_PLL_CONFIG_CTL/4], mb[C0_PLL_CONFIG_CTL_HI/4], + mb[C0_PLL_STATUS/4], mb[C0_PLL_TEST_CTL_LO/4], mb[C0_PLL_TEST_CTL_HI/4]); + printf(" MUX_OFFSET:%x CLK_CTL_OFFSET:%x APC_DIAG_OFFSET:%x MDD_DROOP_CODE:%x\n", + mb[MUX_OFFSET/4], mb[CLK_CTL_OFFSET/4], mb[APC_DIAG_OFFSET/4], mb[MDD_DROOP_CODE/4]); + printf(" PLLA MODE:%x L_VAL:%x ALPHA:%x USER_CTL:%x CONFIG_CTL:%x STATUS:%x TEST_CTL_LO:%x SSSCTL_OFFSET:%x PSCTL_OFFSET:%x\n", + mb[C0_PLLA_MODE/4], mb[C0_PLLA_L_VAL/4], mb[C0_PLLA_ALPHA/4], mb[C0_PLLA_USER_CTL/4], + mb[C0_PLLA_CONFIG_CTL/4], mb[C0_PLLA_STATUS/4], mb[C0_PLLA_TEST_CTL_LO/4], + mb[SSSCTL_OFFSET/4], mb[PSCTL_OFFSET/4]); + usleep(1000*100); + } +} + diff --git a/scripts/jenkins_loop_test.sh b/scripts/jenkins_loop_test.sh deleted file mode 100755 index 8073f4668c1888..00000000000000 --- a/scripts/jenkins_loop_test.sh +++ /dev/null @@ -1,129 +0,0 @@ -#!/usr/bin/env bash -set -e - -YELLOW='\033[0;33m' -GREEN='\033[0;32m' -UNDERLINE='\033[4m' -BOLD='\033[1m' -NC='\033[0m' - -BRANCH="master" -RUNS="20" - -COOKIE_JAR=/tmp/cookies -CRUMB=$(curl -s --cookie-jar $COOKIE_JAR 'https://jenkins.comma.life/crumbIssuer/api/xml?xpath=concat(//crumbRequestField,":",//crumb)') - -FIRST_LOOP=1 - -function loop() { - JENKINS_BRANCH="__jenkins_loop_${BRANCH}_$(date +%s)" - API_ROUTE="https://jenkins.comma.life/job/openpilot/job/$JENKINS_BRANCH" - - for run in $(seq 1 $((RUNS / 2))); do - - N=2 - - if [[ $FIRST_LOOP ]]; then - TEMP_DIR=$(mktemp -d) - GIT_LFS_SKIP_SMUDGE=1 git clone --quiet -b $BRANCH --depth=1 --no-tags git@github.com:commaai/openpilot $TEMP_DIR - git -C $TEMP_DIR checkout --quiet -b $JENKINS_BRANCH - echo "TESTING: $(date)" >> $TEMP_DIR/testing_jenkins - git -C $TEMP_DIR add testing_jenkins - git -C $TEMP_DIR commit --quiet -m "testing" - git -C $TEMP_DIR push --quiet -f origin $JENKINS_BRANCH - rm -rf $TEMP_DIR - FIRST_BUILD=1 - echo '' - echo 'waiting on Jenkins...' - echo '' - sleep 90 - FIRST_LOOP="" - fi - - FIRST_BUILD=$(curl -s $API_ROUTE/api/json | jq .nextBuildNumber) - LAST_BUILD=$((FIRST_BUILD+N-1)) - TEST_BUILDS=( $(seq $FIRST_BUILD $LAST_BUILD) ) - - # Start N new builds - for i in ${TEST_BUILDS[@]}; - do - echo "Starting build $i" - curl -s --output /dev/null --cookie $COOKIE_JAR -H "$CRUMB" -X POST $API_ROUTE/build?delay=0sec - sleep 5 - done - echo "" - - # Wait for all builds to end - while true; do - sleep 30 - - count=0 - for i in ${TEST_BUILDS[@]}; - do - RES=$(curl -s -w "\n%{http_code}" --cookie $COOKIE_JAR -H "$CRUMB" $API_ROUTE/$i/api/json) - HTTP_CODE=$(tail -n1 <<< "$RES") - JSON=$(sed '$ d' <<< "$RES") - - if [[ $HTTP_CODE == "200" ]]; then - STILL_RUNNING=$(echo $JSON | jq .inProgress) - if [[ $STILL_RUNNING == "true" ]]; then - echo -e "Build $i: ${YELLOW}still running${NC}" - continue - else - count=$((count+1)) - echo -e "Build $i: ${GREEN}done${NC}" - fi - else - echo "No status for build $i" - fi - done - echo "See live results: ${API_ROUTE}/buildTimeTrend" - echo "" - - if [[ $count -ge $N ]]; then - break - fi - done - - done -} - -function usage() { - echo "" - echo "Run the Jenkins tests multiple times on a specific branch" - echo "" - echo -e "${BOLD}${UNDERLINE}Options:${NC}" - echo -e " ${BOLD}-n, --n${NC}" - echo -e " Specify how many runs to do (default to ${BOLD}20${NC})" - echo -e " ${BOLD}-b, --branch${NC}" - echo -e " Specify which branch to run the tests against (default to ${BOLD}master${NC})" - echo "" -} - -function _looper() { - if [[ $# -eq 0 ]]; then - usage - exit 0 - fi - - # parse Options - while [[ $# -gt 0 ]]; do - case $1 in - -n | --n ) shift 1; RUNS="$1"; shift 1 ;; - -b | --b | --branch | -branch ) shift 1; BRANCH="$1"; shift 1 ;; - * ) usage; exit 0 ;; - esac - done - - echo "" - echo -e "You are about to start $RUNS Jenkins builds against the $BRANCH branch." - echo -e "If you expect this to run overnight, ${UNDERLINE}${BOLD}unplug the cold reboot power switch${NC} from the testing closet before." - echo "" - read -p "Press (y/Y) to confirm: " choice - if [[ "$choice" == "y" || "$choice" == "Y" ]]; then - loop - fi - -} - -_looper $@ diff --git a/scripts/launch_corolla.sh b/scripts/launch_corolla.sh index 926569a1e0e13d..0801938e71bee1 100755 --- a/scripts/launch_corolla.sh +++ b/scripts/launch_corolla.sh @@ -1,7 +1,6 @@ -#!/usr/bin/env bash +#!/usr/bin/bash DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" -export FINGERPRINT="TOYOTA_COROLLA_TSS2" -export SKIP_FW_QUERY="1" +export FINGERPRINT="TOYOTA COROLLA TSS2 2019" $DIR/../launch_openpilot.sh diff --git a/scripts/lint/check_nomerge_comments.sh b/scripts/lint/check_nomerge_comments.sh deleted file mode 100755 index 6737d62a2053b2..00000000000000 --- a/scripts/lint/check_nomerge_comments.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env bash - -FAIL=0 - -if grep -n '\(#\|//\)\([[:space:]]*\)NOMERGE' $@; then - echo -e "NOMERGE comments found! Remove them before merging\n" - FAIL=1 -fi - -exit $FAIL diff --git a/scripts/lint/check_raylib_includes.sh b/scripts/lint/check_raylib_includes.sh deleted file mode 100755 index e3be73a4897afa..00000000000000 --- a/scripts/lint/check_raylib_includes.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env bash - -FAIL=0 - -if grep -n '#include "third_party/raylib/include/raylib\.h"' $@ | grep -v '^system/ui/raylib/raylib\.h'; then - echo -e "Bad raylib include found! Use '#include \"system/ui/raylib/raylib.h\"' instead\n" - FAIL=1 -fi - -exit $FAIL diff --git a/scripts/lint/check_shebang_format.sh b/scripts/lint/check_shebang_format.sh deleted file mode 100755 index 89b95d5929a3b2..00000000000000 --- a/scripts/lint/check_shebang_format.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash - -FAIL=0 - -if grep '^#!.*python' $@ | grep -v '#!/usr/bin/env python3$'; then - echo -e "Invalid shebang! Must use '#!/usr/bin/env python3'\n" - FAIL=1 -fi - -if grep '^#!.*bash' $@ | grep -v '#!/usr/bin/env bash$'; then - echo -e "Invalid shebang! Must use '#!/usr/bin/env bash'" - FAIL=1 -fi - -exit $FAIL diff --git a/scripts/lint/lint.sh b/scripts/lint/lint.sh deleted file mode 100755 index 5581171e8feb65..00000000000000 --- a/scripts/lint/lint.sh +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env bash -set -e - -RED='\033[0;31m' -GREEN='\033[0;32m' -UNDERLINE='\033[4m' -BOLD='\033[1m' -NC='\033[0m' - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -ROOT="$DIR/../../" -cd $ROOT - -FAILED=0 - -IGNORED_FILES="uv\.lock|docs\/CARS.md" -IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*" - -function run() { - shopt -s extglob - case $1 in - $SKIP | $RUN ) return 0 ;; - esac - - echo -en "$1" - - for ((i=0; i<$((50 - ${#1})); i++)); do - echo -n "." - done - - shift 1; - CMD="$@" - - set +e - log="$((eval "$CMD" ) 2>&1)" - - if [[ $? -eq 0 ]]; then - echo -e "[${GREEN}✔${NC}]" - else - echo -e "[${RED}✗${NC}]" - echo "$log" - FAILED=1 - fi - set -e -} - -function run_tests() { - ALL_FILES=$1 - PYTHON_FILES=$2 - - run "ruff" ruff check $ROOT --quiet - run "check_added_large_files" python3 -m pre_commit_hooks.check_added_large_files --enforce-all $ALL_FILES --maxkb=120 - run "check_shebang_scripts_are_executable" python3 -m pre_commit_hooks.check_shebang_scripts_are_executable $ALL_FILES - run "check_shebang_format" $DIR/check_shebang_format.sh $ALL_FILES - run "check_nomerge_comments" $DIR/check_nomerge_comments.sh $ALL_FILES - - if [[ -z "$FAST" ]]; then - run "ty" ty check - run "codespell" codespell $ALL_FILES - fi - - return $FAILED -} - -function help() { - echo "A fast linter" - echo "" - echo -e "${BOLD}${UNDERLINE}Usage:${NC} op lint [TESTS] [OPTIONS]" - echo "" - echo -e "${BOLD}${UNDERLINE}Tests:${NC}" - echo -e " ${BOLD}ruff${NC}" - echo -e " ${BOLD}ty${NC}" - echo -e " ${BOLD}codespell${NC}" - echo -e " ${BOLD}check_added_large_files${NC}" - echo -e " ${BOLD}check_shebang_scripts_are_executable${NC}" - echo "" - echo -e "${BOLD}${UNDERLINE}Options:${NC}" - echo -e " ${BOLD}-f, --fast${NC}" - echo " Skip slow tests" - echo -e " ${BOLD}-s, --skip${NC}" - echo " Specify tests to skip separated by spaces" - echo "" - echo -e "${BOLD}${UNDERLINE}Examples:${NC}" - echo " op lint ty ruff" - echo " Only run the ty and ruff tests" - echo "" - echo " op lint --skip ty ruff" - echo " Skip the ty and ruff tests" - echo "" - echo " op lint" - echo " Run all the tests" -} - -SKIP="" -RUN="" -while [[ $# -gt 0 ]]; do - case $1 in - -f | --fast ) shift 1; FAST="1" ;; - -s | --skip ) shift 1; SKIP=" " ;; - -h | --help | -help | --h ) help; exit 0 ;; - * ) if [[ -n $SKIP ]]; then SKIP+="$1 "; else RUN+="$1 "; fi; shift 1 ;; - esac -done - -RUN=$([ -z "$RUN" ] && echo "" || echo "!($(echo $RUN | sed 's/ /|/g'))") -SKIP="@($(echo $SKIP | sed 's/ /|/g'))" - -GIT_FILES="$(git ls-files | sed -E "s/$IGNORED_FILES|$IGNORED_DIRS//g")" -ALL_FILES="" -for f in $GIT_FILES; do - if [[ -f $f ]]; then - ALL_FILES+="$f"$'\n' - fi -done -PYTHON_FILES=$(echo "$ALL_FILES" | grep --color=never '.py$' || true) - -run_tests "$ALL_FILES" "$PYTHON_FILES" diff --git a/scripts/post-commit b/scripts/post-commit deleted file mode 100755 index f9964639de4137..00000000000000 --- a/scripts/post-commit +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash -set -e -if [[ -f .git/hooks/post-commit.d/post-commit ]]; then - .git/hooks/post-commit.d/post-commit -fi -tools/op.sh lint --fast -echo "" diff --git a/scripts/pyqt_demo.py b/scripts/pyqt_demo.py new file mode 100755 index 00000000000000..43716fbeb20319 --- /dev/null +++ b/scripts/pyqt_demo.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 + +from PyQt5.QtWidgets import QApplication, QLabel # pylint: disable=no-name-in-module, import-error +from selfdrive.ui.qt.python_helpers import set_main_window + + +if __name__ == "__main__": + app = QApplication([]) + label = QLabel('Hello World!') + + # Set full screen and rotate + set_main_window(label) + + app.exec_() diff --git a/scripts/reporter.py b/scripts/reporter.py deleted file mode 100755 index 903fcc89111d35..00000000000000 --- a/scripts/reporter.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -import os -import glob -import onnx - -BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) -MASTER_PATH = os.getenv("MASTER_PATH", BASEDIR) -MODEL_PATH = "/selfdrive/modeld/models/" - -def get_checkpoint(f): - model = onnx.load(f) - metadata = {prop.key: prop.value for prop in model.metadata_props} - return metadata['model_checkpoint'].split('/')[0] - -if __name__ == "__main__": - print("| | master | PR branch |") - print("|-| ----- | --------- |") - - for f in glob.glob(BASEDIR + MODEL_PATH + "/*.onnx"): - # TODO: add checkpoint to DM - if "dmonitoring" in f: - continue - - fn = os.path.basename(f) - master = get_checkpoint(MASTER_PATH + MODEL_PATH + fn) - pr = get_checkpoint(BASEDIR + MODEL_PATH + fn) - print( - "|", fn, "|", - f"[{master}](https://reporter.comma.life/experiment/{master})", "|", - f"[{pr}](https://reporter.comma.life/experiment/{pr})", "|" - ) diff --git a/scripts/retry.sh b/scripts/retry.sh deleted file mode 100755 index 23501d75595954..00000000000000 --- a/scripts/retry.sh +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env bash - -function fail { - echo $1 >&2 - exit 1 -} - -function retry { - local n=1 - local max=3 # 3 retries before failure - local delay=5 # delay between retries, 5 seconds - while true; do - echo "Running command '$@' with retry, attempt $n/$max" - "$@" && break || { - if [[ $n -lt $max ]]; then - ((n++)) - sleep $delay; - else - fail "The command has failed after $n attempts." - fi - } - done -} - -if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then - retry "$@" -fi diff --git a/scripts/stop_updater.sh b/scripts/stop_updater.sh index 703b3639282a48..4243d30e9f9ea5 100755 --- a/scripts/stop_updater.sh +++ b/scripts/stop_updater.sh @@ -1,7 +1,7 @@ #!/usr/bin/env sh # Stop updater -pkill -2 -f system.updated.updated +pkill -2 -f selfdrive.updated # Remove pending update rm -f /data/safe_staging/finalized/.overlay_consistent diff --git a/scripts/switch_to_master.sh b/scripts/switch_to_master.sh new file mode 100755 index 00000000000000..cad51eb549f6b3 --- /dev/null +++ b/scripts/switch_to_master.sh @@ -0,0 +1,16 @@ +#!/usr/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +cd $DIR/.. + +git clean -xdf . +git rm -r --cached . + +git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*" +git fetch origin master +git checkout master +git reset --hard +git submodule update --init + +printf '\n\n' +echo "master checked out. reboot to start building openpilot master" diff --git a/scripts/update_now.sh b/scripts/update_now.sh index c34228976a54e0..3f0193f081a15c 100755 --- a/scripts/update_now.sh +++ b/scripts/update_now.sh @@ -1,4 +1,4 @@ #!/usr/bin/env sh # Send SIGHUP to updater -pkill -1 -f system.updated +pkill -1 -f selfdrive.updated diff --git a/scripts/usb.sh b/scripts/usb.sh deleted file mode 100755 index 5796cfa028b526..00000000000000 --- a/scripts/usb.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env bash - -# testing the GPU box - -export XDG_CACHE_HOME=/data/tinycache -mkdir -p $XDG_CACHE_HOME - -cd /data/openpilot/tinygrad_repo/examples -while true; do - AMD=1 AMD_IFACE=usb python ./beautiful_cartpole.py - sleep 1 -done diff --git a/scripts/usbgpu/benchmark.sh b/scripts/usbgpu/benchmark.sh deleted file mode 100755 index 04a76d054e7ab4..00000000000000 --- a/scripts/usbgpu/benchmark.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/usr/bin/env bash -set -e - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -cd $DIR/../../tinygrad_repo - -GREEN='\033[0;32m' -NC='\033[0m' - - -#export DEBUG=2 -export PYTHONPATH=. -export AM_RESET=1 -export AMD=1 -export AMD_IFACE=USB -export AMD_LLVM=1 - -python3 -m unittest -q --buffer test.test_tiny.TestTiny.test_plus \ - > /tmp/test_tiny.log 2>&1 || (cat /tmp/test_tiny.log; exit 1) -printf "${GREEN}Booted in ${SECONDS}s${NC}\n" -printf "${GREEN}=============${NC}\n" - -printf "\n\n" -printf "${GREEN}Transfer speeds:${NC}\n" -printf "${GREEN}================${NC}\n" -python3 test/external/external_test_usb_asm24.py TestDevCopySpeeds diff --git a/scripts/waste.py b/scripts/waste.py index bdee09d3f6b583..d3c96bf1980f9d 100755 --- a/scripts/waste.py +++ b/scripts/waste.py @@ -1,24 +1,24 @@ #!/usr/bin/env python3 import os -import time import numpy as np +from common.realtime import sec_since_boot from multiprocessing import Process -from setproctitle import setproctitle +from setproctitle import setproctitle # pylint: disable=no-name-in-module def waste(core): - os.sched_setaffinity(0, [core,]) + os.sched_setaffinity(0, [core,]) # pylint: disable=no-member m1 = np.zeros((200, 200)) + 0.8 m2 = np.zeros((200, 200)) + 1.2 i = 1 - st = time.monotonic() + st = sec_since_boot() j = 0 while 1: if (i % 100) == 0: - setproctitle(f"{core:3d}: {i:8d}") - lt = time.monotonic() - print(f"{core:3d}: {i:8d} {lt-st:f} {j:.2f}") + setproctitle("%3d: %8d" % (core, i)) + lt = sec_since_boot() + print("%3d: %8d %f %.2f" % (core, i, lt-st, j)) st = lt i += 1 j = np.sum(np.matmul(m1, m2)) diff --git a/selfdrive/SConscript b/selfdrive/SConscript deleted file mode 100644 index 55f347c44ebfdf..00000000000000 --- a/selfdrive/SConscript +++ /dev/null @@ -1,6 +0,0 @@ -SConscript(['pandad/SConscript']) -SConscript(['controls/lib/lateral_mpc_lib/SConscript']) -SConscript(['controls/lib/longitudinal_mpc_lib/SConscript']) -SConscript(['locationd/SConscript']) -SConscript(['modeld/SConscript']) -SConscript(['ui/SConscript']) diff --git a/selfdrive/assets/.gitignore b/selfdrive/assets/.gitignore index fffd4b4ed9c8a2..283034ca8b4104 100644 --- a/selfdrive/assets/.gitignore +++ b/selfdrive/assets/.gitignore @@ -1,4 +1 @@ *.cc -fonts/*.fnt -fonts/*.png -translations_assets.qrc diff --git a/selfdrive/assets/assets.qrc b/selfdrive/assets/assets.qrc index 26a7d998edf2eb..39be41aa65d56c 100644 --- a/selfdrive/assets/assets.qrc +++ b/selfdrive/assets/assets.qrc @@ -1,20 +1,17 @@ - ../../third_party/bootstrap/bootstrap-icons.svg - images/button_continue_triangle.svg - icons/circled_check.svg - icons/circled_slash.svg - icons/eye_open.svg - icons/eye_closed.svg + img_continue_triangle.svg + img_circled_check.svg + img_circled_slash.svg + img_eye_open.svg + img_eye_closed.svg icons/close.svg - icons/lock_closed.svg - icons/checkmark.svg - icons/warning.png - icons/wifi_strength_low.svg - icons/wifi_strength_medium.svg - icons/wifi_strength_high.svg - icons/wifi_strength_full.svg - - ../ui/translations/languages.json + offroad/icon_lock_closed.svg + offroad/icon_checkmark.svg + offroad/icon_warning.png + offroad/icon_wifi_strength_low.svg + offroad/icon_wifi_strength_medium.svg + offroad/icon_wifi_strength_high.svg + offroad/icon_wifi_strength_full.svg diff --git a/selfdrive/assets/body/awake.gif b/selfdrive/assets/body/awake.gif index cc22fd9b128e32..7ec67055ddbc5e 100644 Binary files a/selfdrive/assets/body/awake.gif and b/selfdrive/assets/body/awake.gif differ diff --git a/selfdrive/assets/body/sleep.gif b/selfdrive/assets/body/sleep.gif index fcc0b4ed50b379..469cc803389e7b 100644 Binary files a/selfdrive/assets/body/sleep.gif and b/selfdrive/assets/body/sleep.gif differ diff --git a/selfdrive/assets/compress-images.sh b/selfdrive/assets/compress-images.sh index de59099bd13398..8601b2d61bbfa4 100755 --- a/selfdrive/assets/compress-images.sh +++ b/selfdrive/assets/compress-images.sh @@ -1,7 +1,7 @@ -#!/usr/bin/env bash +#!/bin/bash echo "compressing training guide images" -optipng -o7 -strip all training/* +optipng -o7 -strip all training/* training_wide/* # This can sometimes provide smaller images -# mogrify -quality 100 -format jpg training/* +# mogrify -quality 100 -format jpg training_wide/* training/* diff --git a/selfdrive/assets/fonts/Inter-Black.ttf b/selfdrive/assets/fonts/Inter-Black.ttf index 6bfd108896d3ac..565375773523cf 100644 Binary files a/selfdrive/assets/fonts/Inter-Black.ttf and b/selfdrive/assets/fonts/Inter-Black.ttf differ diff --git a/selfdrive/assets/fonts/Inter-Bold.ttf b/selfdrive/assets/fonts/Inter-Bold.ttf index 240f73d3440f12..e98b84ce87fa0b 100644 Binary files a/selfdrive/assets/fonts/Inter-Bold.ttf and b/selfdrive/assets/fonts/Inter-Bold.ttf differ diff --git a/selfdrive/assets/fonts/Inter-ExtraBold.ttf b/selfdrive/assets/fonts/Inter-ExtraBold.ttf index 8cb343bc402345..7f16a0f0f59479 100644 Binary files a/selfdrive/assets/fonts/Inter-ExtraBold.ttf and b/selfdrive/assets/fonts/Inter-ExtraBold.ttf differ diff --git a/selfdrive/assets/fonts/Inter-ExtraLight.ttf b/selfdrive/assets/fonts/Inter-ExtraLight.ttf index 9ab6117be95316..69426a3eb5660e 100644 Binary files a/selfdrive/assets/fonts/Inter-ExtraLight.ttf and b/selfdrive/assets/fonts/Inter-ExtraLight.ttf differ diff --git a/selfdrive/assets/fonts/Inter-Light.ttf b/selfdrive/assets/fonts/Inter-Light.ttf index a09fccdb3d11cd..a5f073690d3ffe 100644 Binary files a/selfdrive/assets/fonts/Inter-Light.ttf and b/selfdrive/assets/fonts/Inter-Light.ttf differ diff --git a/selfdrive/assets/fonts/Inter-Medium.ttf b/selfdrive/assets/fonts/Inter-Medium.ttf index e3a0591ed1a256..721147d8311f1b 100644 Binary files a/selfdrive/assets/fonts/Inter-Medium.ttf and b/selfdrive/assets/fonts/Inter-Medium.ttf differ diff --git a/selfdrive/assets/fonts/Inter-Regular.ttf b/selfdrive/assets/fonts/Inter-Regular.ttf index 3d153fa5679fd4..96fd6a12d0e20d 100644 Binary files a/selfdrive/assets/fonts/Inter-Regular.ttf and b/selfdrive/assets/fonts/Inter-Regular.ttf differ diff --git a/selfdrive/assets/fonts/Inter-SemiBold.ttf b/selfdrive/assets/fonts/Inter-SemiBold.ttf index ec7ef519d07e7c..ddb279290ba32b 100644 Binary files a/selfdrive/assets/fonts/Inter-SemiBold.ttf and b/selfdrive/assets/fonts/Inter-SemiBold.ttf differ diff --git a/selfdrive/assets/fonts/Inter-Thin.ttf b/selfdrive/assets/fonts/Inter-Thin.ttf index abcfac0b75b563..76be6252b91e87 100644 Binary files a/selfdrive/assets/fonts/Inter-Thin.ttf and b/selfdrive/assets/fonts/Inter-Thin.ttf differ diff --git a/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf b/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf deleted file mode 100644 index 34cd543d94d461..00000000000000 --- a/selfdrive/assets/fonts/JetBrainsMono-Medium.ttf +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:64708889e701acf7f2f43fd9c3696eb7f2c849ae67693ce581ad8f92433b3b24 -size 204140 diff --git a/selfdrive/assets/fonts/NotoColorEmoji.ttf b/selfdrive/assets/fonts/NotoColorEmoji.ttf deleted file mode 100644 index 778e821ce35672..00000000000000 --- a/selfdrive/assets/fonts/NotoColorEmoji.ttf +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:93cdc4ee9aa40e2afceecc63da0ca05ec7aab4bec991ece51a6b52389f48a477 -size 10788068 diff --git a/selfdrive/assets/fonts/process.py b/selfdrive/assets/fonts/process.py deleted file mode 100755 index ddc8b3a8682c23..00000000000000 --- a/selfdrive/assets/fonts/process.py +++ /dev/null @@ -1,132 +0,0 @@ -#!/usr/bin/env python3 -from pathlib import Path -import json - -import pyray as rl - -FONT_DIR = Path(__file__).resolve().parent -SELFDRIVE_DIR = FONT_DIR.parents[1] -TRANSLATIONS_DIR = SELFDRIVE_DIR / "ui" / "translations" -LANGUAGES_FILE = TRANSLATIONS_DIR / "languages.json" - -GLYPH_PADDING = 6 -EXTRA_CHARS = "–‑✓×°§•X⚙✕◀▶✔⌫⇧␣○●↳çêüñ–‑✓×°§•€£¥" -UNIFONT_LANGUAGES = {"ar", "th", "zh-CHT", "zh-CHS", "ko", "ja"} - - -def _languages(): - if not LANGUAGES_FILE.exists(): - return {} - with LANGUAGES_FILE.open(encoding="utf-8") as f: - return json.load(f) - - -def _char_sets(): - base = set(map(chr, range(32, 127))) | set(EXTRA_CHARS) - unifont = set(base) - - for language, code in _languages().items(): - unifont.update(language) - po_path = TRANSLATIONS_DIR / f"app_{code}.po" - try: - chars = set(po_path.read_text(encoding="utf-8")) - except FileNotFoundError: - continue - (unifont if code in UNIFONT_LANGUAGES else base).update(chars) - - return tuple(sorted(ord(c) for c in base)), tuple(sorted(ord(c) for c in unifont)) - - -def _glyph_metrics(glyphs, rects, codepoints): - entries = [] - min_offset_y, max_extent = None, 0 - for idx, codepoint in enumerate(codepoints): - glyph = glyphs[idx] - rect = rects[idx] - width = int(round(rect.width)) - height = int(round(rect.height)) - offset_y = int(round(glyph.offsetY)) - min_offset_y = offset_y if min_offset_y is None else min(min_offset_y, offset_y) - max_extent = max(max_extent, offset_y + height) - entries.append({ - "id": codepoint, - "x": int(round(rect.x)), - "y": int(round(rect.y)), - "width": width, - "height": height, - "xoffset": int(round(glyph.offsetX)), - "yoffset": offset_y, - "xadvance": int(round(glyph.advanceX)), - }) - - if min_offset_y is None: - raise RuntimeError("No glyphs were generated") - - line_height = int(round(max_extent - min_offset_y)) - base = int(round(max_extent)) - return entries, line_height, base - - -def _write_bmfont(path: Path, font_size: int, face: str, atlas_name: str, line_height: int, base: int, atlas_size, entries): - # TODO: why doesn't raylib calculate these metrics correctly? - if line_height != font_size: - print("using font size for line height", atlas_name) - line_height = font_size - lines = [ - f"info face=\"{face}\" size=-{font_size} bold=0 italic=0 charset=\"\" unicode=1 stretchH=100 smooth=0 aa=1 padding=0,0,0,0 spacing=0,0 outline=0", - f"common lineHeight={line_height} base={base} scaleW={atlas_size[0]} scaleH={atlas_size[1]} pages=1 packed=0 alphaChnl=0 redChnl=4 greenChnl=4 blueChnl=4", - f"page id=0 file=\"{atlas_name}\"", - f"chars count={len(entries)}", - ] - for entry in entries: - lines.append( - ("char id={id:<4} x={x:<5} y={y:<5} width={width:<5} height={height:<5} " + - "xoffset={xoffset:<5} yoffset={yoffset:<5} xadvance={xadvance:<5} page=0 chnl=15").format(**entry) - ) - path.write_text("\n".join(lines) + "\n") - - -def _process_font(font_path: Path, codepoints: tuple[int, ...]): - print(f"Processing {font_path.name}...") - - font_size = { - "unifont.otf": 16, # unifont is only 16x8 or 16x16 pixels per glyph - }.get(font_path.name, 200) - - data = font_path.read_bytes() - file_buf = rl.ffi.new("unsigned char[]", data) - cp_buffer = rl.ffi.new("int[]", codepoints) - cp_ptr = rl.ffi.cast("int *", cp_buffer) - glyphs = rl.load_font_data(rl.ffi.cast("unsigned char *", file_buf), len(data), font_size, cp_ptr, len(codepoints), rl.FontType.FONT_DEFAULT) - if glyphs == rl.ffi.NULL: - raise RuntimeError("raylib failed to load font data") - - rects_ptr = rl.ffi.new("Rectangle **") - image = rl.gen_image_font_atlas(glyphs, rects_ptr, len(codepoints), font_size, GLYPH_PADDING, 0) - if image.width == 0 or image.height == 0: - raise RuntimeError("raylib returned an empty atlas") - - rects = rects_ptr[0] - atlas_name = f"{font_path.stem}.png" - atlas_path = FONT_DIR / atlas_name - entries, line_height, base = _glyph_metrics(glyphs, rects, codepoints) - - if not rl.export_image(image, atlas_path.as_posix()): - raise RuntimeError("Failed to export atlas image") - - _write_bmfont(FONT_DIR / f"{font_path.stem}.fnt", font_size, font_path.stem, atlas_name, line_height, base, (image.width, image.height), entries) - - -def main(): - base_cp, unifont_cp = _char_sets() - fonts = sorted(FONT_DIR.glob("*.ttf")) + sorted(FONT_DIR.glob("*.otf")) - for font in fonts: - if "emoji" in font.name.lower(): - continue - glyphs = unifont_cp if font.stem.lower().startswith("unifont") else base_cp - _process_font(font, glyphs) - return 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/selfdrive/assets/fonts/unifont.otf b/selfdrive/assets/fonts/unifont.otf deleted file mode 100644 index b85597b1f4c3e3..00000000000000 --- a/selfdrive/assets/fonts/unifont.otf +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9712a9bc089af7ddc06e0826aa84f2ee23ed2f1a1dddaf2a89c2483e753a8475 -size 5321484 diff --git a/selfdrive/assets/icons/arrow-right.png b/selfdrive/assets/icons/arrow-right.png deleted file mode 100644 index b275134f648bd8..00000000000000 --- a/selfdrive/assets/icons/arrow-right.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0a999d5f3e616eeafc310689accdd26efb90769596604a62c460ef8acece18bc -size 1734 diff --git a/selfdrive/assets/icons/backspace.png b/selfdrive/assets/icons/backspace.png deleted file mode 100644 index 16686be6f54fd0..00000000000000 --- a/selfdrive/assets/icons/backspace.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:576da562df8eb513e64ccb614ec727b257cbca8b5507974d01efc0f64c5382c2 -size 6267 diff --git a/selfdrive/assets/icons/calibration.png b/selfdrive/assets/icons/calibration.png deleted file mode 100644 index d2a098ba5a5fc2..00000000000000 --- a/selfdrive/assets/icons/calibration.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0d68f2739bd1b8fc990e0d171923c58c382714c7d8ec6cd43f1f808ac87f963c -size 8910 diff --git a/selfdrive/assets/icons/capslock-fill.png b/selfdrive/assets/icons/capslock-fill.png deleted file mode 100644 index 66854e78f228bd..00000000000000 --- a/selfdrive/assets/icons/capslock-fill.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6872a1047f1a534a037be7b1367640fe1bfb205a6e1c50420a2d1a946cda78ed -size 4397 diff --git a/selfdrive/assets/icons/checkmark.png b/selfdrive/assets/icons/checkmark.png deleted file mode 100644 index 0f9a802a35b5b2..00000000000000 --- a/selfdrive/assets/icons/checkmark.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:dc472f0e575e314c4006cb6e9845fb9fb9a13cf08fe74fbe1593dee53c20d977 -size 4329 diff --git a/selfdrive/assets/icons/checkmark.svg b/selfdrive/assets/icons/checkmark.svg deleted file mode 100644 index 26480698cd3721..00000000000000 --- a/selfdrive/assets/icons/checkmark.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:17a135c18634647a73734300f5d0ad98082b07779b5553e72b99686857380ee7 -size 244 diff --git a/selfdrive/assets/icons/chevron_right.png b/selfdrive/assets/icons/chevron_right.png deleted file mode 100644 index 46baa1324058c7..00000000000000 --- a/selfdrive/assets/icons/chevron_right.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d4909d776263e223853ee49d9101e18dd220909cc8667c45bb040ea1d213ddf4 -size 1420 diff --git a/selfdrive/assets/icons/chffr_wheel.png b/selfdrive/assets/icons/chffr_wheel.png deleted file mode 100644 index 570ba4ee827622..00000000000000 --- a/selfdrive/assets/icons/chffr_wheel.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ed3225588c896efa7105778a5652962b18b2bc2d18e839b38a1753456671860d -size 16884 diff --git a/selfdrive/assets/icons/circled_check.png b/selfdrive/assets/icons/circled_check.png deleted file mode 100644 index 7611bc821f4730..00000000000000 --- a/selfdrive/assets/icons/circled_check.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fdb0be280ac3a78bf95f5b92fafe94de5084ecc06836459c3a9fe1912a5b2454 -size 10479 diff --git a/selfdrive/assets/icons/circled_check.svg b/selfdrive/assets/icons/circled_check.svg deleted file mode 100644 index aab06ec1e0f6f0..00000000000000 --- a/selfdrive/assets/icons/circled_check.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5c88458f6326265965626cbc97c2219bd513b15a6468b08f80b43ef014b7904b -size 372 diff --git a/selfdrive/assets/icons/circled_slash.png b/selfdrive/assets/icons/circled_slash.png deleted file mode 100644 index 74a9b342f67f41..00000000000000 --- a/selfdrive/assets/icons/circled_slash.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e2a992a83eaa87762e12dc226f36af48e1cdbfc3b83ef75b6a2fc4103e3697a0 -size 9120 diff --git a/selfdrive/assets/icons/circled_slash.svg b/selfdrive/assets/icons/circled_slash.svg deleted file mode 100644 index 89c6e2b1aed8a2..00000000000000 --- a/selfdrive/assets/icons/circled_slash.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4d91c86028af58cbe2770799d0fe7e55d143f9b3f67fdebcb47d328d6e410285 -size 223 diff --git a/selfdrive/assets/icons/close.png b/selfdrive/assets/icons/close.png deleted file mode 100644 index 66d15456321663..00000000000000 --- a/selfdrive/assets/icons/close.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7c11f831c17080a8ffaa8469cf91a079a4abfb72e5238afe02b92bceb3442db0 -size 2656 diff --git a/selfdrive/assets/icons/close.svg b/selfdrive/assets/icons/close.svg index e6db01321a82e7..b1e6d3b867580b 100644 --- a/selfdrive/assets/icons/close.svg +++ b/selfdrive/assets/icons/close.svg @@ -1,3 +1,4 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a28a4dcaba33d800d109cc5f9a810065203d3bfccd104b2e503f6fe3fc5b6f91 -size 250 + + + + diff --git a/selfdrive/assets/icons/close2.png b/selfdrive/assets/icons/close2.png deleted file mode 100644 index 4497c97547daf3..00000000000000 --- a/selfdrive/assets/icons/close2.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cbfed12ddb5731b7b568539fe59f382356b46fdd146a9e0a1b768d3e5efd0378 -size 4350 diff --git a/selfdrive/assets/icons/close2.svg b/selfdrive/assets/icons/close2.svg deleted file mode 100644 index 56a36cecd58bd2..00000000000000 --- a/selfdrive/assets/icons/close2.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7d2a7cd3913ab97a386e946969f6a684895adf5d641c38dfd8f5efa0197a6c58 -size 513 diff --git a/selfdrive/assets/icons/couch.png b/selfdrive/assets/icons/couch.png deleted file mode 100644 index 677ad0f9eef6dc..00000000000000 --- a/selfdrive/assets/icons/couch.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:911f59f248015600da7ecc689398103d47dfc57f6be17ac8c8e543a726a6c64b -size 3311 diff --git a/selfdrive/assets/icons/couch.svg b/selfdrive/assets/icons/couch.svg deleted file mode 100644 index f56970f289774b..00000000000000 --- a/selfdrive/assets/icons/couch.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:dd5d8d3fce5e30662797f7eeed224f5775b2cabcc055f01f5ebf6e7b2657b616 -size 1801 diff --git a/selfdrive/assets/icons/disengage_on_accelerator.png b/selfdrive/assets/icons/disengage_on_accelerator.png deleted file mode 100644 index 41348344489c01..00000000000000 --- a/selfdrive/assets/icons/disengage_on_accelerator.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f23fabbf60fff6ef88ba6f27f2775b1ae6be172a994e41267983a9ec0f984bfc -size 15059 diff --git a/selfdrive/assets/icons/disengage_on_accelerator.svg b/selfdrive/assets/icons/disengage_on_accelerator.svg deleted file mode 100644 index eef5181935f586..00000000000000 --- a/selfdrive/assets/icons/disengage_on_accelerator.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:baac01efc894527c8234b774c89cc57b69460d60ad81691a6d50ce0904c60ba7 -size 3638 diff --git a/selfdrive/assets/icons/driver_face.png b/selfdrive/assets/icons/driver_face.png deleted file mode 100644 index 8b6f515b1f8287..00000000000000 --- a/selfdrive/assets/icons/driver_face.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:63a9d5027779d9f1e82da718628933eb31e5072097337c42b09bf368b642ecb7 -size 3769 diff --git a/selfdrive/assets/icons/experimental.png b/selfdrive/assets/icons/experimental.png deleted file mode 100644 index 2332fe11d05acc..00000000000000 --- a/selfdrive/assets/icons/experimental.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1e58deb1778cf2826339f27e9f09eecc79ea137c1436210c14b71c352a649c77 -size 34953 diff --git a/selfdrive/assets/icons/experimental.svg b/selfdrive/assets/icons/experimental.svg deleted file mode 100644 index 8a97cdeac101b9..00000000000000 --- a/selfdrive/assets/icons/experimental.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b7ab989c9fe22e7d2119bac5284cdccfba2889477d69f5f6c6b4470c7ee0aab3 -size 1801 diff --git a/selfdrive/assets/icons/experimental_grey.png b/selfdrive/assets/icons/experimental_grey.png deleted file mode 100644 index 058c3e135811ff..00000000000000 --- a/selfdrive/assets/icons/experimental_grey.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8f37a02dd914405c6f86f415700dd5985eb976b923e7abd6580d2da76533594e -size 9466 diff --git a/selfdrive/assets/icons/experimental_grey.svg b/selfdrive/assets/icons/experimental_grey.svg deleted file mode 100644 index 25ab33f388b234..00000000000000 --- a/selfdrive/assets/icons/experimental_grey.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:90d441310f8e1833c661d8a4f0546aab2eb50eb21cc21beb0aff0d27b5ee6066 -size 1571 diff --git a/selfdrive/assets/icons/experimental_white.png b/selfdrive/assets/icons/experimental_white.png deleted file mode 100644 index 6a9cfc44078bfa..00000000000000 --- a/selfdrive/assets/icons/experimental_white.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b2dad33fead9f064c3a548651d6ef37daf82b6127c329683f538ec6e986ecbc -size 11204 diff --git a/selfdrive/assets/icons/experimental_white.svg b/selfdrive/assets/icons/experimental_white.svg deleted file mode 100644 index 51d76989478380..00000000000000 --- a/selfdrive/assets/icons/experimental_white.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:82648254da89edb0cf65ef63fc5e6e0741414051bfea8a40449132fd61b1ed0f -size 1533 diff --git a/selfdrive/assets/icons/eye_closed.png b/selfdrive/assets/icons/eye_closed.png deleted file mode 100644 index 8eba02e6008874..00000000000000 --- a/selfdrive/assets/icons/eye_closed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:64d9dc106172d3a54088ef51a27a48145154ca040c43ecbc8d626fa42e38886e -size 9352 diff --git a/selfdrive/assets/icons/eye_closed.svg b/selfdrive/assets/icons/eye_closed.svg deleted file mode 100644 index a9cb9251864263..00000000000000 --- a/selfdrive/assets/icons/eye_closed.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fcb0db3b77d6057b94544b6d91b6078fbd91b8f2de189322b90feae3d1ac29da -size 1054 diff --git a/selfdrive/assets/icons/eye_open.png b/selfdrive/assets/icons/eye_open.png deleted file mode 100644 index 9783716ff3476b..00000000000000 --- a/selfdrive/assets/icons/eye_open.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:94246db66e774cbaee618931f76ecc38ecb72eca097d1e6c20a8dec2a5f8cd29 -size 7087 diff --git a/selfdrive/assets/icons/eye_open.svg b/selfdrive/assets/icons/eye_open.svg deleted file mode 100644 index 2befa13adb1f0f..00000000000000 --- a/selfdrive/assets/icons/eye_open.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:411774f4a80831833a344e58932c23a6ebd1db73a6327a63259deca2c6f53613 -size 597 diff --git a/selfdrive/assets/icons/eyes_crossed.png b/selfdrive/assets/icons/eyes_crossed.png deleted file mode 100644 index af2122cd9ac2dd..00000000000000 --- a/selfdrive/assets/icons/eyes_crossed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4def42b5faffc6a8f747c210d24c3a1a8a7f82891738ff7f3317091e63326ba5 -size 1083 diff --git a/selfdrive/assets/icons/eyes_open.png b/selfdrive/assets/icons/eyes_open.png deleted file mode 100644 index ad9afc3a3e01b0..00000000000000 --- a/selfdrive/assets/icons/eyes_open.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:52019b72834e478588114584820313af866d2d7a737591a166c413ccaab6acf5 -size 931 diff --git a/selfdrive/assets/icons/link.png b/selfdrive/assets/icons/link.png deleted file mode 100644 index 8a795c05b8fa7b..00000000000000 --- a/selfdrive/assets/icons/link.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a69514fe68dd1b4c73f28771640dcba10fc40989d1c7e771cb48bfd830fef206 -size 5713 diff --git a/selfdrive/assets/icons/lock_closed.png b/selfdrive/assets/icons/lock_closed.png deleted file mode 100644 index 21e67957374fda..00000000000000 --- a/selfdrive/assets/icons/lock_closed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3b89b8803bb610515aef051c93b833dc62f8c847558873cfd50a0b240c968449 -size 4911 diff --git a/selfdrive/assets/icons/lock_closed.svg b/selfdrive/assets/icons/lock_closed.svg deleted file mode 100644 index 22a510d1c2c210..00000000000000 --- a/selfdrive/assets/icons/lock_closed.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:64e8fc90b79c725a8bf5bbc99643989ae885570997092762e216738805649ade -size 492 diff --git a/selfdrive/assets/icons/menu.png b/selfdrive/assets/icons/menu.png deleted file mode 100644 index d43db96cd8accf..00000000000000 --- a/selfdrive/assets/icons/menu.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ae8c4922909415dcf51cca6ac54790d3019cc934bf719bda112aa7a9cba8eae3 -size 635 diff --git a/selfdrive/assets/icons/metric.png b/selfdrive/assets/icons/metric.png deleted file mode 100644 index 0d38b0478f36b0..00000000000000 --- a/selfdrive/assets/icons/metric.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f27f3dfeaa105c7757b5211cbee99b8e0c86cd059f13ac7e9a0807374f1633e7 -size 604 diff --git a/selfdrive/assets/icons/microphone.png b/selfdrive/assets/icons/microphone.png deleted file mode 100644 index 6cb9cc02543757..00000000000000 --- a/selfdrive/assets/icons/microphone.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9fc1f7f31d41f26ea7d6f52b3096f7a91844a3b897bc233a8489253c46f0403b -size 6324 diff --git a/selfdrive/assets/icons/minus.png b/selfdrive/assets/icons/minus.png deleted file mode 100644 index c2abe3ae90f886..00000000000000 --- a/selfdrive/assets/icons/minus.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8d2c5640258911c94475615b9c4abc1485c74239f65bef70e0bab9ca84619772 -size 2577 diff --git a/selfdrive/assets/icons/monitoring.png b/selfdrive/assets/icons/monitoring.png deleted file mode 100644 index 39d52de13d6b4d..00000000000000 --- a/selfdrive/assets/icons/monitoring.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0bcd5b336b2112f43a6f63937aedec4a85047f14a8c3f482e0f85988e1abbeea -size 58679 diff --git a/selfdrive/assets/icons/network.png b/selfdrive/assets/icons/network.png deleted file mode 100644 index 71ccc89aacd56d..00000000000000 --- a/selfdrive/assets/icons/network.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6eec8334979c2ba5751560a9b6b322d2a5852970b0f783b68112e92bb7c82826 -size 39872 diff --git a/selfdrive/assets/icons/road.png b/selfdrive/assets/icons/road.png deleted file mode 100644 index 7e460dc0d0732f..00000000000000 --- a/selfdrive/assets/icons/road.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:735fa47333bddaa91af6d120fd2f76dc562657962abdc55de810d9a77d6e5516 -size 6674 diff --git a/selfdrive/assets/icons/settings.png b/selfdrive/assets/icons/settings.png deleted file mode 100644 index 3d8159acc66b1c..00000000000000 --- a/selfdrive/assets/icons/settings.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a522be46c662753063fce319bd29f252df1b2ec082eb4dbd6518d59bdf6a5fa4 -size 13369 diff --git a/selfdrive/assets/icons/shell.png b/selfdrive/assets/icons/shell.png deleted file mode 100644 index ea4faf89b9754b..00000000000000 --- a/selfdrive/assets/icons/shell.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c37ee59b9d273193cedae3408ce29b22d8b2d27732235c62065e78c5eca5812d -size 42462 diff --git a/selfdrive/assets/icons/shift-fill.png b/selfdrive/assets/icons/shift-fill.png deleted file mode 100644 index 1ce02d58225e9c..00000000000000 --- a/selfdrive/assets/icons/shift-fill.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e625ca991746abaaac375b095aa9a586601982232f4aae0fc2b17b2a524e9ce9 -size 3946 diff --git a/selfdrive/assets/icons/shift.png b/selfdrive/assets/icons/shift.png deleted file mode 100644 index de2a68b482c3ae..00000000000000 --- a/selfdrive/assets/icons/shift.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a93dd816bd0600ad47d10ebe530326bfa725dc31e4d5e1ee275f39b10f17a59d -size 4931 diff --git a/selfdrive/assets/icons/speed_limit.png b/selfdrive/assets/icons/speed_limit.png deleted file mode 100644 index d4c662cfc0ae30..00000000000000 --- a/selfdrive/assets/icons/speed_limit.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:28a131696208124a61d08f49f2ad6f76e783a405354a8c52a7fa1b9d87fd4e51 -size 3321 diff --git a/selfdrive/assets/icons/triangle.png b/selfdrive/assets/icons/triangle.png deleted file mode 100644 index 47ff24f200305b..00000000000000 --- a/selfdrive/assets/icons/triangle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5f2745ce89c926e507888ea7c6df1884aab045887048cf0d813407396a2e6b18 -size 5894 diff --git a/selfdrive/assets/icons/triangle.svg b/selfdrive/assets/icons/triangle.svg deleted file mode 100644 index 233eb5e979a701..00000000000000 --- a/selfdrive/assets/icons/triangle.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a83c9a78673429caf16d02917be4c8afedadff17b6ceb8c248703ad1120116ed -size 394 diff --git a/selfdrive/assets/icons/warning.png b/selfdrive/assets/icons/warning.png deleted file mode 100644 index 583f9c2443ac6f..00000000000000 --- a/selfdrive/assets/icons/warning.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1535245dbb4e102e07edda7c41e3f993a521e8a18711c6c797ec82c2a94e7db8 -size 8002 diff --git a/selfdrive/assets/icons/wifi_strength_full.png b/selfdrive/assets/icons/wifi_strength_full.png deleted file mode 100644 index 1a710f29676de8..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_full.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8b7f0971cf612b905ccb338e40921932773538517fc9f0f7a4a847ad596287a9 -size 7171 diff --git a/selfdrive/assets/icons/wifi_strength_full.svg b/selfdrive/assets/icons/wifi_strength_full.svg deleted file mode 100644 index 4137d3cd4c1d6a..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_full.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3c16c005d666dea64ba6f85de50dfcf161353176eca3d9f475677cbc04fd0382 -size 1161 diff --git a/selfdrive/assets/icons/wifi_strength_high.png b/selfdrive/assets/icons/wifi_strength_high.png deleted file mode 100644 index 2d360968b630a1..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_high.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a2afacf302bcdc3f5e0d2f734508e8fea6f803813098d52fa6b878c765ba7a58 -size 9360 diff --git a/selfdrive/assets/icons/wifi_strength_high.svg b/selfdrive/assets/icons/wifi_strength_high.svg deleted file mode 100644 index 17d721fe31d4a3..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_high.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0f207b9a1f4ab6009e261f03edd1d05a9d485217415664cba0253d32381e3a03 -size 1164 diff --git a/selfdrive/assets/icons/wifi_strength_low.png b/selfdrive/assets/icons/wifi_strength_low.png deleted file mode 100644 index d0165283043bb5..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_low.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7dfe50815c76d459dc104ce4a5e4b5dfd882e11b65e07706cacd336e69e788f4 -size 9756 diff --git a/selfdrive/assets/icons/wifi_strength_low.svg b/selfdrive/assets/icons/wifi_strength_low.svg deleted file mode 100644 index 4088866370cc88..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_low.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6835937483e5a539618a94e3ec8fad661ae2f7afb39c958e6c8b007b8f4b9b2c -size 1198 diff --git a/selfdrive/assets/icons/wifi_strength_medium.png b/selfdrive/assets/icons/wifi_strength_medium.png deleted file mode 100644 index 9c943543a4500d..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_medium.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:304ab97ac9724a7a133a36c2d14da9fe8ab660c9a29abb99cc0a4828f94d8801 -size 9627 diff --git a/selfdrive/assets/icons/wifi_strength_medium.svg b/selfdrive/assets/icons/wifi_strength_medium.svg deleted file mode 100644 index f0c029dedd64e2..00000000000000 --- a/selfdrive/assets/icons/wifi_strength_medium.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7ebb1fb5539d9f8ea9899acf4f9cb359503dbe22cd38a3461ad43936348475b9 -size 1167 diff --git a/selfdrive/assets/icons/wifi_uploading.png b/selfdrive/assets/icons/wifi_uploading.png deleted file mode 100644 index 19d9bf36ce167f..00000000000000 --- a/selfdrive/assets/icons/wifi_uploading.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8eac201013322db1580649a253da7ae38eb9f16f6089234e769b746951e874ee -size 7171 diff --git a/selfdrive/assets/icons/wifi_uploading.svg b/selfdrive/assets/icons/wifi_uploading.svg deleted file mode 100644 index 07a14a59f4c6b0..00000000000000 --- a/selfdrive/assets/icons/wifi_uploading.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e7cfefbda22b53f7eb72a25893ab41c00dabea49f690cafb1d224375a669e608 -size 1170 diff --git a/selfdrive/assets/icons_mici/adb_short.png b/selfdrive/assets/icons_mici/adb_short.png deleted file mode 100644 index c49226c858a755..00000000000000 --- a/selfdrive/assets/icons_mici/adb_short.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:263598da73c577c01cebd31ae78f45969ef8b335be1a5f55d54a696bb2982c0a -size 2062 diff --git a/selfdrive/assets/icons_mici/buttons/button_circle.png b/selfdrive/assets/icons_mici/buttons/button_circle.png deleted file mode 100644 index b6f4cc9d12b0fd..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_circle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f92e5f0b7fc50c3b64bd18ecee8a8d518017b5461104de76dee6feb0f4f0d70d -size 7496 diff --git a/selfdrive/assets/icons_mici/buttons/button_circle_disabled.png b/selfdrive/assets/icons_mici/buttons/button_circle_disabled.png deleted file mode 100644 index d2104df4e1c891..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_circle_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:947aa3beb7eff6afb44101daf0aeaae7b7f31961c273df00eec0ca8359233c56 -size 5175 diff --git a/selfdrive/assets/icons_mici/buttons/button_circle_hover.png b/selfdrive/assets/icons_mici/buttons/button_circle_hover.png deleted file mode 100644 index 5cae1521065419..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_circle_hover.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:20024203288f144633014422e16119278477099f24fba5c155a804a1864a26b4 -size 7511 diff --git a/selfdrive/assets/icons_mici/buttons/button_circle_red.png b/selfdrive/assets/icons_mici/buttons/button_circle_red.png deleted file mode 100644 index 68ae400b1eb4ec..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_circle_red.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b48d8a191979f27dae8a336f99d944008e2536f698c58cffa5f3dddc17429b45 -size 11451 diff --git a/selfdrive/assets/icons_mici/buttons/button_circle_red_hover.png b/selfdrive/assets/icons_mici/buttons/button_circle_red_hover.png deleted file mode 100644 index 3696334d5e2eec..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_circle_red_hover.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:279c1d8f95eb9f4a3058dff76b0f316ce9eef7bc8f4296936ad25fd08703ce13 -size 10380 diff --git a/selfdrive/assets/icons_mici/buttons/button_rectangle.png b/selfdrive/assets/icons_mici/buttons/button_rectangle.png deleted file mode 100644 index 230c537d6dcd0d..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_rectangle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ffb293236f5f8f7da44b5a3c4c0b72e86c4e1fdb04f89c94507af008ff7de139 -size 8210 diff --git a/selfdrive/assets/icons_mici/buttons/button_rectangle_disabled.png b/selfdrive/assets/icons_mici/buttons/button_rectangle_disabled.png deleted file mode 100644 index 76e75d5421eb35..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_rectangle_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bda53863c9a46c50a1e2920a76c2d2f1fe4df8a94b8d2e26f5d83eef3a9c3bd3 -size 3627 diff --git a/selfdrive/assets/icons_mici/buttons/button_rectangle_hover.png b/selfdrive/assets/icons_mici/buttons/button_rectangle_hover.png deleted file mode 100644 index a9fd28cc35e34d..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_rectangle_hover.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b55e43c50e805ac5e8357e5943374ed02d756cefa3aaffb58c568a0b125c30b -size 7750 diff --git a/selfdrive/assets/icons_mici/buttons/button_rectangle_pressed.png b/selfdrive/assets/icons_mici/buttons/button_rectangle_pressed.png deleted file mode 100644 index 779c219fcbd7d6..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_rectangle_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5528e9c041b824f005bf1ef6e49b2dbbc4ba10f994b0726d2a17a4fbf8c80f55 -size 21379 diff --git a/selfdrive/assets/icons_mici/buttons/button_side_back.png b/selfdrive/assets/icons_mici/buttons/button_side_back.png deleted file mode 100644 index 3d648d34f1a01d..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_side_back.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9df44871e9f5fa910622b0b92205b92a54d137dbdc3827b92e8622d85ff2e08e -size 5189 diff --git a/selfdrive/assets/icons_mici/buttons/button_side_back_pressed.png b/selfdrive/assets/icons_mici/buttons/button_side_back_pressed.png deleted file mode 100644 index e431cb0c7395ae..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_side_back_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:013b368b38b17d9b2ef6aaf0f498f672deed95888084b7287f42bdfba617cbb6 -size 10142 diff --git a/selfdrive/assets/icons_mici/buttons/button_side_check.png b/selfdrive/assets/icons_mici/buttons/button_side_check.png deleted file mode 100644 index 820b2360665a96..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_side_check.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8fd563eec78d5ce4a8204c2f596789e1090cb3e26a35b4ffeacee4ab61968538 -size 8303 diff --git a/selfdrive/assets/icons_mici/buttons/button_side_check_pressed.png b/selfdrive/assets/icons_mici/buttons/button_side_check_pressed.png deleted file mode 100644 index 6c38508af956a5..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/button_side_check_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0be8d5eddcd9f87acbf1daccf446be6218522120f64aee1ee0a3c0b31560f076 -size 15761 diff --git a/selfdrive/assets/icons_mici/buttons/slider_bg.png b/selfdrive/assets/icons_mici/buttons/slider_bg.png deleted file mode 100644 index 9164f74bad14b9..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/slider_bg.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1ca620a05e9e69351b9bbcfcf021dae11fde26be50d7f1a39257d319f6303616 -size 9779 diff --git a/selfdrive/assets/icons_mici/buttons/toggle_dot_disabled.png b/selfdrive/assets/icons_mici/buttons/toggle_dot_disabled.png deleted file mode 100644 index 1ff4db45a55f8a..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/toggle_dot_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:89ac033d879beeb0a7fa1919838e0ec64b1a625a4aafc14f7b990c607a79b676 -size 2220 diff --git a/selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png b/selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png deleted file mode 100644 index 5bb4d778f8132d..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:532bf0e8535e3f9bc13af13029a27d6c14ae788d52224b6c65623334f62fada0 -size 6048 diff --git a/selfdrive/assets/icons_mici/buttons/toggle_pill_disabled.png b/selfdrive/assets/icons_mici/buttons/toggle_pill_disabled.png deleted file mode 100644 index 555c16e095f71d..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/toggle_pill_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7891a628bd9cedc1097114e89fcec4a50a88021c7d6c63f1329d087be9e1783e -size 3065 diff --git a/selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png b/selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png deleted file mode 100644 index d95039da9282fc..00000000000000 --- a/selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ad31da78544edd18d0ac154670f22d1cd1ac57f50576002f04701d22c59502a8 -size 8257 diff --git a/selfdrive/assets/icons_mici/exclamation_point.png b/selfdrive/assets/icons_mici/exclamation_point.png deleted file mode 100644 index ede3b638bc3615..00000000000000 --- a/selfdrive/assets/icons_mici/exclamation_point.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:254b7f753b70c964847b686f0f71af751f2f49beea6ede4aeb333fe06062a257 -size 2289 diff --git a/selfdrive/assets/icons_mici/experimental_mode.png b/selfdrive/assets/icons_mici/experimental_mode.png deleted file mode 100644 index 75850d08f51f04..00000000000000 --- a/selfdrive/assets/icons_mici/experimental_mode.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:01841b602632c66ab14a8e52b874a1623f09641dc2ef0620f4e2d00bb4a913f3 -size 16243 diff --git a/selfdrive/assets/icons_mici/microphone.png b/selfdrive/assets/icons_mici/microphone.png deleted file mode 100644 index 9af8f2f455290d..00000000000000 --- a/selfdrive/assets/icons_mici/microphone.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:744dbaa68ee74e300cd46439bad79449c860e1c5c027304b0f382bd5383fba77 -size 6817 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/big_alert.png b/selfdrive/assets/icons_mici/offroad_alerts/big_alert.png deleted file mode 100644 index 142367d0e68eae..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/big_alert.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:aeee7f049879caff52320fab5f286cf3fd6a52c820cd8e150ff242e53a14176f -size 14774 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/big_alert_pressed.png b/selfdrive/assets/icons_mici/offroad_alerts/big_alert_pressed.png deleted file mode 100644 index 2ff01024d9f6df..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/big_alert_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0b59ddada9c9e0e7972ead27396ebe6c10fd2352687b18ff6b476f61f74d80bf -size 46106 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png b/selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png deleted file mode 100644 index 08181ca35f47c8..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3b11ee84d48972a2499cb29f01594d77a1a39692f6424a315a3f83262bc16087 -size 13481 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/medium_alert.png b/selfdrive/assets/icons_mici/offroad_alerts/medium_alert.png deleted file mode 100644 index 91a7b43c5a6890..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/medium_alert.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:46d08a8a08b42d466ff45d8ad6d2578e345dcbb1c06c126ad361873d9d35eaec -size 12877 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/medium_alert_pressed.png b/selfdrive/assets/icons_mici/offroad_alerts/medium_alert_pressed.png deleted file mode 100644 index 09ca2d08d5eb0e..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/medium_alert_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:42ea275e5fe0a8a0e2fddb5a4a8487806fb22850115ea3646ee8f213d5fd6bb6 -size 37202 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/orange_warning.png b/selfdrive/assets/icons_mici/offroad_alerts/orange_warning.png deleted file mode 100644 index 52e6836d4b4804..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/orange_warning.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d548405a65ba4d4590c55866612dc6aa0e78d9278fc864ef60fe3e463edf4a68 -size 12169 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/red_warning.png b/selfdrive/assets/icons_mici/offroad_alerts/red_warning.png deleted file mode 100644 index df608d3518b747..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/red_warning.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b6fc63326d34fbe72f6daf104d101ce19e547dbfe134427c067c957a7179df74 -size 12124 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/small_alert.png b/selfdrive/assets/icons_mici/offroad_alerts/small_alert.png deleted file mode 100644 index 0a50b6a1ca94ee..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/small_alert.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cfbf38672a893fd1d8fadf942354d2511e2436814a9af0e5c188cbb427fc6c70 -size 12281 diff --git a/selfdrive/assets/icons_mici/offroad_alerts/small_alert_pressed.png b/selfdrive/assets/icons_mici/offroad_alerts/small_alert_pressed.png deleted file mode 100644 index 865355ef01f57a..00000000000000 --- a/selfdrive/assets/icons_mici/offroad_alerts/small_alert_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a7102850bcfb075a285041cecb546559374a905403ab3b9814fa6097d2d822dd -size 34680 diff --git a/selfdrive/assets/icons_mici/onroad/blind_spot_left.png b/selfdrive/assets/icons_mici/onroad/blind_spot_left.png deleted file mode 100644 index fdc189b858274f..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/blind_spot_left.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:77b20a8c478d982412d556afb3a035b80b4aa9fe7a86aea761af4a42147d9435 -size 45297 diff --git a/selfdrive/assets/icons_mici/onroad/blind_spot_right.png b/selfdrive/assets/icons_mici/onroad/blind_spot_right.png deleted file mode 100644 index b6cd7834ef505e..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/blind_spot_right.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:584cea202afff6dd20d67ae1a9cd6d2b8cc07598bccb91a8d1bac0142567308e -size 45489 diff --git a/selfdrive/assets/icons_mici/onroad/bookmark.png b/selfdrive/assets/icons_mici/onroad/bookmark.png deleted file mode 100644 index 305561f509c5b7..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/bookmark.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fd91685bf656e828648acf035a4737acb2c4709e8514cf0aa0a10fa470a9bb60 -size 11580 diff --git a/selfdrive/assets/icons_mici/onroad/bookmark_fill.png b/selfdrive/assets/icons_mici/onroad/bookmark_fill.png deleted file mode 100644 index 531d5db1cfbfb5..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/bookmark_fill.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f3f57346a1cf9a66f9fd746f87bcebb23b7a403e9d6e4fd7701b126abcdd47ea -size 18476 diff --git a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_background.png b/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_background.png deleted file mode 100644 index 4129b13d922c90..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_background.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cb89d9f11cf44992f92142aa5ad84e1ac700a2601aff2abab373e2a822af149e -size 11678 diff --git a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png b/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png deleted file mode 100644 index a8a68b372c26a9..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b5aee9f6cec03f1967014cd2ea2a23982b262e7d86dadca602ecfa8875b38101 -size 5875 diff --git a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png b/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png deleted file mode 100644 index ec2f9489981635..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:26b3660dbd1e60b0ba98914afa7cb3a67151bb6990d218f55c901f243e38ff3e -size 3631 diff --git a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_person.png b/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_person.png deleted file mode 100644 index 5b917f3a4a8442..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_person.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e2772c6a9fe9c57099d347ad49f0cb7c906593f1fdf0e6dde96d104baf0200b0 -size 1365 diff --git a/selfdrive/assets/icons_mici/onroad/eye_fill.png b/selfdrive/assets/icons_mici/onroad/eye_fill.png deleted file mode 100644 index 78758a9809caf0..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/eye_fill.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:07310879d093108435c0011846ae1184966db86443bc6e7ca036a6fa6123700b -size 4983 diff --git a/selfdrive/assets/icons_mici/onroad/eye_orange.png b/selfdrive/assets/icons_mici/onroad/eye_orange.png deleted file mode 100644 index 932c71260b446b..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/eye_orange.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7be447e56d649e0362ef650494b484e140a01ead31799ce43b266f5781c918d2 -size 36473 diff --git a/selfdrive/assets/icons_mici/onroad/glasses.png b/selfdrive/assets/icons_mici/onroad/glasses.png deleted file mode 100644 index 006972fd397c41..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/glasses.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:56de402482b5987ed9a0ff3f793a1c89f857304b34fbb8a3deb5b5d4a332be1c -size 3688 diff --git a/selfdrive/assets/icons_mici/onroad/onroad_fade.png b/selfdrive/assets/icons_mici/onroad/onroad_fade.png deleted file mode 100644 index 3f823061b9b8ec..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/onroad_fade.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2aa6d04ba038f15a92868de6e6c7b04f624b4fe89d03bc3e9c4cd44cb729b24e -size 38317 diff --git a/selfdrive/assets/icons_mici/onroad/turn_signal_left.png b/selfdrive/assets/icons_mici/onroad/turn_signal_left.png deleted file mode 100644 index 97b5cf1443f2cf..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/turn_signal_left.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f9f7d0554c0c79ab605c1119ffdef0a4f55196e53b75a65b6ac5218911e24a02 -size 45701 diff --git a/selfdrive/assets/icons_mici/onroad/turn_signal_right.png b/selfdrive/assets/icons_mici/onroad/turn_signal_right.png deleted file mode 100644 index 6bcb68dac55457..00000000000000 --- a/selfdrive/assets/icons_mici/onroad/turn_signal_right.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7fae4872ab3c24d5e4c2be6150127a844f89bbdcadfccdff2dfed180e125d577 -size 45699 diff --git a/selfdrive/assets/icons_mici/settings.png b/selfdrive/assets/icons_mici/settings.png deleted file mode 100644 index 4ba7df9fdf65f5..00000000000000 --- a/selfdrive/assets/icons_mici/settings.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:14b457d2dc19d8658f525cc6989c9cfcf0edaf695b18767514242acbdbe2a6dd -size 2198 diff --git a/selfdrive/assets/icons_mici/settings/comma_icon.png b/selfdrive/assets/icons_mici/settings/comma_icon.png deleted file mode 100644 index dd38a8938f6440..00000000000000 --- a/selfdrive/assets/icons_mici/settings/comma_icon.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7ad4ee47ec6470f788a026f95ed86bf344f64f9cf3186c9c78927233d2694a1d -size 1388 diff --git a/selfdrive/assets/icons_mici/settings/developer/ssh.png b/selfdrive/assets/icons_mici/settings/developer/ssh.png deleted file mode 100644 index 0f17d04eca8fd2..00000000000000 --- a/selfdrive/assets/icons_mici/settings/developer/ssh.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b26133bee089627202d5e89a4e939ad23aaceb5d8e26d7381b1aea3ef892f2ee -size 2620 diff --git a/selfdrive/assets/icons_mici/settings/developer_icon.png b/selfdrive/assets/icons_mici/settings/developer_icon.png deleted file mode 100644 index f9d553c7c30f6d..00000000000000 --- a/selfdrive/assets/icons_mici/settings/developer_icon.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ebb4f7ad9fd2f9fb3c69a38fbc00cbe690809b0ff202ffd4768ae5b699acc035 -size 1759 diff --git a/selfdrive/assets/icons_mici/settings/device/cameras.png b/selfdrive/assets/icons_mici/settings/device/cameras.png deleted file mode 100644 index ae9a88c4dc8d0a..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/cameras.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5f47e636025e044977f278a35546e0fc971f48fd53c2eeafd3508e95c35f378f -size 3117 diff --git a/selfdrive/assets/icons_mici/settings/device/fcc_logo.png b/selfdrive/assets/icons_mici/settings/device/fcc_logo.png deleted file mode 100644 index f29b24fd099208..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/fcc_logo.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3cac8546d19e75a9edcbc0721a887fd74c8a3c41bfe19e36186b2b2bcabdae98 -size 1817 diff --git a/selfdrive/assets/icons_mici/settings/device/info.png b/selfdrive/assets/icons_mici/settings/device/info.png deleted file mode 100644 index 9a29c46d0d2c35..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/info.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:66858a5d3302333485fa391f7a9bb3a9b1ab4ae881e7fb47b04c3a4507011c94 -size 2613 diff --git a/selfdrive/assets/icons_mici/settings/device/language.png b/selfdrive/assets/icons_mici/settings/device/language.png deleted file mode 100644 index d2ef27de36b02d..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/language.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f646263b26de46f79cac836ef6865b0f25ddc91e386b99311723b68bd06693c9 -size 3304 diff --git a/selfdrive/assets/icons_mici/settings/device/lkas.png b/selfdrive/assets/icons_mici/settings/device/lkas.png deleted file mode 100644 index 80d37d4d5c1a4a..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/lkas.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a05a41e66c7a24d461a4bbcdab0979031e5900e1db270af52ca363f0bed521f5 -size 2028 diff --git a/selfdrive/assets/icons_mici/settings/device/pair.png b/selfdrive/assets/icons_mici/settings/device/pair.png deleted file mode 100644 index 807d44335dcc67..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/pair.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:678483230831d0a7d3dcad5f067a7b641e5d2ae0db477665dfc6c53a675eba18 -size 1779 diff --git a/selfdrive/assets/icons_mici/settings/device/power.png b/selfdrive/assets/icons_mici/settings/device/power.png deleted file mode 100644 index 711f1a4ab9b997..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/power.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a34885e79f42d19b7777dd07e7ab51df344880cb770c48e0baaddb177c2ae938 -size 2228 diff --git a/selfdrive/assets/icons_mici/settings/device/reboot.png b/selfdrive/assets/icons_mici/settings/device/reboot.png deleted file mode 100644 index 298a85c5041d86..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/reboot.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1356fe3ddda14568e9be1dca4e16ca9048852e3a27a3f531cd58d7d368485a82 -size 2362 diff --git a/selfdrive/assets/icons_mici/settings/device/uninstall.png b/selfdrive/assets/icons_mici/settings/device/uninstall.png deleted file mode 100644 index 53f8bc0e7d30cb..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/uninstall.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:50a8ce4fa8ff7f5b0f56ba0dc65b4802dc0be2dc0967b5cb3a15e3b79a4e513e -size 2424 diff --git a/selfdrive/assets/icons_mici/settings/device/up_to_date.png b/selfdrive/assets/icons_mici/settings/device/up_to_date.png deleted file mode 100644 index e09f7d33085d99..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/up_to_date.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:61bc44b6e0f99640434d6abcb64880c7bf575eda5cdcf7d74cba7d73307dd39a -size 2739 diff --git a/selfdrive/assets/icons_mici/settings/device/update.png b/selfdrive/assets/icons_mici/settings/device/update.png deleted file mode 100644 index 498c066191a021..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device/update.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f28cdeaba9146521335bc11ad60a8e0368eb0ed1381e88b35a12a6138ba22ed6 -size 2409 diff --git a/selfdrive/assets/icons_mici/settings/device_icon.png b/selfdrive/assets/icons_mici/settings/device_icon.png deleted file mode 100644 index 6a716e4dfde507..00000000000000 --- a/selfdrive/assets/icons_mici/settings/device_icon.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2273629450aa870f0964dd285721c35d3d313fb8b4684122215a65844ae744d0 -size 1888 diff --git a/selfdrive/assets/icons_mici/settings/firehose.png b/selfdrive/assets/icons_mici/settings/firehose.png deleted file mode 100644 index 37451c0482c186..00000000000000 --- a/selfdrive/assets/icons_mici/settings/firehose.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:416656861380981acc114e5285b448d6e4dc42b98539d0ba16821cbc3db89208 -size 1364 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/backspace.png b/selfdrive/assets/icons_mici/settings/keyboard/backspace.png deleted file mode 100644 index 53ff00c2ae7ff9..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/backspace.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:69bb4a401429c3fdf473778f751288b2aafea27eb13f09b20e83d55212f084ba -size 1963 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/caps_lock.png b/selfdrive/assets/icons_mici/settings/keyboard/caps_lock.png deleted file mode 100644 index 2d173bfc9fa5b8..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/caps_lock.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:563c211fd98018e24418235602e596f3a481f04fddde0a14590e563474fcffd2 -size 1423 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/caps_lower.png b/selfdrive/assets/icons_mici/settings/keyboard/caps_lower.png deleted file mode 100644 index a3ce71f04924c0..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/caps_lower.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f81811ea9cdc409d5549035ca928c76e22396193e1cefb6cacab3747ee0c297 -size 1142 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/caps_upper.png b/selfdrive/assets/icons_mici/settings/keyboard/caps_upper.png deleted file mode 100644 index 7c147bc07bb05e..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/caps_upper.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:60875e73dd9659122c9248d8e99d5cfd301d68dabeec2cb42cebce812c9baae9 -size 1102 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/enter.png b/selfdrive/assets/icons_mici/settings/keyboard/enter.png deleted file mode 100644 index 0b7fc95c510e84..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/enter.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3dd956d5ccfce01a01bea74ef59c9e73dfca406a5ff9ac62417203afa6027fba -size 5620 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png b/selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png deleted file mode 100644 index 251d5d8d14027d..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1dd1c2308872729d58adab390030ae9c987dc7908f0c39391651ea2b6cb620c5 -size 2445 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/keyboard_background.png b/selfdrive/assets/icons_mici/settings/keyboard/keyboard_background.png deleted file mode 100644 index 8c2c068d4131cf..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/keyboard_background.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:399e7ff9dea6710244827c91014f1a08d8ae989dce922928d6b7f7504b15ba79 -size 11321 diff --git a/selfdrive/assets/icons_mici/settings/keyboard/space.png b/selfdrive/assets/icons_mici/settings/keyboard/space.png deleted file mode 100644 index 3d61109721b5ea..00000000000000 --- a/selfdrive/assets/icons_mici/settings/keyboard/space.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f431e428772991323ee3ce662479e1ab29c3d80a72b93cf9c9673716ba245d5f -size 654 diff --git a/selfdrive/assets/icons_mici/settings/network/cell_strength_full.png b/selfdrive/assets/icons_mici/settings/network/cell_strength_full.png deleted file mode 100644 index 13f70386d44fa3..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/cell_strength_full.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fb7af523411c5ed75c6e1418dfc2a379486f6dbd7f2f1c281d3ff54e1ea7810e -size 777 diff --git a/selfdrive/assets/icons_mici/settings/network/cell_strength_high.png b/selfdrive/assets/icons_mici/settings/network/cell_strength_high.png deleted file mode 100644 index 1fea6d23b809b5..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/cell_strength_high.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:db86e176e016458fcff00d40e37636a808977e0cc01bcc9c04b31a1001562de8 -size 936 diff --git a/selfdrive/assets/icons_mici/settings/network/cell_strength_low.png b/selfdrive/assets/icons_mici/settings/network/cell_strength_low.png deleted file mode 100644 index d763f86c7fa313..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/cell_strength_low.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1cd0b3a00db36ee7eacf5887d07d40e5351fb441d98643a02df4c742cd1e935d -size 945 diff --git a/selfdrive/assets/icons_mici/settings/network/cell_strength_medium.png b/selfdrive/assets/icons_mici/settings/network/cell_strength_medium.png deleted file mode 100644 index 148ee63e990d17..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/cell_strength_medium.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:25724acfe0c261070b103ef5933053d5dd8b726ece42d0e5f715f05c67be2294 -size 956 diff --git a/selfdrive/assets/icons_mici/settings/network/cell_strength_none.png b/selfdrive/assets/icons_mici/settings/network/cell_strength_none.png deleted file mode 100644 index c6d82ac316eb8f..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/cell_strength_none.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cb0aeb6260bcd0642204f842112479f4b19b350db9addae5e14c9c5131bcf956 -size 781 diff --git a/selfdrive/assets/icons_mici/settings/network/new/connect_button.png b/selfdrive/assets/icons_mici/settings/network/new/connect_button.png deleted file mode 100644 index eae5af77f09381..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/connect_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:04236fa0f2759a01c6e321ac7b1c86c7a039215a7953b1a23d250ecf2ef1fa87 -size 8563 diff --git a/selfdrive/assets/icons_mici/settings/network/new/connect_button_pressed.png b/selfdrive/assets/icons_mici/settings/network/new/connect_button_pressed.png deleted file mode 100644 index 0da6c384d91a1c..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/connect_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4337098554af30c98ebd512e17ab08207db868ff34acca5f865fcbfc940286d3 -size 21123 diff --git a/selfdrive/assets/icons_mici/settings/network/new/forget_button.png b/selfdrive/assets/icons_mici/settings/network/new/forget_button.png deleted file mode 100644 index 541433be763012..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/forget_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6ccb5f2298389ae36df87de84d85440ee5a82c50e803c9bd362c9b89ea45aa69 -size 6611 diff --git a/selfdrive/assets/icons_mici/settings/network/new/forget_button_pressed.png b/selfdrive/assets/icons_mici/settings/network/new/forget_button_pressed.png deleted file mode 100644 index 26cc8b4fca6d6b..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/forget_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d9f17c82b2f349d107d27c69418f054be1f1753f970c7d3d3520c1e65de00511 -size 12894 diff --git a/selfdrive/assets/icons_mici/settings/network/new/full_connect_button.png b/selfdrive/assets/icons_mici/settings/network/new/full_connect_button.png deleted file mode 100644 index 905170fd10ff51..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/full_connect_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ffd37d5e5d5980efa98fee1cd0e8ebbf4139149b41c099e7dc3d5bd402cffb92 -size 9072 diff --git a/selfdrive/assets/icons_mici/settings/network/new/full_connect_button_pressed.png b/selfdrive/assets/icons_mici/settings/network/new/full_connect_button_pressed.png deleted file mode 100644 index 88eb4ac2a3b1ed..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/full_connect_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1b1d58704f8808dcb5a7ce9d86bc4212477759e96ac2419475f16f9184ee6a42 -size 21892 diff --git a/selfdrive/assets/icons_mici/settings/network/new/lock.png b/selfdrive/assets/icons_mici/settings/network/new/lock.png deleted file mode 100644 index 9fc152d3dbc52c..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/lock.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:782161f35b4925c7063c441b0c341331c814614cf241f21b4e70134280c630f0 -size 1182 diff --git a/selfdrive/assets/icons_mici/settings/network/new/trash.png b/selfdrive/assets/icons_mici/settings/network/new/trash.png deleted file mode 100644 index 81e5f13e43a663..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/trash.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9074162bf0469fc5ab0b5711a121289a983c887161df269ac120edd8fd024499 -size 1533 diff --git a/selfdrive/assets/icons_mici/settings/network/new/wifi_selected.png b/selfdrive/assets/icons_mici/settings/network/new/wifi_selected.png deleted file mode 100644 index 2a3e8371381612..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/new/wifi_selected.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:160f67162e075436200d6719e614ddf96caaa2b7c0a3943f728c2afef10aa4ad -size 2489 diff --git a/selfdrive/assets/icons_mici/settings/network/tethering.png b/selfdrive/assets/icons_mici/settings/network/tethering.png deleted file mode 100644 index 4bb416b0b105d8..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/tethering.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b1e322ea6e57b05b3515fcd4e9100f890e6ff80607c11360b7927fa5a9765beb -size 2752 diff --git a/selfdrive/assets/icons_mici/settings/network/wifi_strength_full.png b/selfdrive/assets/icons_mici/settings/network/wifi_strength_full.png deleted file mode 100644 index fe81ffa572076d..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/wifi_strength_full.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:73c76e5240bdff64c1d1ed0ac2bb9c3fadb2fd61fbf8dc710b812757af8bcf6c -size 2026 diff --git a/selfdrive/assets/icons_mici/settings/network/wifi_strength_low.png b/selfdrive/assets/icons_mici/settings/network/wifi_strength_low.png deleted file mode 100644 index 2649cc89dce40e..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/wifi_strength_low.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e66cc6174a54177793c42ef3525a9aa1592e05b0abb677442c7226269d1371a5 -size 2196 diff --git a/selfdrive/assets/icons_mici/settings/network/wifi_strength_medium.png b/selfdrive/assets/icons_mici/settings/network/wifi_strength_medium.png deleted file mode 100644 index 8881833375319c..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/wifi_strength_medium.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7948a9234f2bc996aefb3a9e58a37c06ebbf54e8e4596e47800f78ef7e81961f -size 2231 diff --git a/selfdrive/assets/icons_mici/settings/network/wifi_strength_none.png b/selfdrive/assets/icons_mici/settings/network/wifi_strength_none.png deleted file mode 100644 index 848d7849a23c10..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/wifi_strength_none.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a57ea402448dacc2026631174e448b6254698fe92309221576400cbf28196936 -size 2195 diff --git a/selfdrive/assets/icons_mici/settings/network/wifi_strength_slash.png b/selfdrive/assets/icons_mici/settings/network/wifi_strength_slash.png deleted file mode 100644 index 4457a3fcd27908..00000000000000 --- a/selfdrive/assets/icons_mici/settings/network/wifi_strength_slash.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7e6d166bdbbcdc106e7cd4a44ba85848888f18a6ef34e86daac8e12a3f519443 -size 2318 diff --git a/selfdrive/assets/icons_mici/settings/vertical_scroll_indicator.png b/selfdrive/assets/icons_mici/settings/vertical_scroll_indicator.png deleted file mode 100644 index 77d9a77d6f3c85..00000000000000 --- a/selfdrive/assets/icons_mici/settings/vertical_scroll_indicator.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:88e6c50358f627fc714c1e9883143aeed00baabeab16132e16001aa1051e5eb8 -size 1272 diff --git a/selfdrive/assets/icons_mici/setup/back_new.png b/selfdrive/assets/icons_mici/setup/back_new.png deleted file mode 100644 index 20e7fe3b88b149..00000000000000 --- a/selfdrive/assets/icons_mici/setup/back_new.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d29a9c295b33b3164c37a68ad77795595e6ac877a5b308d28112b0315ecd498f -size 1687 diff --git a/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_check.png b/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_check.png deleted file mode 100644 index dfb9799b0b8535..00000000000000 --- a/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_check.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2290105f9b055b3c3d482d883d148de3418cad07b653133b0f61137e1976c407 -size 1412 diff --git a/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_question.png b/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_question.png deleted file mode 100644 index fa29be1827ffec..00000000000000 --- a/selfdrive/assets/icons_mici/setup/driver_monitoring/dm_question.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ec9691d2572e2e084f0b3c99a1dcd0daadf5040d16c02347ffec9dd5466c061a -size 1438 diff --git a/selfdrive/assets/icons_mici/setup/green_button.png b/selfdrive/assets/icons_mici/setup/green_button.png deleted file mode 100644 index 9708cfe28470e0..00000000000000 --- a/selfdrive/assets/icons_mici/setup/green_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:163ac31cb990bdddfe552efef9a68870404caadb1c40fa8a5042b5ae956e6b4c -size 24687 diff --git a/selfdrive/assets/icons_mici/setup/green_button_pressed.png b/selfdrive/assets/icons_mici/setup/green_button_pressed.png deleted file mode 100644 index 030ce61d5b6827..00000000000000 --- a/selfdrive/assets/icons_mici/setup/green_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6e4614adb2d3d0e44c64a855c221ec462a7aee22fff26132ad551035141c1a53 -size 62056 diff --git a/selfdrive/assets/icons_mici/setup/green_dm.png b/selfdrive/assets/icons_mici/setup/green_dm.png deleted file mode 100644 index 87f4ffe78850e6..00000000000000 --- a/selfdrive/assets/icons_mici/setup/green_dm.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8b6d7747dd6bbf47d9782fc0d847c224b933f6616218ade1f9220018aa9d6acc -size 15052 diff --git a/selfdrive/assets/icons_mici/setup/green_info.png b/selfdrive/assets/icons_mici/setup/green_info.png deleted file mode 100644 index 57e005abd67620..00000000000000 --- a/selfdrive/assets/icons_mici/setup/green_info.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5055bc385a1de674e6f3cbafdb611ee4b1088de2a3c357bce76f6a192226c952 -size 14154 diff --git a/selfdrive/assets/icons_mici/setup/medium_button_bg.png b/selfdrive/assets/icons_mici/setup/medium_button_bg.png deleted file mode 100644 index e79dc2eb588cab..00000000000000 --- a/selfdrive/assets/icons_mici/setup/medium_button_bg.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9e363a79dc35ca4c4e9efaa6a843d37ad219efa5299d3e538d8249affa230096 -size 7935 diff --git a/selfdrive/assets/icons_mici/setup/medium_button_pressed_bg.png b/selfdrive/assets/icons_mici/setup/medium_button_pressed_bg.png deleted file mode 100644 index e52fb0c17d01ac..00000000000000 --- a/selfdrive/assets/icons_mici/setup/medium_button_pressed_bg.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cc6fb48520143b6fa1f060d8212e6d929917ab616ce943b5fab5a60665f00da5 -size 18225 diff --git a/selfdrive/assets/icons_mici/setup/orange_dm.png b/selfdrive/assets/icons_mici/setup/orange_dm.png deleted file mode 100644 index 97df767a987215..00000000000000 --- a/selfdrive/assets/icons_mici/setup/orange_dm.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c45ab0b949c1c71651f9f48cf6ff10196d64eb85e042b063e92b1d7ca02dcb5 -size 13155 diff --git a/selfdrive/assets/icons_mici/setup/red_warning.png b/selfdrive/assets/icons_mici/setup/red_warning.png deleted file mode 100644 index 387794cf13a9a8..00000000000000 --- a/selfdrive/assets/icons_mici/setup/red_warning.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e8e8bc3c15df7512a81b902e47fb069eff1370c833095d3b25f3866efb815fff -size 11123 diff --git a/selfdrive/assets/icons_mici/setup/reset/small_button.png b/selfdrive/assets/icons_mici/setup/reset/small_button.png deleted file mode 100644 index e3f58b1078c99f..00000000000000 --- a/selfdrive/assets/icons_mici/setup/reset/small_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7a198f13f30b3dbc09f30d7fd8033a0bc07a0da9b010b7ca6ed2678430c9e5b4 -size 6949 diff --git a/selfdrive/assets/icons_mici/setup/reset/small_button_pressed.png b/selfdrive/assets/icons_mici/setup/reset/small_button_pressed.png deleted file mode 100644 index 5b502e00aa9629..00000000000000 --- a/selfdrive/assets/icons_mici/setup/reset/small_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:75289d004709def2a2d6101a0330ec867895068ec3807aefc2a26d423d907a13 -size 13437 diff --git a/selfdrive/assets/icons_mici/setup/reset/wide_button.png b/selfdrive/assets/icons_mici/setup/reset/wide_button.png deleted file mode 100644 index 3892f6eb8ccce2..00000000000000 --- a/selfdrive/assets/icons_mici/setup/reset/wide_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2452aaf59da18be1b74b475851d66e5c73c50aa49820419a288b1fdb7b42dee1 -size 9071 diff --git a/selfdrive/assets/icons_mici/setup/reset/wide_button_pressed.png b/selfdrive/assets/icons_mici/setup/reset/wide_button_pressed.png deleted file mode 100644 index 3a34af88467afc..00000000000000 --- a/selfdrive/assets/icons_mici/setup/reset/wide_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6478f7c1c5ef2013e94fc4218ab370889883c5c12231ba3e0975874cb0b6fec9 -size 21893 diff --git a/selfdrive/assets/icons_mici/setup/restore.png b/selfdrive/assets/icons_mici/setup/restore.png deleted file mode 100644 index 5eff9240406666..00000000000000 --- a/selfdrive/assets/icons_mici/setup/restore.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f5ee67cd334d259ac33f932281db36533877009b5769c92d9cff3054fd5627c -size 2942 diff --git a/selfdrive/assets/icons_mici/setup/scroll_down_indicator.png b/selfdrive/assets/icons_mici/setup/scroll_down_indicator.png deleted file mode 100644 index 3cd26e51810625..00000000000000 --- a/selfdrive/assets/icons_mici/setup/scroll_down_indicator.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a733c425113a7f6ff5ec3dc50ef94b5481c0f2d306e33d1485be8ee6b2798532 -size 1136 diff --git a/selfdrive/assets/icons_mici/setup/small_button.png b/selfdrive/assets/icons_mici/setup/small_button.png deleted file mode 100644 index 1ee01aeac2f0f1..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:13919cf5df3137fdffdb8cc53a1215f13bf478a780ca8614234b7af0cdc0e766 -size 5409 diff --git a/selfdrive/assets/icons_mici/setup/small_button_disabled.png b/selfdrive/assets/icons_mici/setup/small_button_disabled.png deleted file mode 100644 index da8bb3eefd78ce..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_button_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6ed258d8e0531c19705953ded065c6d5e14929728a2909d8d4e335898fa5d080 -size 4056 diff --git a/selfdrive/assets/icons_mici/setup/small_button_pressed.png b/selfdrive/assets/icons_mici/setup/small_button_pressed.png deleted file mode 100644 index 6e30f47fba4cb8..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c032fd71ccfebe161827de420771e7927fe1ed799e615e24d458cfd79fead7f7 -size 7875 diff --git a/selfdrive/assets/icons_mici/setup/small_red_pill.png b/selfdrive/assets/icons_mici/setup/small_red_pill.png deleted file mode 100644 index 4a7db930a0b06c..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_red_pill.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b3a336afddad80dc91caca91d54bd29897ce491f180374edf9a5ba517cbc00e9 -size 8765 diff --git a/selfdrive/assets/icons_mici/setup/small_red_pill_pressed.png b/selfdrive/assets/icons_mici/setup/small_red_pill_pressed.png deleted file mode 100644 index a8d51960c41b06..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_red_pill_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8eee9f10ca80a4e6100c00c02bb46aa5f253b14b086ab9982cfa85ee94eec162 -size 22512 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_arrow.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_arrow.png deleted file mode 100644 index acf5b174147742..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_arrow.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:75a6557935075a646b17d083202832daafb263d4cfa38aea2af407afc04e2ef4 -size 1312 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_bg.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_bg.png deleted file mode 100644 index 43c10a54ad8c84..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_bg.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:94a86fac6ffe8a8179812cf55350ab9ca6935f36244c6f679c1cf521a842316b -size 5723 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_bg_larger.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_bg_larger.png deleted file mode 100644 index 11c3ae2d3f2678..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_bg_larger.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:950d55fd7294fb05c10ba9944537c02637776497c159e1b7d145c73f0f9d3253 -size 7119 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_black_rounded_rectangle.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_black_rounded_rectangle.png deleted file mode 100644 index 683587a060c8a4..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_black_rounded_rectangle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:61281d3e3ef5ac5a8fe75405a93c2096bf235f090b27832e986444e3fb85715e -size 7427 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png deleted file mode 100644 index 9ebff76b506564..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bcd08444c77b3e559876eeb88d17808f72496adc26e27c3c21c00ff410879447 -size 10966 diff --git a/selfdrive/assets/icons_mici/setup/small_slider/slider_red_circle.png b/selfdrive/assets/icons_mici/setup/small_slider/slider_red_circle.png deleted file mode 100644 index 541433be763012..00000000000000 --- a/selfdrive/assets/icons_mici/setup/small_slider/slider_red_circle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6ccb5f2298389ae36df87de84d85440ee5a82c50e803c9bd362c9b89ea45aa69 -size 6611 diff --git a/selfdrive/assets/icons_mici/setup/smaller_button.png b/selfdrive/assets/icons_mici/setup/smaller_button.png deleted file mode 100644 index 9b4851c56898e2..00000000000000 --- a/selfdrive/assets/icons_mici/setup/smaller_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:89ca7e6bb01dfa78300126ce828cb2a64e7a2e68e1e9152de242f57a36d0e57a -size 8604 diff --git a/selfdrive/assets/icons_mici/setup/smaller_button_disabled.png b/selfdrive/assets/icons_mici/setup/smaller_button_disabled.png deleted file mode 100644 index 6514791de75ec9..00000000000000 --- a/selfdrive/assets/icons_mici/setup/smaller_button_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b3242a411b559f1d0308f189fe0d25b81d6c7d964ca418a0c599a1bab4bffcbb -size 5341 diff --git a/selfdrive/assets/icons_mici/setup/smaller_button_pressed.png b/selfdrive/assets/icons_mici/setup/smaller_button_pressed.png deleted file mode 100644 index 64235b3a2f6ade..00000000000000 --- a/selfdrive/assets/icons_mici/setup/smaller_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d354651c0c8107dcc5f599777d260f53ef1901123315785ed8190466166cdce8 -size 17554 diff --git a/selfdrive/assets/icons_mici/setup/warning.png b/selfdrive/assets/icons_mici/setup/warning.png deleted file mode 100644 index 1b7839f47f6777..00000000000000 --- a/selfdrive/assets/icons_mici/setup/warning.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7584d32ac0231381e38646fdac2f71b4517905ef22024f01bd9e124d3918f33a -size 9194 diff --git a/selfdrive/assets/icons_mici/setup/widish_button.png b/selfdrive/assets/icons_mici/setup/widish_button.png deleted file mode 100644 index 529b7c80ccaa04..00000000000000 --- a/selfdrive/assets/icons_mici/setup/widish_button.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:74fc21132b1e761ea54ce64617730c6ee79d01668244ab555b3b89870cfea181 -size 7112 diff --git a/selfdrive/assets/icons_mici/setup/widish_button_disabled.png b/selfdrive/assets/icons_mici/setup/widish_button_disabled.png deleted file mode 100644 index 5028a8cd21efa8..00000000000000 --- a/selfdrive/assets/icons_mici/setup/widish_button_disabled.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9728423bd5e3197ef02d62e4bae415e6694aab875ca8630ffc9f188c38e18e5f -size 4141 diff --git a/selfdrive/assets/icons_mici/setup/widish_button_pressed.png b/selfdrive/assets/icons_mici/setup/widish_button_pressed.png deleted file mode 100644 index 1095d4fc239d45..00000000000000 --- a/selfdrive/assets/icons_mici/setup/widish_button_pressed.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0ff179f93f421edcb503ca5c22a12b37e3a2aaabc414bf90f57e20ff5255dd75 -size 15572 diff --git a/selfdrive/assets/icons_mici/ssh_short.png b/selfdrive/assets/icons_mici/ssh_short.png deleted file mode 100644 index 699ddd72e8fb90..00000000000000 --- a/selfdrive/assets/icons_mici/ssh_short.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ef1735e6effcb625ea618fa35a6b908b28ca483d5997e15241d48e2d3d29819e -size 1433 diff --git a/selfdrive/assets/icons_mici/turn_intent_left.png b/selfdrive/assets/icons_mici/turn_intent_left.png deleted file mode 100644 index 3934200c9d9c7f..00000000000000 --- a/selfdrive/assets/icons_mici/turn_intent_left.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:001cb8227eaaff5367055395d9b3ccd5822f9a47276091832d8ad28b074d77c9 -size 914 diff --git a/selfdrive/assets/icons_mici/turn_intent_right.png b/selfdrive/assets/icons_mici/turn_intent_right.png deleted file mode 100644 index e342778731d214..00000000000000 --- a/selfdrive/assets/icons_mici/turn_intent_right.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7b7e0194a8b9009e493cdce35cd15711596a54227c740e9d6419a3891c6c4037 -size 912 diff --git a/selfdrive/assets/icons_mici/wheel.png b/selfdrive/assets/icons_mici/wheel.png deleted file mode 100644 index a43bcb3b9933f2..00000000000000 --- a/selfdrive/assets/icons_mici/wheel.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8cf9c6361ed82551eb99e028e0a75ff56b72ca856ccf7c9a76afe6745434980a -size 2720 diff --git a/selfdrive/assets/icons_mici/wheel_critical.png b/selfdrive/assets/icons_mici/wheel_critical.png deleted file mode 100644 index 676b0b4d7108c9..00000000000000 --- a/selfdrive/assets/icons_mici/wheel_critical.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4c3d9082b295f9e5ddef93f8d4e9cb961ea2374c7affd26394bbccb26e7137b2 -size 11023 diff --git a/selfdrive/assets/images/button_continue_triangle.png b/selfdrive/assets/images/button_continue_triangle.png deleted file mode 100644 index c56e1120945770..00000000000000 --- a/selfdrive/assets/images/button_continue_triangle.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b9218e02c42b0f80858477255e24a97da4cf0b2898fc76f3806409d65b104668 -size 4510 diff --git a/selfdrive/assets/images/button_continue_triangle.svg b/selfdrive/assets/images/button_continue_triangle.svg deleted file mode 100644 index e6d362927cd8a9..00000000000000 --- a/selfdrive/assets/images/button_continue_triangle.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8084e5a8bbc16956a98b010092ae1c4e32b3391b0551fa1cd65cb1e2bb59d3df -size 169 diff --git a/selfdrive/assets/images/button_flag.png b/selfdrive/assets/images/button_flag.png deleted file mode 100644 index 745c8d576bf106..00000000000000 --- a/selfdrive/assets/images/button_flag.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cd1528167f0cd22cb9a16f2f04e914b2bff78ffca26be8157b9ad208b485d8ea -size 1611 diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png index dd3f97f44bbeea..9f52faf9e2da48 100644 Binary files a/selfdrive/assets/images/button_home.png and b/selfdrive/assets/images/button_home.png differ diff --git a/selfdrive/assets/images/button_settings.png b/selfdrive/assets/images/button_settings.png index 4bbfe581e6740e..e04262b887cea5 100644 Binary files a/selfdrive/assets/images/button_settings.png and b/selfdrive/assets/images/button_settings.png differ diff --git a/selfdrive/assets/images/spinner_comma.png b/selfdrive/assets/images/spinner_comma.png deleted file mode 100644 index 81f2fb38c4da46..00000000000000 --- a/selfdrive/assets/images/spinner_comma.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:78d140abe572b00765a7ece46bd990cff1287c2e8705a5f551b74461ab307b6b -size 15713 diff --git a/selfdrive/assets/images/spinner_track.png b/selfdrive/assets/images/spinner_track.png deleted file mode 100644 index 989a6cefd1f63e..00000000000000 --- a/selfdrive/assets/images/spinner_track.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:49e9bad844598b0aa13c75fad7685f89f7ab190932963a8c70014553b9da37ae -size 41523 diff --git a/selfdrive/assets/images/triangle.svg b/selfdrive/assets/images/triangle.svg new file mode 100644 index 00000000000000..9320269bde3cd5 --- /dev/null +++ b/selfdrive/assets/images/triangle.svg @@ -0,0 +1,56 @@ + + + + + + image/svg+xml + + + + + + + + diff --git a/selfdrive/assets/img_chffr_wheel.png b/selfdrive/assets/img_chffr_wheel.png new file mode 100644 index 00000000000000..3f09a35a79bf45 Binary files /dev/null and b/selfdrive/assets/img_chffr_wheel.png differ diff --git a/selfdrive/assets/img_circled_check.svg b/selfdrive/assets/img_circled_check.svg new file mode 100644 index 00000000000000..27c37395b29bda --- /dev/null +++ b/selfdrive/assets/img_circled_check.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/img_circled_slash.svg b/selfdrive/assets/img_circled_slash.svg new file mode 100644 index 00000000000000..b10a3938d5d644 --- /dev/null +++ b/selfdrive/assets/img_circled_slash.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/img_continue_triangle.svg b/selfdrive/assets/img_continue_triangle.svg new file mode 100644 index 00000000000000..20f9e45dcfaa9d --- /dev/null +++ b/selfdrive/assets/img_continue_triangle.svg @@ -0,0 +1,3 @@ + + + diff --git a/selfdrive/assets/img_driver_face.png b/selfdrive/assets/img_driver_face.png new file mode 100644 index 00000000000000..ddde478cd789ec Binary files /dev/null and b/selfdrive/assets/img_driver_face.png differ diff --git a/selfdrive/assets/img_eye_closed.svg b/selfdrive/assets/img_eye_closed.svg new file mode 100644 index 00000000000000..91b229e911bc2a --- /dev/null +++ b/selfdrive/assets/img_eye_closed.svg @@ -0,0 +1,5 @@ + + + + + diff --git a/selfdrive/assets/img_eye_open.svg b/selfdrive/assets/img_eye_open.svg new file mode 100644 index 00000000000000..ea6e41ac54532d --- /dev/null +++ b/selfdrive/assets/img_eye_open.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/img_map.png b/selfdrive/assets/img_map.png new file mode 100644 index 00000000000000..8bdae4d7d81e0b Binary files /dev/null and b/selfdrive/assets/img_map.png differ diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png new file mode 100644 index 00000000000000..16109557f85911 Binary files /dev/null and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/assets/img_spinner_track.png b/selfdrive/assets/img_spinner_track.png new file mode 100644 index 00000000000000..931c17e8367cef Binary files /dev/null and b/selfdrive/assets/img_spinner_track.png differ diff --git a/selfdrive/assets/navigation/direction_arrive.png b/selfdrive/assets/navigation/direction_arrive.png new file mode 100644 index 00000000000000..733c1290911269 Binary files /dev/null and b/selfdrive/assets/navigation/direction_arrive.png differ diff --git a/selfdrive/assets/navigation/direction_arrive_left.png b/selfdrive/assets/navigation/direction_arrive_left.png new file mode 100644 index 00000000000000..92ff8e034141ac Binary files /dev/null and b/selfdrive/assets/navigation/direction_arrive_left.png differ diff --git a/selfdrive/assets/navigation/direction_arrive_right.png b/selfdrive/assets/navigation/direction_arrive_right.png new file mode 100644 index 00000000000000..f5983bfe61777a Binary files /dev/null and b/selfdrive/assets/navigation/direction_arrive_right.png differ diff --git a/selfdrive/assets/navigation/direction_arrive_straight.png b/selfdrive/assets/navigation/direction_arrive_straight.png new file mode 100644 index 00000000000000..733c1290911269 Binary files /dev/null and b/selfdrive/assets/navigation/direction_arrive_straight.png differ diff --git a/selfdrive/assets/navigation/direction_close.png b/selfdrive/assets/navigation/direction_close.png new file mode 100644 index 00000000000000..4fdb5d195df1c0 Binary files /dev/null and b/selfdrive/assets/navigation/direction_close.png differ diff --git a/selfdrive/assets/navigation/direction_continue.png b/selfdrive/assets/navigation/direction_continue.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue.png differ diff --git a/selfdrive/assets/navigation/direction_continue_left.png b/selfdrive/assets/navigation/direction_continue_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_left.png differ diff --git a/selfdrive/assets/navigation/direction_continue_right.png b/selfdrive/assets/navigation/direction_continue_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_right.png differ diff --git a/selfdrive/assets/navigation/direction_continue_slight_left.png b/selfdrive/assets/navigation/direction_continue_slight_left.png new file mode 100644 index 00000000000000..08e964dbd6a68a Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_continue_slight_right.png b/selfdrive/assets/navigation/direction_continue_slight_right.png new file mode 100644 index 00000000000000..3e21cae11ed520 Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_continue_straight.png b/selfdrive/assets/navigation/direction_continue_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_straight.png differ diff --git a/selfdrive/assets/navigation/direction_continue_uturn.png b/selfdrive/assets/navigation/direction_continue_uturn.png new file mode 100644 index 00000000000000..0bd1b91777ed8d Binary files /dev/null and b/selfdrive/assets/navigation/direction_continue_uturn.png differ diff --git a/selfdrive/assets/navigation/direction_depart.png b/selfdrive/assets/navigation/direction_depart.png new file mode 100644 index 00000000000000..4bf32c870d5f26 Binary files /dev/null and b/selfdrive/assets/navigation/direction_depart.png differ diff --git a/selfdrive/assets/navigation/direction_depart_left.png b/selfdrive/assets/navigation/direction_depart_left.png new file mode 100644 index 00000000000000..1f8d726911fcb7 Binary files /dev/null and b/selfdrive/assets/navigation/direction_depart_left.png differ diff --git a/selfdrive/assets/navigation/direction_depart_right.png b/selfdrive/assets/navigation/direction_depart_right.png new file mode 100644 index 00000000000000..f359a685ffa568 Binary files /dev/null and b/selfdrive/assets/navigation/direction_depart_right.png differ diff --git a/selfdrive/assets/navigation/direction_depart_straight.png b/selfdrive/assets/navigation/direction_depart_straight.png new file mode 100644 index 00000000000000..4bf32c870d5f26 Binary files /dev/null and b/selfdrive/assets/navigation/direction_depart_straight.png differ diff --git a/selfdrive/assets/navigation/direction_end_of_road_left.png b/selfdrive/assets/navigation/direction_end_of_road_left.png new file mode 100644 index 00000000000000..5c0a24e7cb11c4 Binary files /dev/null and b/selfdrive/assets/navigation/direction_end_of_road_left.png differ diff --git a/selfdrive/assets/navigation/direction_end_of_road_right.png b/selfdrive/assets/navigation/direction_end_of_road_right.png new file mode 100644 index 00000000000000..8d9b89d36cdd61 Binary files /dev/null and b/selfdrive/assets/navigation/direction_end_of_road_right.png differ diff --git a/selfdrive/assets/navigation/direction_flag.png b/selfdrive/assets/navigation/direction_flag.png new file mode 100644 index 00000000000000..bad12ec6664679 Binary files /dev/null and b/selfdrive/assets/navigation/direction_flag.png differ diff --git a/selfdrive/assets/navigation/direction_fork.png b/selfdrive/assets/navigation/direction_fork.png new file mode 100644 index 00000000000000..3e0c262e2a2785 Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork.png differ diff --git a/selfdrive/assets/navigation/direction_fork_left.png b/selfdrive/assets/navigation/direction_fork_left.png new file mode 100644 index 00000000000000..b244b42b51e906 Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork_left.png differ diff --git a/selfdrive/assets/navigation/direction_fork_right.png b/selfdrive/assets/navigation/direction_fork_right.png new file mode 100644 index 00000000000000..aa3efaabca2cf7 Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork_right.png differ diff --git a/selfdrive/assets/navigation/direction_fork_slight_left.png b/selfdrive/assets/navigation/direction_fork_slight_left.png new file mode 100644 index 00000000000000..82fa59859b603e Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_fork_slight_right.png b/selfdrive/assets/navigation/direction_fork_slight_right.png new file mode 100644 index 00000000000000..3596a2fbf2945d Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_fork_straight.png b/selfdrive/assets/navigation/direction_fork_straight.png new file mode 100644 index 00000000000000..86f30ab9b6edec Binary files /dev/null and b/selfdrive/assets/navigation/direction_fork_straight.png differ diff --git a/selfdrive/assets/navigation/direction_invalid.png b/selfdrive/assets/navigation/direction_invalid.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_left.png b/selfdrive/assets/navigation/direction_invalid_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_left.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_right.png b/selfdrive/assets/navigation/direction_invalid_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_right.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_slight_left.png b/selfdrive/assets/navigation/direction_invalid_slight_left.png new file mode 100644 index 00000000000000..08e964dbd6a68a Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_slight_right.png b/selfdrive/assets/navigation/direction_invalid_slight_right.png new file mode 100644 index 00000000000000..3e21cae11ed520 Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_straight.png b/selfdrive/assets/navigation/direction_invalid_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_straight.png differ diff --git a/selfdrive/assets/navigation/direction_invalid_uturn.png b/selfdrive/assets/navigation/direction_invalid_uturn.png new file mode 100644 index 00000000000000..0bd1b91777ed8d Binary files /dev/null and b/selfdrive/assets/navigation/direction_invalid_uturn.png differ diff --git a/selfdrive/assets/navigation/direction_merge_left.png b/selfdrive/assets/navigation/direction_merge_left.png new file mode 100644 index 00000000000000..a713f52c56dddf Binary files /dev/null and b/selfdrive/assets/navigation/direction_merge_left.png differ diff --git a/selfdrive/assets/navigation/direction_merge_right.png b/selfdrive/assets/navigation/direction_merge_right.png new file mode 100644 index 00000000000000..3390b31a05b26b Binary files /dev/null and b/selfdrive/assets/navigation/direction_merge_right.png differ diff --git a/selfdrive/assets/navigation/direction_merge_slight_left.png b/selfdrive/assets/navigation/direction_merge_slight_left.png new file mode 100644 index 00000000000000..308f97b5a5d5b7 Binary files /dev/null and b/selfdrive/assets/navigation/direction_merge_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_merge_slight_right.png b/selfdrive/assets/navigation/direction_merge_slight_right.png new file mode 100644 index 00000000000000..8f5289011d67b4 Binary files /dev/null and b/selfdrive/assets/navigation/direction_merge_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_merge_straight.png b/selfdrive/assets/navigation/direction_merge_straight.png new file mode 100644 index 00000000000000..49c464389d2da2 Binary files /dev/null and b/selfdrive/assets/navigation/direction_merge_straight.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_left.png b/selfdrive/assets/navigation/direction_new_name_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_left.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_right.png b/selfdrive/assets/navigation/direction_new_name_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_right.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_sharp_left.png b/selfdrive/assets/navigation/direction_new_name_sharp_left.png new file mode 100644 index 00000000000000..77106b493ff6d5 Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_sharp_right.png b/selfdrive/assets/navigation/direction_new_name_sharp_right.png new file mode 100644 index 00000000000000..eb3a02f8b3adfe Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_slight_left.png b/selfdrive/assets/navigation/direction_new_name_slight_left.png new file mode 100644 index 00000000000000..08e964dbd6a68a Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_slight_right.png b/selfdrive/assets/navigation/direction_new_name_slight_right.png new file mode 100644 index 00000000000000..3e21cae11ed520 Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_new_name_straight.png b/selfdrive/assets/navigation/direction_new_name_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_new_name_straight.png differ diff --git a/selfdrive/assets/navigation/direction_notificaiton_right.png b/selfdrive/assets/navigation/direction_notificaiton_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_notificaiton_right.png differ diff --git a/selfdrive/assets/navigation/direction_notificaiton_sharp_right.png b/selfdrive/assets/navigation/direction_notificaiton_sharp_right.png new file mode 100644 index 00000000000000..a7e3c4cee56a7d Binary files /dev/null and b/selfdrive/assets/navigation/direction_notificaiton_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_notification_left.png b/selfdrive/assets/navigation/direction_notification_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_notification_left.png differ diff --git a/selfdrive/assets/navigation/direction_notification_sharp_left.png b/selfdrive/assets/navigation/direction_notification_sharp_left.png new file mode 100644 index 00000000000000..dd8a4301db6dbe Binary files /dev/null and b/selfdrive/assets/navigation/direction_notification_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_notification_slight_left.png b/selfdrive/assets/navigation/direction_notification_slight_left.png new file mode 100644 index 00000000000000..08e964dbd6a68a Binary files /dev/null and b/selfdrive/assets/navigation/direction_notification_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_notification_slight_right.png b/selfdrive/assets/navigation/direction_notification_slight_right.png new file mode 100644 index 00000000000000..3e21cae11ed520 Binary files /dev/null and b/selfdrive/assets/navigation/direction_notification_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_notification_straight.png b/selfdrive/assets/navigation/direction_notification_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_notification_straight.png differ diff --git a/selfdrive/assets/navigation/direction_off_ramp_left.png b/selfdrive/assets/navigation/direction_off_ramp_left.png new file mode 100644 index 00000000000000..d3fd182893762e Binary files /dev/null and b/selfdrive/assets/navigation/direction_off_ramp_left.png differ diff --git a/selfdrive/assets/navigation/direction_off_ramp_right.png b/selfdrive/assets/navigation/direction_off_ramp_right.png new file mode 100644 index 00000000000000..722e3f808f632c Binary files /dev/null and b/selfdrive/assets/navigation/direction_off_ramp_right.png differ diff --git a/selfdrive/assets/navigation/direction_off_ramp_slight_left.png b/selfdrive/assets/navigation/direction_off_ramp_slight_left.png new file mode 100644 index 00000000000000..ddac4aad664592 Binary files /dev/null and b/selfdrive/assets/navigation/direction_off_ramp_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_off_ramp_slight_right.png b/selfdrive/assets/navigation/direction_off_ramp_slight_right.png new file mode 100644 index 00000000000000..ed576088645187 Binary files /dev/null and b/selfdrive/assets/navigation/direction_off_ramp_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_left.png b/selfdrive/assets/navigation/direction_on_ramp_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_left.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_right.png b/selfdrive/assets/navigation/direction_on_ramp_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_right.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_sharp_left.png b/selfdrive/assets/navigation/direction_on_ramp_sharp_left.png new file mode 100644 index 00000000000000..77106b493ff6d5 Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_sharp_right.png b/selfdrive/assets/navigation/direction_on_ramp_sharp_right.png new file mode 100644 index 00000000000000..a7e3c4cee56a7d Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_slight_left.png b/selfdrive/assets/navigation/direction_on_ramp_slight_left.png new file mode 100644 index 00000000000000..a5ea8a881e6b5e Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_slight_right.png b/selfdrive/assets/navigation/direction_on_ramp_slight_right.png new file mode 100644 index 00000000000000..f8ea3800e880fc Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_on_ramp_straight.png b/selfdrive/assets/navigation/direction_on_ramp_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_on_ramp_straight.png differ diff --git a/selfdrive/assets/navigation/direction_rotary.png b/selfdrive/assets/navigation/direction_rotary.png new file mode 100644 index 00000000000000..2a5d264bd27426 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_left.png b/selfdrive/assets/navigation/direction_rotary_left.png new file mode 100644 index 00000000000000..0c4e4ab5e6cb83 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_left.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_right.png b/selfdrive/assets/navigation/direction_rotary_right.png new file mode 100644 index 00000000000000..32a6b2504ba9d9 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_right.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_sharp_left.png b/selfdrive/assets/navigation/direction_rotary_sharp_left.png new file mode 100644 index 00000000000000..c84a6d96c01c67 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_sharp_right.png b/selfdrive/assets/navigation/direction_rotary_sharp_right.png new file mode 100644 index 00000000000000..d15cbee00232cc Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_slight_left.png b/selfdrive/assets/navigation/direction_rotary_slight_left.png new file mode 100644 index 00000000000000..3838e720a3553a Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_slight_right.png b/selfdrive/assets/navigation/direction_rotary_slight_right.png new file mode 100644 index 00000000000000..8cd45fe6125748 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_rotary_straight.png b/selfdrive/assets/navigation/direction_rotary_straight.png new file mode 100644 index 00000000000000..b6b0a7311bb2f5 Binary files /dev/null and b/selfdrive/assets/navigation/direction_rotary_straight.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout.png b/selfdrive/assets/navigation/direction_roundabout.png new file mode 100644 index 00000000000000..2a5d264bd27426 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_left.png b/selfdrive/assets/navigation/direction_roundabout_left.png new file mode 100644 index 00000000000000..0c4e4ab5e6cb83 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_left.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_right.png b/selfdrive/assets/navigation/direction_roundabout_right.png new file mode 100644 index 00000000000000..32a6b2504ba9d9 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_right.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_sharp_left.png b/selfdrive/assets/navigation/direction_roundabout_sharp_left.png new file mode 100644 index 00000000000000..1e8cce8c8e19d8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_sharp_right.png b/selfdrive/assets/navigation/direction_roundabout_sharp_right.png new file mode 100644 index 00000000000000..d15cbee00232cc Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_slight_left.png b/selfdrive/assets/navigation/direction_roundabout_slight_left.png new file mode 100644 index 00000000000000..da1b1127051a19 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_slight_right.png b/selfdrive/assets/navigation/direction_roundabout_slight_right.png new file mode 100644 index 00000000000000..8cd45fe6125748 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_roundabout_straight.png b/selfdrive/assets/navigation/direction_roundabout_straight.png new file mode 100644 index 00000000000000..b6b0a7311bb2f5 Binary files /dev/null and b/selfdrive/assets/navigation/direction_roundabout_straight.png differ diff --git a/selfdrive/assets/navigation/direction_turn_left.png b/selfdrive/assets/navigation/direction_turn_left.png new file mode 100644 index 00000000000000..9a618026f0e00d Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_left.png differ diff --git a/selfdrive/assets/navigation/direction_turn_left_inactive.png b/selfdrive/assets/navigation/direction_turn_left_inactive.png new file mode 100644 index 00000000000000..2946984acd3252 Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_left_inactive.png differ diff --git a/selfdrive/assets/navigation/direction_turn_right.png b/selfdrive/assets/navigation/direction_turn_right.png new file mode 100644 index 00000000000000..0fbaa3f253edb8 Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_right.png differ diff --git a/selfdrive/assets/navigation/direction_turn_right_inactive.png b/selfdrive/assets/navigation/direction_turn_right_inactive.png new file mode 100644 index 00000000000000..7d327766af13be Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_right_inactive.png differ diff --git a/selfdrive/assets/navigation/direction_turn_sharp_left.png b/selfdrive/assets/navigation/direction_turn_sharp_left.png new file mode 100644 index 00000000000000..dd8a4301db6dbe Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_sharp_left.png differ diff --git a/selfdrive/assets/navigation/direction_turn_sharp_right.png b/selfdrive/assets/navigation/direction_turn_sharp_right.png new file mode 100644 index 00000000000000..a7e3c4cee56a7d Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_sharp_right.png differ diff --git a/selfdrive/assets/navigation/direction_turn_slight_left.png b/selfdrive/assets/navigation/direction_turn_slight_left.png new file mode 100644 index 00000000000000..08e964dbd6a68a Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_slight_left.png differ diff --git a/selfdrive/assets/navigation/direction_turn_slight_right.png b/selfdrive/assets/navigation/direction_turn_slight_right.png new file mode 100644 index 00000000000000..3e21cae11ed520 Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_slight_right.png differ diff --git a/selfdrive/assets/navigation/direction_turn_straight.png b/selfdrive/assets/navigation/direction_turn_straight.png new file mode 100644 index 00000000000000..a01045ae6a2b58 Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_straight.png differ diff --git a/selfdrive/assets/navigation/direction_turn_straight_inactive.png b/selfdrive/assets/navigation/direction_turn_straight_inactive.png new file mode 100644 index 00000000000000..4c567966ee8238 Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_straight_inactive.png differ diff --git a/selfdrive/assets/navigation/direction_turn_uturn.png b/selfdrive/assets/navigation/direction_turn_uturn.png new file mode 100644 index 00000000000000..0bd1b91777ed8d Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_uturn.png differ diff --git a/selfdrive/assets/navigation/direction_updown.png b/selfdrive/assets/navigation/direction_updown.png new file mode 100644 index 00000000000000..16d0979f3ed040 Binary files /dev/null and b/selfdrive/assets/navigation/direction_updown.png differ diff --git a/selfdrive/assets/navigation/home.png b/selfdrive/assets/navigation/home.png new file mode 100644 index 00000000000000..8a4f65c7d7a836 Binary files /dev/null and b/selfdrive/assets/navigation/home.png differ diff --git a/selfdrive/assets/navigation/home.svg b/selfdrive/assets/navigation/home.svg new file mode 100644 index 00000000000000..f5d89514c36bc0 --- /dev/null +++ b/selfdrive/assets/navigation/home.svg @@ -0,0 +1,65 @@ + + + + + + image/svg+xml + + + + + + + + + + diff --git a/selfdrive/assets/navigation/home_inactive.png b/selfdrive/assets/navigation/home_inactive.png new file mode 100644 index 00000000000000..a58fd3864fdf30 Binary files /dev/null and b/selfdrive/assets/navigation/home_inactive.png differ diff --git a/selfdrive/assets/navigation/screenshot.png b/selfdrive/assets/navigation/screenshot.png new file mode 100644 index 00000000000000..3e89c04759fd8e Binary files /dev/null and b/selfdrive/assets/navigation/screenshot.png differ diff --git a/selfdrive/assets/navigation/work.png b/selfdrive/assets/navigation/work.png new file mode 100644 index 00000000000000..611f9b038dc9cd Binary files /dev/null and b/selfdrive/assets/navigation/work.png differ diff --git a/selfdrive/assets/navigation/work.svg b/selfdrive/assets/navigation/work.svg new file mode 100644 index 00000000000000..2da7bb7d39f2e9 --- /dev/null +++ b/selfdrive/assets/navigation/work.svg @@ -0,0 +1,66 @@ + + + + + + image/svg+xml + + + + + + + + + + diff --git a/selfdrive/assets/navigation/work_inactive.png b/selfdrive/assets/navigation/work_inactive.png new file mode 100644 index 00000000000000..679e6a54b2852e Binary files /dev/null and b/selfdrive/assets/navigation/work_inactive.png differ diff --git a/selfdrive/assets/offroad/fcc.html b/selfdrive/assets/offroad/fcc.html index 960a7a06cb2ced..793bea533c61ef 100644 --- a/selfdrive/assets/offroad/fcc.html +++ b/selfdrive/assets/offroad/fcc.html @@ -12,10 +12,11 @@
    Thundersoft TurboX D845 SOM
    Quectel/EG25-G

    FCC ID: XMR201903EG25G

    -

    This device complies with Part 15 of the FCC Rules.

    -

    Operation is subject to the following two conditions:

    +

    + This device complies with Part 15 of the FCC Rules. + Operation is subject to the following two conditions: -

    (1) this device may not cause harmful interference, and

    +

    (1) this device may not cause harmful interference, and

    (2) this device must accept any interference received, including interference that may cause undesired operation.

    The following test reports are subject to this declaration: diff --git a/selfdrive/assets/offroad/icon_calibration.png b/selfdrive/assets/offroad/icon_calibration.png new file mode 100644 index 00000000000000..c4ee0d63d46210 Binary files /dev/null and b/selfdrive/assets/offroad/icon_calibration.png differ diff --git a/selfdrive/assets/offroad/icon_checkmark.svg b/selfdrive/assets/offroad/icon_checkmark.svg new file mode 100644 index 00000000000000..b024eccd9e8192 --- /dev/null +++ b/selfdrive/assets/offroad/icon_checkmark.svg @@ -0,0 +1,3 @@ + + + diff --git a/selfdrive/assets/offroad/icon_chevron_right.png b/selfdrive/assets/offroad/icon_chevron_right.png new file mode 100644 index 00000000000000..a3aaa76486170d Binary files /dev/null and b/selfdrive/assets/offroad/icon_chevron_right.png differ diff --git a/selfdrive/assets/offroad/icon_close.svg b/selfdrive/assets/offroad/icon_close.svg new file mode 100644 index 00000000000000..4c063371afcfa5 --- /dev/null +++ b/selfdrive/assets/offroad/icon_close.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/offroad/icon_disengage_on_accelerator.svg b/selfdrive/assets/offroad/icon_disengage_on_accelerator.svg new file mode 100644 index 00000000000000..0175e672c6a07e --- /dev/null +++ b/selfdrive/assets/offroad/icon_disengage_on_accelerator.svg @@ -0,0 +1,7 @@ + + + + + + + diff --git a/selfdrive/assets/offroad/icon_lock_closed.svg b/selfdrive/assets/offroad/icon_lock_closed.svg new file mode 100644 index 00000000000000..7dc9283c81fde3 --- /dev/null +++ b/selfdrive/assets/offroad/icon_lock_closed.svg @@ -0,0 +1,4 @@ + + + + diff --git a/selfdrive/assets/offroad/icon_map.png b/selfdrive/assets/offroad/icon_map.png new file mode 100644 index 00000000000000..21dd0bacc65f76 Binary files /dev/null and b/selfdrive/assets/offroad/icon_map.png differ diff --git a/selfdrive/assets/offroad/icon_map_speed.png b/selfdrive/assets/offroad/icon_map_speed.png new file mode 100644 index 00000000000000..1eeab84600e3f0 Binary files /dev/null and b/selfdrive/assets/offroad/icon_map_speed.png differ diff --git a/selfdrive/assets/offroad/icon_menu.png b/selfdrive/assets/offroad/icon_menu.png new file mode 100644 index 00000000000000..837cf5831c3b15 Binary files /dev/null and b/selfdrive/assets/offroad/icon_menu.png differ diff --git a/selfdrive/assets/offroad/icon_metric.png b/selfdrive/assets/offroad/icon_metric.png new file mode 100644 index 00000000000000..eaa2438fa3701c Binary files /dev/null and b/selfdrive/assets/offroad/icon_metric.png differ diff --git a/selfdrive/assets/offroad/icon_minus.png b/selfdrive/assets/offroad/icon_minus.png new file mode 100644 index 00000000000000..e5327c0d3b2d48 Binary files /dev/null and b/selfdrive/assets/offroad/icon_minus.png differ diff --git a/selfdrive/assets/offroad/icon_monitoring.png b/selfdrive/assets/offroad/icon_monitoring.png new file mode 100644 index 00000000000000..05f78811e274a5 Binary files /dev/null and b/selfdrive/assets/offroad/icon_monitoring.png differ diff --git a/selfdrive/assets/offroad/icon_network.png b/selfdrive/assets/offroad/icon_network.png new file mode 100644 index 00000000000000..3236924f4dd90b Binary files /dev/null and b/selfdrive/assets/offroad/icon_network.png differ diff --git a/selfdrive/assets/offroad/icon_openpilot.png b/selfdrive/assets/offroad/icon_openpilot.png new file mode 100644 index 00000000000000..0a90a879106c89 Binary files /dev/null and b/selfdrive/assets/offroad/icon_openpilot.png differ diff --git a/selfdrive/assets/offroad/icon_plus.png b/selfdrive/assets/offroad/icon_plus.png new file mode 100644 index 00000000000000..92b448b0bdcfcb Binary files /dev/null and b/selfdrive/assets/offroad/icon_plus.png differ diff --git a/selfdrive/assets/offroad/icon_road.png b/selfdrive/assets/offroad/icon_road.png new file mode 100644 index 00000000000000..5868ed1ccc3f73 Binary files /dev/null and b/selfdrive/assets/offroad/icon_road.png differ diff --git a/selfdrive/assets/offroad/icon_settings.png b/selfdrive/assets/offroad/icon_settings.png new file mode 100644 index 00000000000000..d0c90a620d780d Binary files /dev/null and b/selfdrive/assets/offroad/icon_settings.png differ diff --git a/selfdrive/assets/offroad/icon_shell.png b/selfdrive/assets/offroad/icon_shell.png new file mode 100644 index 00000000000000..f1d655416a88c6 Binary files /dev/null and b/selfdrive/assets/offroad/icon_shell.png differ diff --git a/selfdrive/assets/offroad/icon_speed_limit.png b/selfdrive/assets/offroad/icon_speed_limit.png new file mode 100644 index 00000000000000..0aa7038f909985 Binary files /dev/null and b/selfdrive/assets/offroad/icon_speed_limit.png differ diff --git a/selfdrive/assets/offroad/icon_warning.png b/selfdrive/assets/offroad/icon_warning.png new file mode 100644 index 00000000000000..50fe8211277954 Binary files /dev/null and b/selfdrive/assets/offroad/icon_warning.png differ diff --git a/selfdrive/assets/offroad/icon_wifi_strength_full.svg b/selfdrive/assets/offroad/icon_wifi_strength_full.svg new file mode 100644 index 00000000000000..758198e97fcd7e --- /dev/null +++ b/selfdrive/assets/offroad/icon_wifi_strength_full.svg @@ -0,0 +1,6 @@ + + + + + + diff --git a/selfdrive/assets/offroad/icon_wifi_strength_high.svg b/selfdrive/assets/offroad/icon_wifi_strength_high.svg new file mode 100644 index 00000000000000..a8db07f91ef043 --- /dev/null +++ b/selfdrive/assets/offroad/icon_wifi_strength_high.svg @@ -0,0 +1,6 @@ + + + + + + diff --git a/selfdrive/assets/offroad/icon_wifi_strength_low.svg b/selfdrive/assets/offroad/icon_wifi_strength_low.svg new file mode 100644 index 00000000000000..8963c3dbc1ca0b --- /dev/null +++ b/selfdrive/assets/offroad/icon_wifi_strength_low.svg @@ -0,0 +1,6 @@ + + + + + + diff --git a/selfdrive/assets/offroad/icon_wifi_strength_medium.svg b/selfdrive/assets/offroad/icon_wifi_strength_medium.svg new file mode 100644 index 00000000000000..8f8d503260b7cc --- /dev/null +++ b/selfdrive/assets/offroad/icon_wifi_strength_medium.svg @@ -0,0 +1,6 @@ + + + + + + diff --git a/selfdrive/assets/offroad/mici_fcc.html b/selfdrive/assets/offroad/mici_fcc.html deleted file mode 100644 index e6e4189128f286..00000000000000 --- a/selfdrive/assets/offroad/mici_fcc.html +++ /dev/null @@ -1,16 +0,0 @@ -

    HVIN: comma four

    -

    FCC ID: 2BFC6-MICI

    -

    IC: 32232-MICI

    -

    Contains FCC ID: XMR2023EG916QGL

    -

    Contains IC: 10224A-023EG916QGL

    -

    -This device contains licence-exempt transmitter(s)/receiver(s) that comply with Innovation, Science and Economic Development -Canada's licence-exempt RSS(s) and complies with part 15 of the FCC Rules. Operation is subject to the following two conditions:
    -1. This device may not cause harmful interference.
    -2. This device must accept any interference received, including interference that may cause undesired operation of the device.
    -

    -L'émetteur/récepteur exempt de licence contenu dans le présent appareil est conforme aux CNR d'Innovation, Sciences -et Développement économique Canada applicables aux appareils radio exempts de licence. L'exploitation est autorisée -aux deux conditions suivantes :
    -1. L'appareil ne doit pas produire de brouillage.
    -2. L'appareil doit accepter tout brouillage radioélectrique subi, même si le brouillage est susceptible d'en compromettre
    diff --git a/selfdrive/assets/offroad/tc.html b/selfdrive/assets/offroad/tc.html new file mode 100644 index 00000000000000..f88daf08f24602 --- /dev/null +++ b/selfdrive/assets/offroad/tc.html @@ -0,0 +1,44 @@ + + + + + openpilot Terms of Service + + + + +

    The Terms and Conditions below are effective for all users

    +

    Last Updated on October 18, 2019

    +

    Please read these Terms of Use (“Terms”) carefully before using openpilot which is open-sourced software developed by Comma.ai, Inc., a corporation organized under the laws of Delaware (“comma,” “us,” “we,” or “our”).

    +

    Before using and by accessing openpilot, you indicate that you have read, understood, and agree to these Terms. These Terms apply to all users and others who access or use openpilot. If others use openpilot through your user account or vehicle, you are responsible to ensure that they only use openpilot when it is safe to do so, and in compliance with these Terms and with applicable law. If you disagree with any part of the Terms, you should not access or use openpilot.

    +

    Communications

    +

    You agree that comma may contact you by email or telephone in connection with openpilot or for other business purposes. You may opt out of receiving email messages at any time by contacting us at support@comma.ai.

    +

    We collect, use, and share information from and about you and your vehicle in connection with openpilot. You consent to comma accessing the systems associated with openpilot, without additional notice or consent, for the purposes of providing openpilot, data collection, software updates, safety and cybersecurity, suspension or removal of your account, and as disclosed in the Privacy Policy (available at https://connect.comma.ai/privacy).

    +

    Safety

    +

    openpilot performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) designed for use in compatible motor vehicles. While using openpilot, it is your responsibility to obey all laws, traffic rules, and traffic regulations governing your vehicle and its operation. Access to and use of openpilot is at your own risk and responsibility, and openpilot should be accessed and/or used only when you can do so safely.

    +

    openpilot does not make your vehicle “autonomous” or capable of operation without the active monitoring of a licensed driver. It is designed to assist a licensed driver. A licensed driver must pay attention to the road, remain aware of navigation at all times, and be prepared to take immediate action. Failure to do so can cause damage, injury, or death.

    +

    Supported Locations and Models

    +

    openpilot is compatible only with particular makes and models of vehicles. For a complete list of currently supported vehicles, visit https://comma.ai. openpilot will not function properly when installed in an incompatible vehicle. openpilot is compatible only within the geographical boundaries of the United States of America.

    +

    Indemnification

    +

    To the maximum extent allowable by law, you agree to defend, indemnify and hold harmless comma, and its employees, partners, suppliers, contractors, investors, agents, officers, directors, and affiliates, from and against any and all claims, damages, causes of action, penalties, interest, demands, obligations, losses, liabilities, costs or debt, additional taxes, and expenses (including but not limited to attorneys’ fees), resulting from or arising out of (i) your use and access of, or inability to use or access, openpilot, (ii) your breach of these Terms, (iii) the inaccuracy of any information, representation or warranty made by you, (iv) activities of anyone other than you in connection with openpilot conducted through your comma device or account, (v) any other of your activities under or in connection with these Terms or openpilot.

    +

    Limitation of Liability

    +

    In no event shall comma, nor its directors, employees, partners, agents, suppliers, or affiliates, be liable for any indirect, incidental, special, consequential or punitive damages, including without limitation, loss of profits, data, use, goodwill, or other intangible losses, resulting from (i) your access to or use of or inability to access or use of the Software; or (ii) any conduct or content of any third party on the Software whether based on warranty, contract, tort (including negligence) or any other legal theory, whether or not we have been informed of the possibility of such damage, and even if a remedy set forth herein is found to have failed of its essential purpose.

    +

    No Warranty or Obligations to Maintain or Service

    +

    comma provides openpilot without representations, conditions, or warranties of any kind. openpilot is provided on an “AS IS” and “AS AVAILABLE” basis, including with all faults and errors as may occur. To the extent permitted by law and unless prohibited by law, comma on behalf of itself and all persons and parties acting by, through, or for comma, explicitly disclaims all warranties or conditions, express, implied, or collateral, including any implied warranties of merchantability, satisfactory quality, and fitness for a particular purpose in respect of openpilot.

    +

    To the extent permitted by law, comma does not warrant the operation, performance, or availability of openpilot under all conditions. comma is not responsible for any failures caused by server errors, misdirected or redirected transmissions, failed internet connections, interruptions or failures in the transmission of data, any computer virus, or any acts or omissions of third parties that damage the network or impair wireless service.

    +

    We undertake reasonable measures to preserve and secure information collected through our openpilot. However, no data collection, transmission or storage system is 100% secure, and there is always a risk that your information may be intercepted without our consent. In using openpilot, you acknowledge that comma is not responsible for intercepted information, and you hereby release us from any and all claims arising out of or related to the use of intercepted information in any unauthorized manner.

    +

    By providing openpilot, comma does not transfer or license its intellectual property or grant rights in its brand names, nor does comma make representations with respect to third-party intellectual property rights.

    +

    We are not obligated to provide any maintenance or support for openpilot, technical or otherwise. If we voluntarily provide any maintenance or support for openpilot, we may stop any such maintenance, support, or services at any time in our sole discretion.

    +

    Modification of Software

    +

    In no event shall comma, nor its directors, employees, partners, agents, suppliers, or affiliates, be liable if you choose to modify the software.

    +

    Changes

    +

    We reserve the right, at our sole discretion, to modify or replace these Terms at any time. If a revision is material we will provide at least 15 days’ notice prior to any new terms taking effect. What constitutes a material change will be determined at our sole discretion.

    +

    By continuing to access or use our Software after any revisions become effective, you agree to be bound by the revised terms. If you do not agree to the new terms, you are no longer authorized to use the Software.

    +

    Contact Us

    +

    If you have any questions about these Terms, please contact us at support@comma.ai.

    + + diff --git a/selfdrive/assets/prep-svg.sh b/selfdrive/assets/prep-svg.sh deleted file mode 100755 index 2332cd25c52dd9..00000000000000 --- a/selfdrive/assets/prep-svg.sh +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env bash -set -e - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -ICONS_DIR="$DIR/icons" -BOOTSTRAP_SVG="$DIR/../../third_party/bootstrap/bootstrap-icons.svg" - -ICON_IDS=( - arrow-right - backspace - capslock-fill - shift - shift-fill -) -ICON_FILL_COLOR="#fff" - -# extract bootstrap icons -for id in "${ICON_IDS[@]}"; do - svg="${ICONS_DIR}/${id}.svg" - perl -0777 -ne "print \$& if /]*id=\"$id\"[^>]*>.*?<\/symbol>/s" "$BOOTSTRAP_SVG" \ - | sed "s//<\/svg>/" > "$svg" -done - -# sudo apt install inkscape - -for svg in $(find $DIR -type f | grep svg$); do - bunx svgo $svg --multipass --pretty --indent 2 - - # convert to PNG - png="${svg%.svg}.png" - width=$(inkscape --query-width "$svg") - height=$(inkscape --query-height "$svg") - if (( $(echo "$width > $height" | bc -l) )); then - export_dim="--export-width=512" - else - export_dim="--export-height=512" - fi - inkscape "$svg" --export-filename="$png" "$export_dim" - - optipng -o7 -strip all "$png" -done - -# cleanup bootstrap SVGs -for id in "${ICON_IDS[@]}"; do - rm "${ICONS_DIR}/${id}.svg" -done diff --git a/selfdrive/assets/sounds/disengage.wav b/selfdrive/assets/sounds/disengage.wav index 7bfd97ad715e2e..ba583c41f362d3 100644 Binary files a/selfdrive/assets/sounds/disengage.wav and b/selfdrive/assets/sounds/disengage.wav differ diff --git a/selfdrive/assets/sounds/disengage_tizi.wav b/selfdrive/assets/sounds/disengage_tizi.wav deleted file mode 100644 index f3b5f21a27c7ae..00000000000000 --- a/selfdrive/assets/sounds/disengage_tizi.wav +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f061777d66d8d856a5fcb17378416f959088885ed770181355195ad15de881b -size 68628 diff --git a/selfdrive/assets/sounds/engage.wav b/selfdrive/assets/sounds/engage.wav index 8633b5ac2d82b1..41e9b2d588d4aa 100644 Binary files a/selfdrive/assets/sounds/engage.wav and b/selfdrive/assets/sounds/engage.wav differ diff --git a/selfdrive/assets/sounds/engage_tizi.wav b/selfdrive/assets/sounds/engage_tizi.wav deleted file mode 100644 index fc24a23c2f7da4..00000000000000 --- a/selfdrive/assets/sounds/engage_tizi.wav +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b54a85cc09b8ee79fce23d48209205d2e70510eed4c5a86fa400fe3f7caebfd -size 63120 diff --git a/selfdrive/assets/sounds/make_beeps.py b/selfdrive/assets/sounds/make_beeps.py deleted file mode 100644 index 6161e80e742a04..00000000000000 --- a/selfdrive/assets/sounds/make_beeps.py +++ /dev/null @@ -1,19 +0,0 @@ -import numpy as np -from scipy.io import wavfile - - -sr = 48000 -max_int16 = 2**15 - 1 - -def harmonic_beep(freq, duration_seconds): - n_total = int(sr * duration_seconds) - - signal = np.sin(2 * np.pi * freq * np.arange(n_total) / sr) - x = np.arange(n_total) - exp_scale = np.exp(-x/5.5e3) - return max_int16 * signal * exp_scale - -engage_beep = harmonic_beep(1661.219, 0.5) -wavfile.write("engage.wav", sr, engage_beep.astype(np.int16)) -disengage_beep = harmonic_beep(1318.51, 0.5) -wavfile.write("disengage.wav", sr, disengage_beep.astype(np.int16)) diff --git a/selfdrive/assets/sounds/prompt.wav b/selfdrive/assets/sounds/prompt.wav index e482c85a629806..1ae77051eb5722 100644 Binary files a/selfdrive/assets/sounds/prompt.wav and b/selfdrive/assets/sounds/prompt.wav differ diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav index 750d580f04827d..c3d4475caa70ce 100644 Binary files a/selfdrive/assets/sounds/prompt_distracted.wav and b/selfdrive/assets/sounds/prompt_distracted.wav differ diff --git a/selfdrive/assets/sounds/refuse.wav b/selfdrive/assets/sounds/refuse.wav index 1e0c47697d89cb..0e80f7d127dcd4 100644 Binary files a/selfdrive/assets/sounds/refuse.wav and b/selfdrive/assets/sounds/refuse.wav differ diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav index fcbfed79ed87d6..9f6f672e2829c0 100644 Binary files a/selfdrive/assets/sounds/warning_immediate.wav and b/selfdrive/assets/sounds/warning_immediate.wav differ diff --git a/selfdrive/assets/sounds/warning_soft.wav b/selfdrive/assets/sounds/warning_soft.wav index 7db30303d60f5d..261c7e1376c672 100644 Binary files a/selfdrive/assets/sounds/warning_soft.wav and b/selfdrive/assets/sounds/warning_soft.wav differ diff --git a/selfdrive/assets/strip-svg-metadata.sh b/selfdrive/assets/strip-svg-metadata.sh new file mode 100755 index 00000000000000..a8b35eadde300d --- /dev/null +++ b/selfdrive/assets/strip-svg-metadata.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# sudo apt install scour + +for svg in $(find icons/ -type f | grep svg$); do + # scour doesn't support overwriting input file + scour $svg --remove-metadata $svg.tmp + mv $svg.tmp $svg +done diff --git a/selfdrive/assets/training/step0.png b/selfdrive/assets/training/step0.png index fb869e317d3561..b942703b5d5694 100644 Binary files a/selfdrive/assets/training/step0.png and b/selfdrive/assets/training/step0.png differ diff --git a/selfdrive/assets/training/step1.png b/selfdrive/assets/training/step1.png index 8bd9fbaa676123..e2c9f9f60efd53 100644 Binary files a/selfdrive/assets/training/step1.png and b/selfdrive/assets/training/step1.png differ diff --git a/selfdrive/assets/training/step10.png b/selfdrive/assets/training/step10.png index da71f97e830035..c5ed8fd624560d 100644 Binary files a/selfdrive/assets/training/step10.png and b/selfdrive/assets/training/step10.png differ diff --git a/selfdrive/assets/training/step11.png b/selfdrive/assets/training/step11.png index aa3791304ea29e..4776593922c67b 100644 Binary files a/selfdrive/assets/training/step11.png and b/selfdrive/assets/training/step11.png differ diff --git a/selfdrive/assets/training/step12.png b/selfdrive/assets/training/step12.png index 745a8d0553d31b..497170c9787b93 100644 Binary files a/selfdrive/assets/training/step12.png and b/selfdrive/assets/training/step12.png differ diff --git a/selfdrive/assets/training/step13.png b/selfdrive/assets/training/step13.png index 68638ffec138d1..228d7549d45b6e 100644 Binary files a/selfdrive/assets/training/step13.png and b/selfdrive/assets/training/step13.png differ diff --git a/selfdrive/assets/training/step14.png b/selfdrive/assets/training/step14.png index 2f5956b1ff4057..7f8da0552b35f2 100644 Binary files a/selfdrive/assets/training/step14.png and b/selfdrive/assets/training/step14.png differ diff --git a/selfdrive/assets/training/step15.png b/selfdrive/assets/training/step15.png index 798e01d00aac14..9aa861c9fa61d8 100644 Binary files a/selfdrive/assets/training/step15.png and b/selfdrive/assets/training/step15.png differ diff --git a/selfdrive/assets/training/step16.png b/selfdrive/assets/training/step16.png index 27288141819116..e0b36b033725fd 100644 Binary files a/selfdrive/assets/training/step16.png and b/selfdrive/assets/training/step16.png differ diff --git a/selfdrive/assets/training/step17.png b/selfdrive/assets/training/step17.png index ab54f9c992ec2c..c6b33c237ed93e 100644 Binary files a/selfdrive/assets/training/step17.png and b/selfdrive/assets/training/step17.png differ diff --git a/selfdrive/assets/training/step18.png b/selfdrive/assets/training/step18.png index 8149e79364c32a..bd062d4cc02fe0 100644 Binary files a/selfdrive/assets/training/step18.png and b/selfdrive/assets/training/step18.png differ diff --git a/selfdrive/assets/training/step2.png b/selfdrive/assets/training/step2.png index 35d65c9b01137e..97c2eb0f4bd3c7 100644 Binary files a/selfdrive/assets/training/step2.png and b/selfdrive/assets/training/step2.png differ diff --git a/selfdrive/assets/training/step3.png b/selfdrive/assets/training/step3.png index 0d3a6bbecbf900..74897223160954 100644 Binary files a/selfdrive/assets/training/step3.png and b/selfdrive/assets/training/step3.png differ diff --git a/selfdrive/assets/training/step4.png b/selfdrive/assets/training/step4.png index 1d4da570b19a19..8139349ff7e814 100644 Binary files a/selfdrive/assets/training/step4.png and b/selfdrive/assets/training/step4.png differ diff --git a/selfdrive/assets/training/step5.png b/selfdrive/assets/training/step5.png index 1ce43e28dc47cf..714162ae1f41af 100644 Binary files a/selfdrive/assets/training/step5.png and b/selfdrive/assets/training/step5.png differ diff --git a/selfdrive/assets/training/step6.png b/selfdrive/assets/training/step6.png index d0e06469c7f11c..356d76a3e87e74 100644 Binary files a/selfdrive/assets/training/step6.png and b/selfdrive/assets/training/step6.png differ diff --git a/selfdrive/assets/training/step7.png b/selfdrive/assets/training/step7.png index 63e6e27e077df0..ac09faffe8b1ac 100644 Binary files a/selfdrive/assets/training/step7.png and b/selfdrive/assets/training/step7.png differ diff --git a/selfdrive/assets/training/step8.png b/selfdrive/assets/training/step8.png index de881446dc1fbf..f081ac6e45b553 100644 Binary files a/selfdrive/assets/training/step8.png and b/selfdrive/assets/training/step8.png differ diff --git a/selfdrive/assets/training/step9.png b/selfdrive/assets/training/step9.png index 43ca1d03dfc25f..540dafe787b663 100644 Binary files a/selfdrive/assets/training/step9.png and b/selfdrive/assets/training/step9.png differ diff --git a/selfdrive/assets/training_wide/step0.png b/selfdrive/assets/training_wide/step0.png new file mode 100644 index 00000000000000..3c2c5c72a0d519 Binary files /dev/null and b/selfdrive/assets/training_wide/step0.png differ diff --git a/selfdrive/assets/training_wide/step1.png b/selfdrive/assets/training_wide/step1.png new file mode 100644 index 00000000000000..085789311835b3 Binary files /dev/null and b/selfdrive/assets/training_wide/step1.png differ diff --git a/selfdrive/assets/training_wide/step10.png b/selfdrive/assets/training_wide/step10.png new file mode 100644 index 00000000000000..2941316d1745d8 Binary files /dev/null and b/selfdrive/assets/training_wide/step10.png differ diff --git a/selfdrive/assets/training_wide/step11.png b/selfdrive/assets/training_wide/step11.png new file mode 100644 index 00000000000000..7a7c72e3df295d Binary files /dev/null and b/selfdrive/assets/training_wide/step11.png differ diff --git a/selfdrive/assets/training_wide/step12.png b/selfdrive/assets/training_wide/step12.png new file mode 100644 index 00000000000000..0d6f64eb84e18e Binary files /dev/null and b/selfdrive/assets/training_wide/step12.png differ diff --git a/selfdrive/assets/training_wide/step13.png b/selfdrive/assets/training_wide/step13.png new file mode 100644 index 00000000000000..565e02fa3f4dfb Binary files /dev/null and b/selfdrive/assets/training_wide/step13.png differ diff --git a/selfdrive/assets/training_wide/step14.png b/selfdrive/assets/training_wide/step14.png new file mode 100644 index 00000000000000..225231cbaa42b6 Binary files /dev/null and b/selfdrive/assets/training_wide/step14.png differ diff --git a/selfdrive/assets/training_wide/step15.png b/selfdrive/assets/training_wide/step15.png new file mode 100644 index 00000000000000..929c759b269c94 Binary files /dev/null and b/selfdrive/assets/training_wide/step15.png differ diff --git a/selfdrive/assets/training_wide/step16.png b/selfdrive/assets/training_wide/step16.png new file mode 100644 index 00000000000000..161af863aaeedf Binary files /dev/null and b/selfdrive/assets/training_wide/step16.png differ diff --git a/selfdrive/assets/training_wide/step17.png b/selfdrive/assets/training_wide/step17.png new file mode 100644 index 00000000000000..1b0cdb6fbcf035 Binary files /dev/null and b/selfdrive/assets/training_wide/step17.png differ diff --git a/selfdrive/assets/training_wide/step18.png b/selfdrive/assets/training_wide/step18.png new file mode 100644 index 00000000000000..0e3b64bab5107b Binary files /dev/null and b/selfdrive/assets/training_wide/step18.png differ diff --git a/selfdrive/assets/training_wide/step2.png b/selfdrive/assets/training_wide/step2.png new file mode 100644 index 00000000000000..55814b8ef9185f Binary files /dev/null and b/selfdrive/assets/training_wide/step2.png differ diff --git a/selfdrive/assets/training_wide/step3.png b/selfdrive/assets/training_wide/step3.png new file mode 100644 index 00000000000000..831095b0ae67ea Binary files /dev/null and b/selfdrive/assets/training_wide/step3.png differ diff --git a/selfdrive/assets/training_wide/step4.png b/selfdrive/assets/training_wide/step4.png new file mode 100644 index 00000000000000..5433034939de64 Binary files /dev/null and b/selfdrive/assets/training_wide/step4.png differ diff --git a/selfdrive/assets/training_wide/step5.png b/selfdrive/assets/training_wide/step5.png new file mode 100644 index 00000000000000..7191b63a0c254d Binary files /dev/null and b/selfdrive/assets/training_wide/step5.png differ diff --git a/selfdrive/assets/training_wide/step6.png b/selfdrive/assets/training_wide/step6.png new file mode 100644 index 00000000000000..8eafd4a198428f Binary files /dev/null and b/selfdrive/assets/training_wide/step6.png differ diff --git a/selfdrive/assets/training_wide/step7.png b/selfdrive/assets/training_wide/step7.png new file mode 100644 index 00000000000000..502f5f1b2e401a Binary files /dev/null and b/selfdrive/assets/training_wide/step7.png differ diff --git a/selfdrive/assets/training_wide/step8.png b/selfdrive/assets/training_wide/step8.png new file mode 100644 index 00000000000000..c4e8668332cddc Binary files /dev/null and b/selfdrive/assets/training_wide/step8.png differ diff --git a/selfdrive/assets/training_wide/step9.png b/selfdrive/assets/training_wide/step9.png new file mode 100644 index 00000000000000..84eae3a066de9f Binary files /dev/null and b/selfdrive/assets/training_wide/step9.png differ diff --git a/selfdrive/modeld/models/__init__.py b/selfdrive/athena/__init__.py similarity index 100% rename from selfdrive/modeld/models/__init__.py rename to selfdrive/athena/__init__.py diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py new file mode 100755 index 00000000000000..5b351ca0f53748 --- /dev/null +++ b/selfdrive/athena/athenad.py @@ -0,0 +1,755 @@ +#!/usr/bin/env python3 +import base64 +import bz2 +import hashlib +import io +import json +import os +import queue +import random +import select +import socket +import subprocess +import sys +import tempfile +import threading +import time +from collections import namedtuple +from datetime import datetime +from functools import partial +from typing import Any, Dict + +import requests +from jsonrpc import JSONRPCResponseManager, dispatcher +from websocket import (ABNF, WebSocketException, WebSocketTimeoutException, + create_connection) + +import cereal.messaging as messaging +from cereal import log +from cereal.services import service_list +from common.api import Api +from common.basedir import PERSIST +from common.file_helpers import CallbackReader +from common.params import Params +from common.realtime import sec_since_boot, set_core_affinity +from system.hardware import HARDWARE, PC, AGNOS +from selfdrive.loggerd.config import ROOT +from selfdrive.loggerd.xattr_cache import getxattr, setxattr +from selfdrive.statsd import STATS_DIR +from system.swaglog import SWAGLOG_DIR, cloudlog +from system.version import get_commit, get_origin, get_short_branch, get_version + +ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') +HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) +LOCAL_PORT_WHITELIST = {8022} + +LOG_ATTR_NAME = 'user.upload' +LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder) +RECONNECT_TIMEOUT_S = 70 + +RETRY_DELAY = 10 # seconds +MAX_RETRY_COUNT = 30 # Try for at most 5 minutes if upload fails immediately +MAX_AGE = 31 * 24 * 3600 # seconds +WS_FRAME_SIZE = 4096 + +NetworkType = log.DeviceState.NetworkType + +dispatcher["echo"] = lambda s: s +recv_queue: Any = queue.Queue() +send_queue: Any = queue.Queue() +upload_queue: Any = queue.Queue() +low_priority_send_queue: Any = queue.Queue() +log_recv_queue: Any = queue.Queue() +cancelled_uploads: Any = set() +UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress', 'allow_cellular'], defaults=(0, False, 0, False)) + +cur_upload_items: Dict[int, Any] = {} + + +def strip_bz2_extension(fn): + if fn.endswith('.bz2'): + return fn[:-4] + return fn + + +class AbortTransferException(Exception): + pass + + +class UploadQueueCache(): + params = Params() + + @staticmethod + def initialize(upload_queue): + try: + upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue") + if upload_queue_json is not None: + for item in json.loads(upload_queue_json): + upload_queue.put(UploadItem(**item)) + except Exception: + cloudlog.exception("athena.UploadQueueCache.initialize.exception") + + @staticmethod + def cache(upload_queue): + try: + items = [i._asdict() for i in upload_queue.queue if i.id not in cancelled_uploads] + UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items)) + except Exception: + cloudlog.exception("athena.UploadQueueCache.cache.exception") + + +def handle_long_poll(ws): + end_event = threading.Event() + + threads = [ + threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'), + threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'), + threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'), + threading.Thread(target=log_handler, args=(end_event,), name='log_handler'), + threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'), + ] + [ + threading.Thread(target=jsonrpc_handler, args=(end_event,), name=f'worker_{x}') + for x in range(HANDLER_THREADS) + ] + + for thread in threads: + thread.start() + try: + while not end_event.is_set(): + time.sleep(0.1) + except (KeyboardInterrupt, SystemExit): + end_event.set() + raise + finally: + for thread in threads: + cloudlog.debug(f"athena.joining {thread.name}") + thread.join() + + +def jsonrpc_handler(end_event): + dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event) + while not end_event.is_set(): + try: + data = recv_queue.get(timeout=1) + if "method" in data: + cloudlog.debug(f"athena.jsonrpc_handler.call_method {data}") + response = JSONRPCResponseManager.handle(data, dispatcher) + send_queue.put_nowait(response.json) + elif "id" in data and ("result" in data or "error" in data): + log_recv_queue.put_nowait(data) + else: + raise Exception("not a valid request or response") + except queue.Empty: + pass + except Exception as e: + cloudlog.exception("athena jsonrpc handler failed") + send_queue.put_nowait(json.dumps({"error": str(e)})) + + +def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None: + if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: + item = cur_upload_items[tid] + new_retry_count = item.retry_count + 1 if increase_count else item.retry_count + + item = item._replace( + retry_count=new_retry_count, + progress=0, + current=False + ) + upload_queue.put_nowait(item) + UploadQueueCache.cache(upload_queue) + + cur_upload_items[tid] = None + + for _ in range(RETRY_DELAY): + time.sleep(1) + if end_event.is_set(): + break + + +def upload_handler(end_event: threading.Event) -> None: + sm = messaging.SubMaster(['deviceState']) + tid = threading.get_ident() + + while not end_event.is_set(): + cur_upload_items[tid] = None + + try: + cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True) + + if cur_upload_items[tid].id in cancelled_uploads: + cancelled_uploads.remove(cur_upload_items[tid].id) + continue + + # Remove item if too old + age = datetime.now() - datetime.fromtimestamp(cur_upload_items[tid].created_at / 1000) + if age.total_seconds() > MAX_AGE: + cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True) + continue + + # Check if uploading over metered connection is allowed + sm.update(0) + metered = sm['deviceState'].networkMetered + network_type = sm['deviceState'].networkType.raw + if metered and (not cur_upload_items[tid].allow_cellular): + retry_upload(tid, end_event, False) + continue + + try: + def cb(sz, cur): + # Abort transfer if connection changed to metered after starting upload + sm.update(0) + metered = sm['deviceState'].networkMetered + if metered and (not cur_upload_items[tid].allow_cellular): + raise AbortTransferException + + cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) + + fn = cur_upload_items[tid].path + try: + sz = os.path.getsize(fn) + except OSError: + sz = -1 + + cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=cur_upload_items[tid].retry_count) + response = _do_upload(cur_upload_items[tid], cb) + + if response.status_code not in (200, 201, 401, 403, 412): + cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered) + retry_upload(tid, end_event) + else: + cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered) + + UploadQueueCache.cache(upload_queue) + except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError): + cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered) + retry_upload(tid, end_event) + except AbortTransferException: + cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered) + retry_upload(tid, end_event, False) + + except queue.Empty: + pass + except Exception: + cloudlog.exception("athena.upload_handler.exception") + + +def _do_upload(upload_item, callback=None): + path = upload_item.path + compress = False + + # If file does not exist, but does exist without the .bz2 extension we will compress on the fly + if not os.path.exists(path) and os.path.exists(strip_bz2_extension(path)): + path = strip_bz2_extension(path) + compress = True + + with open(path, "rb") as f: + if compress: + cloudlog.event("athena.upload_handler.compress", fn=path, fn_orig=upload_item.path) + data = bz2.compress(f.read()) + size = len(data) + data = io.BytesIO(data) + else: + size = os.fstat(f.fileno()).st_size + data = f + + if callback: + data = CallbackReader(data, callback, size) + + return requests.put(upload_item.url, + data=data, + headers={**upload_item.headers, 'Content-Length': str(size)}, + timeout=30) + + +# security: user should be able to request any message from their car +@dispatcher.add_method +def getMessage(service=None, timeout=1000): + if service is None or service not in service_list: + raise Exception("invalid service") + + socket = messaging.sub_sock(service, timeout=timeout) + ret = messaging.recv_one(socket) + + if ret is None: + raise TimeoutError + + return ret.to_dict() + + +@dispatcher.add_method +def getVersion() -> Dict[str, str]: + return { + "version": get_version(), + "remote": get_origin(''), + "branch": get_short_branch(''), + "commit": get_commit(default=''), + } + + +@dispatcher.add_method +def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=None): + destination = { + "latitude": latitude, + "longitude": longitude, + "place_name": place_name, + "place_details": place_details, + } + Params().put("NavDestination", json.dumps(destination)) + + return {"success": 1} + + +def scan_dir(path, prefix): + files = list() + # only walk directories that match the prefix + # (glob and friends traverse entire dir tree) + with os.scandir(path) as i: + for e in i: + rel_path = os.path.relpath(e.path, ROOT) + if e.is_dir(follow_symlinks=False): + # add trailing slash + rel_path = os.path.join(rel_path, '') + # if prefix is a partial dir name, current dir will start with prefix + # if prefix is a partial file name, prefix with start with dir name + if rel_path.startswith(prefix) or prefix.startswith(rel_path): + files.extend(scan_dir(e.path, prefix)) + else: + if rel_path.startswith(prefix): + files.append(rel_path) + return files + +@dispatcher.add_method +def listDataDirectory(prefix=''): + return scan_dir(ROOT, prefix) + + +@dispatcher.add_method +def reboot(): + sock = messaging.sub_sock("deviceState", timeout=1000) + ret = messaging.recv_one(sock) + if ret is None or ret.deviceState.started: + raise Exception("Reboot unavailable") + + def do_reboot(): + time.sleep(2) + HARDWARE.reboot() + + threading.Thread(target=do_reboot).start() + + return {"success": 1} + + +@dispatcher.add_method +def uploadFileToUrl(fn, url, headers): + return uploadFilesToUrls([{ + "fn": fn, + "url": url, + "headers": headers, + }]) + + +@dispatcher.add_method +def uploadFilesToUrls(files_data): + items = [] + failed = [] + for file in files_data: + fn = file.get('fn', '') + if len(fn) == 0 or fn[0] == '/' or '..' in fn or 'url' not in file: + failed.append(fn) + continue + + path = os.path.join(ROOT, fn) + if not os.path.exists(path) and not os.path.exists(strip_bz2_extension(path)): + failed.append(fn) + continue + + # Skip item if already in queue + url = file['url'].split('?')[0] + if any(url == item['url'].split('?')[0] for item in listUploadQueue()): + continue + + item = UploadItem( + path=path, + url=file['url'], + headers=file.get('headers', {}), + created_at=int(time.time() * 1000), + id=None, + allow_cellular=file.get('allow_cellular', False), + ) + upload_id = hashlib.sha1(str(item).encode()).hexdigest() + item = item._replace(id=upload_id) + upload_queue.put_nowait(item) + items.append(item._asdict()) + + UploadQueueCache.cache(upload_queue) + + resp = {"enqueued": len(items), "items": items} + if failed: + resp["failed"] = failed + + return resp + + +@dispatcher.add_method +def listUploadQueue(): + items = list(upload_queue.queue) + list(cur_upload_items.values()) + return [i._asdict() for i in items if (i is not None) and (i.id not in cancelled_uploads)] + + +@dispatcher.add_method +def cancelUpload(upload_id): + if not isinstance(upload_id, list): + upload_id = [upload_id] + + uploading_ids = {item.id for item in list(upload_queue.queue)} + cancelled_ids = uploading_ids.intersection(upload_id) + if len(cancelled_ids) == 0: + return 404 + + cancelled_uploads.update(cancelled_ids) + return {"success": 1} + + +@dispatcher.add_method +def primeActivated(activated): + return {"success": 1} + + +@dispatcher.add_method +def setBandwithLimit(upload_speed_kbps, download_speed_kbps): + if not AGNOS: + return {"success": 0, "error": "only supported on AGNOS"} + + try: + HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps) + return {"success": 1} + except subprocess.CalledProcessError as e: + return {"success": 0, "error": "failed to set limit", "stdout": e.stdout, "stderr": e.stderr} + + +def startLocalProxy(global_end_event, remote_ws_uri, local_port): + try: + if local_port not in LOCAL_PORT_WHITELIST: + raise Exception("Requested local port not whitelisted") + + cloudlog.debug("athena.startLocalProxy.starting") + + dongle_id = Params().get("DongleId").decode('utf8') + identity_token = Api(dongle_id).get_token() + ws = create_connection(remote_ws_uri, + cookie="jwt=" + identity_token, + enable_multithread=True) + + ssock, csock = socket.socketpair() + local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + local_sock.connect(('127.0.0.1', local_port)) + local_sock.setblocking(False) + + proxy_end_event = threading.Event() + threads = [ + threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)), + threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event)) + ] + for thread in threads: + thread.start() + + cloudlog.debug("athena.startLocalProxy.started") + return {"success": 1} + except Exception as e: + cloudlog.exception("athenad.startLocalProxy.exception") + raise e + + +@dispatcher.add_method +def getPublicKey(): + if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'): + return None + + with open(PERSIST + '/comma/id_rsa.pub') as f: + return f.read() + + +@dispatcher.add_method +def getSshAuthorizedKeys(): + return Params().get("GithubSshKeys", encoding='utf8') or '' + + +@dispatcher.add_method +def getSimInfo(): + return HARDWARE.get_sim_info() + + +@dispatcher.add_method +def getNetworkType(): + return HARDWARE.get_network_type() + + +@dispatcher.add_method +def getNetworkMetered(): + network_type = HARDWARE.get_network_type() + return HARDWARE.get_network_metered(network_type) + + +@dispatcher.add_method +def getNetworks(): + return HARDWARE.get_networks() + + +@dispatcher.add_method +def takeSnapshot(): + from system.camerad.snapshot.snapshot import jpeg_write, snapshot + ret = snapshot() + if ret is not None: + def b64jpeg(x): + if x is not None: + f = io.BytesIO() + jpeg_write(f, x) + return base64.b64encode(f.getvalue()).decode("utf-8") + else: + return None + return {'jpegBack': b64jpeg(ret[0]), + 'jpegFront': b64jpeg(ret[1])} + else: + raise Exception("not available while camerad is started") + + +def get_logs_to_send_sorted(): + # TODO: scan once then use inotify to detect file creation/deletion + curr_time = int(time.time()) + logs = [] + for log_entry in os.listdir(SWAGLOG_DIR): + log_path = os.path.join(SWAGLOG_DIR, log_entry) + try: + time_sent = int.from_bytes(getxattr(log_path, LOG_ATTR_NAME), sys.byteorder) + except (ValueError, TypeError): + time_sent = 0 + # assume send failed and we lost the response if sent more than one hour ago + if not time_sent or curr_time - time_sent > 3600: + logs.append(log_entry) + # excluding most recent (active) log file + return sorted(logs)[:-1] + + +def log_handler(end_event): + if PC: + return + + log_files = [] + last_scan = 0 + while not end_event.is_set(): + try: + curr_scan = sec_since_boot() + if curr_scan - last_scan > 10: + log_files = get_logs_to_send_sorted() + last_scan = curr_scan + + # send one log + curr_log = None + if len(log_files) > 0: + log_entry = log_files.pop() # newest log file + cloudlog.debug(f"athena.log_handler.forward_request {log_entry}") + try: + curr_time = int(time.time()) + log_path = os.path.join(SWAGLOG_DIR, log_entry) + setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder)) + with open(log_path) as f: + jsonrpc = { + "method": "forwardLogs", + "params": { + "logs": f.read() + }, + "jsonrpc": "2.0", + "id": log_entry + } + low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) + curr_log = log_entry + except OSError: + pass # file could be deleted by log rotation + + # wait for response up to ~100 seconds + # always read queue at least once to process any old responses that arrive + for _ in range(100): + if end_event.is_set(): + break + try: + log_resp = json.loads(log_recv_queue.get(timeout=1)) + log_entry = log_resp.get("id") + log_success = "result" in log_resp and log_resp["result"].get("success") + cloudlog.debug(f"athena.log_handler.forward_response {log_entry} {log_success}") + if log_entry and log_success: + log_path = os.path.join(SWAGLOG_DIR, log_entry) + try: + setxattr(log_path, LOG_ATTR_NAME, LOG_ATTR_VALUE_MAX_UNIX_TIME) + except OSError: + pass # file could be deleted by log rotation + if curr_log == log_entry: + break + except queue.Empty: + if curr_log is None: + break + + except Exception: + cloudlog.exception("athena.log_handler.exception") + + +def stat_handler(end_event): + while not end_event.is_set(): + last_scan = 0 + curr_scan = sec_since_boot() + try: + if curr_scan - last_scan > 10: + stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR))) + if len(stat_filenames) > 0: + stat_path = os.path.join(STATS_DIR, stat_filenames[0]) + with open(stat_path) as f: + jsonrpc = { + "method": "storeStats", + "params": { + "stats": f.read() + }, + "jsonrpc": "2.0", + "id": stat_filenames[0] + } + low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) + os.remove(stat_path) + last_scan = curr_scan + except Exception: + cloudlog.exception("athena.stat_handler.exception") + time.sleep(0.1) + + +def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): + while not (end_event.is_set() or global_end_event.is_set()): + try: + data = ws.recv() + local_sock.sendall(data) + except WebSocketTimeoutException: + pass + except Exception: + cloudlog.exception("athenad.ws_proxy_recv.exception") + break + + cloudlog.debug("athena.ws_proxy_recv closing sockets") + ssock.close() + local_sock.close() + cloudlog.debug("athena.ws_proxy_recv done closing sockets") + + end_event.set() + + +def ws_proxy_send(ws, local_sock, signal_sock, end_event): + while not end_event.is_set(): + try: + r, _, _ = select.select((local_sock, signal_sock), (), ()) + if r: + if r[0].fileno() == signal_sock.fileno(): + # got end signal from ws_proxy_recv + end_event.set() + break + data = local_sock.recv(4096) + if not data: + # local_sock is dead + end_event.set() + break + + ws.send(data, ABNF.OPCODE_BINARY) + except Exception: + cloudlog.exception("athenad.ws_proxy_send.exception") + end_event.set() + + cloudlog.debug("athena.ws_proxy_send closing sockets") + signal_sock.close() + cloudlog.debug("athena.ws_proxy_send done closing sockets") + + +def ws_recv(ws, end_event): + last_ping = int(sec_since_boot() * 1e9) + while not end_event.is_set(): + try: + opcode, data = ws.recv_data(control_frame=True) + if opcode in (ABNF.OPCODE_TEXT, ABNF.OPCODE_BINARY): + if opcode == ABNF.OPCODE_TEXT: + data = data.decode("utf-8") + recv_queue.put_nowait(data) + elif opcode == ABNF.OPCODE_PING: + last_ping = int(sec_since_boot() * 1e9) + Params().put("LastAthenaPingTime", str(last_ping)) + except WebSocketTimeoutException: + ns_since_last_ping = int(sec_since_boot() * 1e9) - last_ping + if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9: + cloudlog.exception("athenad.ws_recv.timeout") + end_event.set() + except Exception: + cloudlog.exception("athenad.ws_recv.exception") + end_event.set() + + +def ws_send(ws, end_event): + while not end_event.is_set(): + try: + try: + data = send_queue.get_nowait() + except queue.Empty: + data = low_priority_send_queue.get(timeout=1) + for i in range(0, len(data), WS_FRAME_SIZE): + frame = data[i:i+WS_FRAME_SIZE] + last = i + WS_FRAME_SIZE >= len(data) + opcode = ABNF.OPCODE_TEXT if i == 0 else ABNF.OPCODE_CONT + ws.send_frame(ABNF.create_frame(frame, opcode, last)) + except queue.Empty: + pass + except Exception: + cloudlog.exception("athenad.ws_send.exception") + end_event.set() + + +def backoff(retries): + return random.randrange(0, min(128, int(2 ** retries))) + + +def main(): + try: + set_core_affinity([0, 1, 2, 3]) + except Exception: + cloudlog.exception("failed to set core affinity") + + params = Params() + dongle_id = params.get("DongleId", encoding='utf-8') + UploadQueueCache.initialize(upload_queue) + + ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id + api = Api(dongle_id) + + conn_retries = 0 + while 1: + try: + cloudlog.event("athenad.main.connecting_ws", ws_uri=ws_uri) + ws = create_connection(ws_uri, + cookie="jwt=" + api.get_token(), + enable_multithread=True, + timeout=30.0) + cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri) + + conn_retries = 0 + cur_upload_items.clear() + + handle_long_poll(ws) + except (KeyboardInterrupt, SystemExit): + break + except (ConnectionError, TimeoutError, WebSocketException): + conn_retries += 1 + params.remove("LastAthenaPingTime") + except socket.timeout: + params.remove("LastAthenaPingTime") + except Exception: + cloudlog.exception("athenad.main.exception") + + conn_retries += 1 + params.remove("LastAthenaPingTime") + + time.sleep(backoff(conn_retries)) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py new file mode 100755 index 00000000000000..59ca2430ce3116 --- /dev/null +++ b/selfdrive/athena/manage_athenad.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 + +import time +from multiprocessing import Process + +from common.params import Params +from selfdrive.manager.process import launcher +from system.swaglog import cloudlog +from system.version import get_version, is_dirty + +ATHENA_MGR_PID_PARAM = "AthenadPid" + + +def main(): + params = Params() + dongle_id = params.get("DongleId").decode('utf-8') + cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty()) + + try: + while 1: + cloudlog.info("starting athena daemon") + proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad')) + proc.start() + proc.join() + cloudlog.event("athenad exited", exitcode=proc.exitcode) + time.sleep(5) + except Exception: + cloudlog.exception("manage_athenad.exception") + finally: + params.remove(ATHENA_MGR_PID_PARAM) + + +if __name__ == '__main__': + main() diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py new file mode 100755 index 00000000000000..32bc92059cf231 --- /dev/null +++ b/selfdrive/athena/registration.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +import time +import json +import jwt +from pathlib import Path +from typing import Optional + +from datetime import datetime, timedelta +from common.api import api_get +from common.params import Params +from common.spinner import Spinner +from common.basedir import PERSIST +from selfdrive.controls.lib.alertmanager import set_offroad_alert +from system.hardware import HARDWARE, PC +from system.swaglog import cloudlog + + +UNREGISTERED_DONGLE_ID = "UnregisteredDevice" + + +def is_registered_device() -> bool: + dongle = Params().get("DongleId", encoding='utf-8') + return dongle not in (None, UNREGISTERED_DONGLE_ID) + + +def register(show_spinner=False) -> Optional[str]: + params = Params() + params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) + + IMEI = params.get("IMEI", encoding='utf8') + HardwareSerial = params.get("HardwareSerial", encoding='utf8') + dongle_id: Optional[str] = params.get("DongleId", encoding='utf8') + needs_registration = None in (IMEI, HardwareSerial, dongle_id) + + pubkey = Path(PERSIST+"/comma/id_rsa.pub") + if not pubkey.is_file(): + dongle_id = UNREGISTERED_DONGLE_ID + cloudlog.warning(f"missing public key: {pubkey}") + elif needs_registration: + if show_spinner: + spinner = Spinner() + spinner.update("registering device") + + # Create registration token, in the future, this key will make JWTs directly + with open(PERSIST+"/comma/id_rsa.pub") as f1, open(PERSIST+"/comma/id_rsa") as f2: + public_key = f1.read() + private_key = f2.read() + + # Block until we get the imei + serial = HARDWARE.get_serial() + start_time = time.monotonic() + imei1: Optional[str] = None + imei2: Optional[str] = None + while imei1 is None and imei2 is None: + try: + imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) + except Exception: + cloudlog.exception("Error getting imei, trying again...") + time.sleep(1) + + if time.monotonic() - start_time > 60 and show_spinner: + spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") + + params.put("IMEI", imei1) + params.put("HardwareSerial", serial) + + backoff = 0 + start_time = time.monotonic() + while True: + try: + register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') + cloudlog.info("getting pilotauth") + resp = api_get("v2/pilotauth/", method='POST', timeout=15, + imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) + + if resp.status_code in (402, 403): + cloudlog.info(f"Unable to register device, got {resp.status_code}") + dongle_id = UNREGISTERED_DONGLE_ID + else: + dongleauth = json.loads(resp.text) + dongle_id = dongleauth["dongle_id"] + break + except Exception: + cloudlog.exception("failed to authenticate") + backoff = min(backoff + 1, 15) + time.sleep(backoff) + + if time.monotonic() - start_time > 60 and show_spinner: + spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") + + if show_spinner: + spinner.close() + + if dongle_id: + params.put("DongleId", dongle_id) + set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC) + return dongle_id + + +if __name__ == "__main__": + print(register()) diff --git a/selfdrive/pandad/tests/__init__.py b/selfdrive/athena/tests/__init__.py similarity index 100% rename from selfdrive/pandad/tests/__init__.py rename to selfdrive/athena/tests/__init__.py diff --git a/selfdrive/athena/tests/helpers.py b/selfdrive/athena/tests/helpers.py new file mode 100644 index 00000000000000..071393cb14fa1f --- /dev/null +++ b/selfdrive/athena/tests/helpers.py @@ -0,0 +1,129 @@ +import http.server +import random +import requests +import socket +import time +from functools import wraps +from multiprocessing import Process + +from common.timeout import Timeout + + +class MockResponse: + def __init__(self, json, status_code): + self.json = json + self.text = json + self.status_code = status_code + + +class EchoSocket(): + def __init__(self, port): + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind(('127.0.0.1', port)) + self.socket.listen(1) + + def run(self): + conn, _ = self.socket.accept() + conn.settimeout(5.0) + + try: + while True: + data = conn.recv(4096) + if data: + print(f'EchoSocket got {data}') + conn.sendall(data) + else: + break + finally: + conn.shutdown(0) + conn.close() + self.socket.shutdown(0) + self.socket.close() + + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get_token(self): + return "fake-token" + + +class MockParams(): + default_params = { + "DongleId": b"0000000000000000", + "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501 + "AthenadUploadQueue": '[]', + "CellularUnmetered": False, + } + params = default_params.copy() + + @staticmethod + def restore_defaults(): + MockParams.params = MockParams.default_params.copy() + + def get_bool(self, k): + return bool(MockParams.params.get(k)) + + def get(self, k, encoding=None): + ret = MockParams.params.get(k) + if ret is not None and encoding is not None: + ret = ret.decode(encoding) + return ret + + def put(self, k, v): + if k not in MockParams.params: + raise KeyError(f"key: {k} not in MockParams") + MockParams.params[k] = v + + +class MockWebsocket(): + def __init__(self, recv_queue, send_queue): + self.recv_queue = recv_queue + self.send_queue = send_queue + + def recv(self): + data = self.recv_queue.get() + if isinstance(data, Exception): + raise data + return data + + def send(self, data, opcode): + self.send_queue.put_nowait((data, opcode)) + + +class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): + def do_PUT(self): + length = int(self.headers['Content-Length']) + self.rfile.read(length) + self.send_response(201, "Created") + self.end_headers() + + +def with_http_server(func): + @wraps(func) + def inner(*args, **kwargs): + with Timeout(2, 'HTTP Server did not start'): + p = None + host = '127.0.0.1' + while p is None or p.exitcode is not None: + port = random.randrange(40000, 50000) + p = Process(target=http.server.test, + kwargs={'port': port, 'HandlerClass': HTTPRequestHandler, 'bind': host}) + p.start() + time.sleep(0.1) + + with Timeout(2, 'HTTP Server seeding failed'): + while True: + try: + requests.put(f'http://{host}:{port}/qlog.bz2', data='', timeout=10) + break + except requests.exceptions.ConnectionError: + time.sleep(0.1) + + try: + return func(*args, f'http://{host}:{port}', **kwargs) + finally: + p.terminate() + + return inner diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py new file mode 100755 index 00000000000000..7f511eecf686fe --- /dev/null +++ b/selfdrive/athena/tests/test_athenad.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python3 +import json +import os +import requests +import shutil +import tempfile +import time +import threading +import queue +import unittest +from datetime import datetime, timedelta + +from multiprocessing import Process +from pathlib import Path +from unittest import mock +from websocket import ABNF +from websocket._exceptions import WebSocketConnectionClosedException + +from system import swaglog +from selfdrive.athena import athenad +from selfdrive.athena.athenad import MAX_RETRY_COUNT, dispatcher +from selfdrive.athena.tests.helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server +from cereal import messaging + + +class TestAthenadMethods(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.SOCKET_PORT = 45454 + athenad.Params = MockParams + athenad.ROOT = tempfile.mkdtemp() + athenad.SWAGLOG_DIR = swaglog.SWAGLOG_DIR = tempfile.mkdtemp() + athenad.Api = MockApi + athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT} + + def setUp(self): + MockParams.restore_defaults() + athenad.upload_queue = queue.Queue() + athenad.cur_upload_items.clear() + athenad.cancelled_uploads.clear() + + for i in os.listdir(athenad.ROOT): + p = os.path.join(athenad.ROOT, i) + if os.path.isdir(p): + shutil.rmtree(p) + else: + os.unlink(p) + + def wait_for_upload(self): + now = time.time() + while time.time() - now < 5: + if athenad.upload_queue.qsize() == 0: + break + + def test_echo(self): + assert dispatcher["echo"]("bob") == "bob" + + def test_getMessage(self): + with self.assertRaises(TimeoutError) as _: + dispatcher["getMessage"]("controlsState") + + def send_deviceState(): + messaging.context = messaging.Context() + pub_sock = messaging.pub_sock("deviceState") + start = time.time() + + while time.time() - start < 1: + msg = messaging.new_message('deviceState') + pub_sock.send(msg.to_bytes()) + time.sleep(0.01) + + p = Process(target=send_deviceState) + p.start() + time.sleep(0.1) + try: + deviceState = dispatcher["getMessage"]("deviceState") + assert deviceState['deviceState'] + finally: + p.terminate() + + def test_listDataDirectory(self): + route = '2021-03-29--13-32-47' + segments = [0, 1, 2, 3, 11] + + filenames = ['qlog', 'qcamera.ts', 'rlog', 'fcamera.hevc', 'ecamera.hevc', 'dcamera.hevc'] + files = [f'{route}--{s}/{f}' for s in segments for f in filenames] + for file in files: + fn = os.path.join(athenad.ROOT, file) + os.makedirs(os.path.dirname(fn), exist_ok=True) + Path(fn).touch() + + resp = dispatcher["listDataDirectory"]() + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, files) + + resp = dispatcher["listDataDirectory"](f'{route}--123') + self.assertCountEqual(resp, []) + + prefix = f'{route}' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1/' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + prefix = f'{route}--1/q' + expected = filter(lambda f: f.startswith(prefix), files) + resp = dispatcher["listDataDirectory"](prefix) + self.assertTrue(resp, 'list empty!') + self.assertCountEqual(resp, expected) + + def test_strip_bz2_extension(self): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + if fn.endswith('.bz2'): + self.assertEqual(athenad.strip_bz2_extension(fn), fn[:-4]) + + + @with_http_server + def test_do_upload(self, host): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + + item = athenad.UploadItem(path=fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') + with self.assertRaises(requests.exceptions.ConnectionError): + athenad._do_upload(item) + + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + resp = athenad._do_upload(item) + self.assertEqual(resp.status_code, 201) + + @with_http_server + def test_uploadFileToUrl(self, host): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", f"{host}/qlog.bz2", {}) + self.assertEqual(resp['enqueued'], 1) + self.assertNotIn('failed', resp) + self.assertDictContainsSubset({"path": fn, "url": f"{host}/qlog.bz2", "headers": {}}, resp['items'][0]) + self.assertIsNotNone(resp['items'][0].get('id')) + self.assertEqual(athenad.upload_queue.qsize(), 1) + + @with_http_server + def test_uploadFileToUrl_duplicate(self, host): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + + url1 = f"{host}/qlog.bz2?sig=sig1" + dispatcher["uploadFileToUrl"]("qlog.bz2", url1, {}) + + # Upload same file again, but with different signature + url2 = f"{host}/qlog.bz2?sig=sig2" + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", url2, {}) + self.assertEqual(resp, {'enqueued': 0, 'items': []}) + + @with_http_server + def test_uploadFileToUrl_does_not_exist(self, host): + not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {}) + self.assertEqual(not_exists_resp, {'enqueued': 0, 'items': [], 'failed': ['does_not_exist.bz2']}) + + @with_http_server + def test_upload_handler(self, host): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + athenad.upload_queue.put_nowait(item) + try: + self.wait_for_upload() + time.sleep(0.1) + + # TODO: verify that upload actually succeeded + self.assertEqual(athenad.upload_queue.qsize(), 0) + finally: + end_event.set() + + @with_http_server + @mock.patch('requests.put') + def test_upload_handler_retry(self, host, mock_put): + for status, retry in ((500, True), (412, False)): + mock_put.return_value.status_code = status + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + athenad.upload_queue.put_nowait(item) + try: + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0) + finally: + end_event.set() + + if retry: + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + + def test_upload_handler_timeout(self): + """When an upload times out or fails to connect it should be placed back in the queue""" + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + item_no_retry = item._replace(retry_count=MAX_RETRY_COUNT) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + try: + athenad.upload_queue.put_nowait(item_no_retry) + self.wait_for_upload() + time.sleep(0.1) + + # Check that upload with retry count exceeded is not put back + self.assertEqual(athenad.upload_queue.qsize(), 0) + + athenad.upload_queue.put_nowait(item) + self.wait_for_upload() + time.sleep(0.1) + + # Check that upload item was put back in the queue with incremented retry count + self.assertEqual(athenad.upload_queue.qsize(), 1) + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + + finally: + end_event.set() + + def test_cancelUpload(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + dispatcher["cancelUpload"](item.id) + + self.assertIn(item.id, athenad.cancelled_uploads) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 0) + self.assertEqual(len(athenad.cancelled_uploads), 0) + finally: + end_event.set() + + def test_cancelExpiry(self): + t_future = datetime.now() - timedelta(days=40) + ts = int(t_future.strftime("%s")) * 1000 + + # Item that would time out if actually uploaded + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=ts, id='', allow_cellular=True) + + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + athenad.upload_queue.put_nowait(item) + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 0) + finally: + end_event.set() + + def test_listUploadQueueEmpty(self): + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 0) + + @with_http_server + def test_listUploadQueueCurrent(self, host): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + try: + athenad.upload_queue.put_nowait(item) + self.wait_for_upload() + + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 1) + self.assertTrue(items[0]['current']) + + finally: + end_event.set() + + def test_listUploadQueue(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 1) + self.assertDictEqual(items[0], item._asdict()) + self.assertFalse(items[0]['current']) + + athenad.cancelled_uploads.add(item.id) + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 0) + + def test_upload_queue_persistence(self): + item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1') + item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2') + + athenad.upload_queue.put_nowait(item1) + athenad.upload_queue.put_nowait(item2) + + # Ensure cancelled items are not persisted + athenad.cancelled_uploads.add(item2.id) + + # serialize item + athenad.UploadQueueCache.cache(athenad.upload_queue) + + # deserialize item + athenad.upload_queue.queue.clear() + athenad.UploadQueueCache.initialize(athenad.upload_queue) + + self.assertEqual(athenad.upload_queue.qsize(), 1) + self.assertDictEqual(athenad.upload_queue.queue[-1]._asdict(), item1._asdict()) + + @mock.patch('selfdrive.athena.athenad.create_connection') + def test_startLocalProxy(self, mock_create_connection): + end_event = threading.Event() + + ws_recv = queue.Queue() + ws_send = queue.Queue() + mock_ws = MockWebsocket(ws_recv, ws_send) + mock_create_connection.return_value = mock_ws + + echo_socket = EchoSocket(self.SOCKET_PORT) + socket_thread = threading.Thread(target=echo_socket.run) + socket_thread.start() + + athenad.startLocalProxy(end_event, 'ws://localhost:1234', self.SOCKET_PORT) + + ws_recv.put_nowait(b'ping') + try: + recv = ws_send.get(timeout=5) + assert recv == (b'ping', ABNF.OPCODE_BINARY), recv + finally: + # signal websocket close to athenad.ws_proxy_recv + ws_recv.put_nowait(WebSocketConnectionClosedException()) + socket_thread.join() + + def test_getSshAuthorizedKeys(self): + keys = dispatcher["getSshAuthorizedKeys"]() + self.assertEqual(keys, MockParams().params["GithubSshKeys"].decode('utf-8')) + + def test_getVersion(self): + resp = dispatcher["getVersion"]() + keys = ["version", "remote", "branch", "commit"] + self.assertEqual(list(resp.keys()), keys) + for k in keys: + self.assertIsInstance(resp[k], str, f"{k} is not a string") + self.assertTrue(len(resp[k]) > 0, f"{k} has no value") + + def test_jsonrpc_handler(self): + end_event = threading.Event() + thread = threading.Thread(target=athenad.jsonrpc_handler, args=(end_event,)) + thread.daemon = True + thread.start() + try: + # with params + athenad.recv_queue.put_nowait(json.dumps({"method": "echo", "params": ["hello"], "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': 'hello', 'id': 0, 'jsonrpc': '2.0'}) + # without params + athenad.recv_queue.put_nowait(json.dumps({"method": "getNetworkType", "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': 1, 'id': 0, 'jsonrpc': '2.0'}) + # log forwarding + athenad.recv_queue.put_nowait(json.dumps({'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'})) + resp = athenad.log_recv_queue.get(timeout=3) + self.assertDictEqual(json.loads(resp), {'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'}) + finally: + end_event.set() + thread.join() + + def test_get_logs_to_send_sorted(self): + fl = list() + for i in range(10): + fn = os.path.join(swaglog.SWAGLOG_DIR, f'swaglog.{i:010}') + Path(fn).touch() + fl.append(os.path.basename(fn)) + + # ensure the list is all logs except most recent + sl = athenad.get_logs_to_send_sorted() + self.assertListEqual(sl, fl[:-1]) + +if __name__ == '__main__': + unittest.main() diff --git a/selfdrive/athena/tests/test_registration.py b/selfdrive/athena/tests/test_registration.py new file mode 100755 index 00000000000000..7a3847730572b8 --- /dev/null +++ b/selfdrive/athena/tests/test_registration.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +import json +import os +import tempfile +import unittest +from Crypto.PublicKey import RSA +from pathlib import Path +from unittest import mock + +from common.params import Params +from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID +from selfdrive.athena.tests.helpers import MockResponse + + +class TestRegistration(unittest.TestCase): + + def setUp(self): + # clear params and setup key paths + self.params = Params() + self.params.clear_all() + + self.persist = tempfile.TemporaryDirectory() + os.mkdir(os.path.join(self.persist.name, "comma")) + self.priv_key = Path(os.path.join(self.persist.name, "comma/id_rsa")) + self.pub_key = Path(os.path.join(self.persist.name, "comma/id_rsa.pub")) + self.persist_patcher = mock.patch("selfdrive.athena.registration.PERSIST", self.persist.name) + self.persist_patcher.start() + + def tearDown(self): + self.persist_patcher.stop() + self.persist.cleanup() + + def _generate_keys(self): + self.pub_key.touch() + k = RSA.generate(2048) + with open(self.priv_key, "wb") as f: + f.write(k.export_key()) + with open(self.pub_key, "wb") as f: + f.write(k.publickey().export_key()) + + def test_valid_cache(self): + # if all params are written, return the cached dongle id + self.params.put("IMEI", "imei") + self.params.put("HardwareSerial", "serial") + self._generate_keys() + + with mock.patch("selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = "DONGLE_ID_123" + self.params.put("DongleId", dongle) + self.assertEqual(register(), dongle) + self.assertFalse(m.called) + + def test_no_keys(self): + # missing pubkey + with mock.patch("selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = register() + self.assertEqual(m.call_count, 0) + self.assertEqual(dongle, UNREGISTERED_DONGLE_ID) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + def test_missing_cache(self): + # keys exist but no dongle id + self._generate_keys() + with mock.patch("selfdrive.athena.registration.api_get", autospec=True) as m: + dongle = "DONGLE_ID_123" + m.return_value = MockResponse(json.dumps({'dongle_id': dongle}), 200) + self.assertEqual(register(), dongle) + self.assertEqual(m.call_count, 1) + + # call again, shouldn't hit the API this time + self.assertEqual(register(), dongle) + self.assertEqual(m.call_count, 1) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + def test_unregistered(self): + # keys exist, but unregistered + self._generate_keys() + with mock.patch("selfdrive.athena.registration.api_get", autospec=True) as m: + m.return_value = MockResponse(None, 402) + dongle = register() + self.assertEqual(m.call_count, 1) + self.assertEqual(dongle, UNREGISTERED_DONGLE_ID) + self.assertEqual(self.params.get("DongleId", encoding='utf-8'), dongle) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore new file mode 100644 index 00000000000000..e8daa2ef273e5c --- /dev/null +++ b/selfdrive/boardd/.gitignore @@ -0,0 +1,3 @@ +boardd +boardd_api_impl.cpp +tests/test_boardd_usbprotocol diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript new file mode 100644 index 00000000000000..dcbea03d3c8e42 --- /dev/null +++ b/selfdrive/boardd/SConscript @@ -0,0 +1,9 @@ +Import('env', 'envCython', 'common', 'cereal', 'messaging') + +libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc'], LIBS=libs) +env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) + +envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) +if GetOption('test'): + env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc'], LIBS=libs) diff --git a/selfdrive/ui/layouts/__init__.py b/selfdrive/boardd/__init__.py similarity index 100% rename from selfdrive/ui/layouts/__init__.py rename to selfdrive/boardd/__init__.py diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc new file mode 100644 index 00000000000000..47bff1c5b67ade --- /dev/null +++ b/selfdrive/boardd/boardd.cc @@ -0,0 +1,583 @@ +#include "selfdrive/boardd/boardd.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cereal/gen/cpp/car.capnp.h" +#include "cereal/messaging/messaging.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "system/hardware/hw.h" + +// -- Multi-panda conventions -- +// Ordering: +// - The internal panda will always be the first panda +// - Consecutive pandas will be sorted based on panda type, and then serial number +// Connecting: +// - If a panda connection is dropped, boardd will reconnect to all pandas +// - If a panda is added, we will only reconnect when we are offroad +// CAN buses: +// - Each panda will have it's block of 4 buses. E.g.: the second panda will use +// bus numbers 4, 5, 6 and 7 +// - The internal panda will always be used for accessing the OBD2 port, +// and thus firmware queries +// Safety: +// - SafetyConfig is a list, which is mapped to the connected pandas +// - If there are more pandas connected than there are SafetyConfigs, +// the excess pandas will remain in "silent" or "noOutput" mode +// Ignition: +// - If any of the ignition sources in any panda is high, ignition is high + +#define MAX_IR_POWER 0.5f +#define MIN_IR_POWER 0.0f +#define CUTOFF_IL 200 +#define SATURATE_IL 1600 +#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') +using namespace std::chrono_literals; + +std::atomic ignition(false); +std::atomic pigeon_active(false); + +ExitHandler do_exit; + +static std::string get_time_str(const struct tm &time) { + char s[30] = {'\0'}; + std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time); + return s; +} + +bool check_all_connected(const std::vector &pandas) { + for (const auto& panda : pandas) { + if (!panda->connected) { + do_exit = true; + return false; + } + } + return true; +} + +enum class SyncTimeDir { TO_PANDA, FROM_PANDA }; + +void sync_time(Panda *panda, SyncTimeDir dir) { + if (!panda->has_rtc) return; + + setenv("TZ", "UTC", 1); + struct tm sys_time = util::get_time(); + struct tm rtc_time = panda->get_rtc(); + + if (dir == SyncTimeDir::TO_PANDA) { + if (util::time_valid(sys_time)) { + // Write time to RTC if it looks reasonable + double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); + if (std::abs(seconds) > 1.1) { + panda->set_rtc(sys_time); + LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s", + seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); + } + } + } else if (dir == SyncTimeDir::FROM_PANDA) { + if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) { + const struct timeval tv = {mktime(&rtc_time), 0}; + settimeofday(&tv, 0); + LOGE("System time wrong, setting from RTC. System: %s RTC: %s", + get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); + } + } +} + +bool safety_setter_thread(std::vector pandas) { + LOGD("Starting safety setter thread"); + + // there should be at least one panda connected + if (pandas.size() == 0) { + return false; + } + + // set to ELM327 for fingerprinting + pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327); + for (int i = 1; i < pandas.size(); i++) { + pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::SILENT); + } + + Params p = Params(); + + // wait for VIN to be read + while (true) { + if (do_exit || !check_all_connected(pandas) || !ignition) { + return false; + } + + std::string value_vin = p.get("CarVin"); + if (value_vin.size() > 0) { + // sanity check VIN format + assert(value_vin.size() == 17); + LOGW("got CarVin %s", value_vin.c_str()); + break; + } + util::sleep_for(20); + } + + // set to ELM327 for ECU knockouts + pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); + + std::string params; + LOGW("waiting for params to set safety model"); + while (true) { + if (do_exit || !check_all_connected(pandas) || !ignition) { + return false; + } + + if (p.getBool("ControlsReady")) { + params = p.get("CarParams"); + if (params.size() > 0) break; + } + util::sleep_for(100); + } + LOGW("got %d bytes CarParams", params.size()); + + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size())); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + cereal::CarParams::SafetyModel safety_model; + uint16_t safety_param; + + auto safety_configs = car_params.getSafetyConfigs(); + uint16_t alternative_experience = car_params.getAlternativeExperience(); + for (uint32_t i = 0; i < pandas.size(); i++) { + auto panda = pandas[i]; + + if (safety_configs.size() > i) { + safety_model = safety_configs[i].getSafetyModel(); + safety_param = safety_configs[i].getSafetyParam(); + } else { + // If no safety mode is specified, default to silent + safety_model = cereal::CarParams::SafetyModel::SILENT; + safety_param = 0U; + } + + LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); + panda->set_alternative_experience(alternative_experience); + panda->set_safety_model(safety_model, safety_param); + } + + return true; +} + +Panda *usb_connect(std::string serial="", uint32_t index=0) { + std::unique_ptr panda; + try { + panda = std::make_unique(serial, (index * PANDA_BUS_CNT)); + } catch (std::exception &e) { + return nullptr; + } + + // common panda config + if (getenv("BOARDD_LOOPBACK")) { + panda->set_loopback(true); + } + //panda->enable_deepsleep(); + + sync_time(panda.get(), SyncTimeDir::FROM_PANDA); + return panda.release(); +} + +void can_send_thread(std::vector pandas, bool fake_send) { + util::set_thread_name("boardd_can_send"); + + AlignedBuffer aligned_buf; + std::unique_ptr context(Context::create()); + std::unique_ptr subscriber(SubSocket::create(context.get(), "sendcan")); + assert(subscriber != NULL); + subscriber->setTimeout(100); + + // run as fast as messages come in + while (!do_exit && check_all_connected(pandas)) { + std::unique_ptr msg(subscriber->receive()); + if (!msg) { + if (errno == EINTR) { + do_exit = true; + } + continue; + } + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); + cereal::Event::Reader event = cmsg.getRoot(); + + //Dont send if older than 1 second + if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { + for (const auto& panda : pandas) { + LOGT("sending sendcan to panda: %s", (panda->usb_serial).c_str()); + panda->can_send(event.getSendcan()); + LOGT("sendcan sent to panda: %s", (panda->usb_serial).c_str()); + } + } + } +} + +void can_recv_thread(std::vector pandas) { + util::set_thread_name("boardd_can_recv"); + + // can = 8006 + PubMaster pm({"can"}); + + // run at 100hz + const uint64_t dt = 10000000ULL; + uint64_t next_frame_time = nanos_since_boot() + dt; + std::vector raw_can_data; + + while (!do_exit && check_all_connected(pandas)) { + bool comms_healthy = true; + raw_can_data.clear(); + for (const auto& panda : pandas) { + comms_healthy &= panda->can_receive(raw_can_data); + } + + MessageBuilder msg; + auto evt = msg.initEvent(); + evt.setValid(comms_healthy); + auto canData = evt.initCan(raw_can_data.size()); + for (uint i = 0; i 0) { + std::this_thread::sleep_for(std::chrono::nanoseconds(remaining)); + } else { + if (ignition) { + LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining); + } + next_frame_time = cur_time; + } + + next_frame_time += dt; + } +} + +void send_empty_peripheral_state(PubMaster *pm) { + MessageBuilder msg; + auto peripheralState = msg.initEvent().initPeripheralState(); + peripheralState.setPandaType(cereal::PandaState::PandaType::UNKNOWN); + pm->send("peripheralState", msg); +} + +void send_empty_panda_state(PubMaster *pm) { + MessageBuilder msg; + auto pandaStates = msg.initEvent().initPandaStates(1); + pandaStates[0].setPandaType(cereal::PandaState::PandaType::UNKNOWN); + pm->send("pandaStates", msg); +} + +std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { + bool ignition_local = false; + + // build msg + MessageBuilder msg; + auto evt = msg.initEvent(); + auto pss = evt.initPandaStates(pandas.size()); + + std::vector pandaStates; + for (const auto& panda : pandas){ + auto health_opt = panda->get_state(); + if (!health_opt) { + return std::nullopt; + } + + health_t health = *health_opt; + + if (spoofing_started) { + health.ignition_line_pkt = 1; + } + + ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); + + pandaStates.push_back(health); + } + + for (uint32_t i = 0; i < pandas.size(); i++) { + auto panda = pandas[i]; + const auto &health = pandaStates[i]; + + // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node + if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } + + #ifndef __x86_64__ + bool power_save_desired = !ignition_local && !pigeon_active; + if (health.power_save_enabled_pkt != power_save_desired) { + panda->set_power_saving(power_save_desired); + } + + // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect + if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } + #endif + + if (!panda->comms_healthy) { + evt.setValid(false); + } + + auto ps = pss[i]; + ps.setUptime(health.uptime_pkt); + ps.setBlockedCnt(health.blocked_msg_cnt_pkt); + ps.setIgnitionLine(health.ignition_line_pkt); + ps.setIgnitionCan(health.ignition_can_pkt); + ps.setControlsAllowed(health.controls_allowed_pkt); + ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); + ps.setCanRxErrs(health.can_rx_errs_pkt); + ps.setCanSendErrs(health.can_send_errs_pkt); + ps.setCanFwdErrs(health.can_fwd_errs_pkt); + ps.setGmlanSendErrs(health.gmlan_send_errs_pkt); + ps.setPandaType(panda->hw_type); + ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); + ps.setSafetyParam(health.safety_param_pkt); + ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); + ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); + ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); + ps.setAlternativeExperience(health.alternative_experience_pkt); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); + ps.setInterruptLoad(health.interrupt_load); + ps.setFanPower(health.fan_power); + + // Convert faults bitset to capnp list + std::bitset fault_bits(health.faults_pkt); + auto faults = ps.initFaults(fault_bits.count()); + + size_t j = 0; + for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); + f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) { + if (fault_bits.test(f)) { + faults.set(j, cereal::PandaState::FaultType(f)); + j++; + } + } + } + + pm->send("pandaStates", msg); + return ignition_local; +} + +void send_peripheral_state(PubMaster *pm, Panda *panda) { + // build msg + MessageBuilder msg; + auto evt = msg.initEvent(); + evt.setValid(panda->comms_healthy); + + auto ps = evt.initPeripheralState(); + ps.setPandaType(panda->hw_type); + + double read_time = millis_since_boot(); + ps.setVoltage(Hardware::get_voltage()); + ps.setCurrent(Hardware::get_current()); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); + } + + uint16_t fan_speed_rpm = panda->get_fan_speed(); + ps.setFanSpeedRpm(fan_speed_rpm); + + pm->send("peripheralState", msg); +} + +void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofing_started) { + util::set_thread_name("boardd_panda_state"); + + Params params; + SubMaster sm({"controlsState"}); + + Panda *peripheral_panda = pandas[0]; + bool ignition_last = false; + std::future safety_future; + + LOGD("start panda state thread"); + + // run at 2hz + while (!do_exit && check_all_connected(pandas)) { + uint64_t start_time = nanos_since_boot(); + + // send out peripheralState + send_peripheral_state(pm, peripheral_panda); + auto ignition_opt = send_panda_states(pm, pandas, spoofing_started); + + if (!ignition_opt) { + continue; + } + + ignition = *ignition_opt; + + // TODO: make this check fast, currently takes 16ms + // check if we have new pandas and are offroad + if (!ignition && (pandas.size() != Panda::list().size())) { + LOGW("Reconnecting to changed amount of pandas!"); + do_exit = true; + break; + } + + // clear ignition-based params and set new safety on car start + if (ignition && !ignition_last) { + params.clearAll(CLEAR_ON_IGNITION_ON); + if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) { + safety_future = std::async(std::launch::async, safety_setter_thread, pandas); + } else { + LOGW("Safety setter thread already running"); + } + } else if (!ignition && ignition_last) { + params.clearAll(CLEAR_ON_IGNITION_OFF); + } + + ignition_last = ignition; + + sm.update(0); + const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled(); + + for (const auto &panda : pandas) { + panda->send_heartbeat(engaged); + } + + uint64_t dt = nanos_since_boot() - start_time; + util::sleep_for(500 - dt / 1000000ULL); + } +} + + +void peripheral_control_thread(Panda *panda) { + util::set_thread_name("boardd_peripheral_control"); + + SubMaster sm({"deviceState", "driverCameraState"}); + + uint64_t last_front_frame_t = 0; + uint16_t prev_fan_speed = 999; + uint16_t ir_pwr = 0; + uint16_t prev_ir_pwr = 999; + unsigned int cnt = 0; + + FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); + + while (!do_exit && panda->connected) { + cnt++; + sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? + + // Other pandas don't have fan/IR to control + if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue; + + if (sm.updated("deviceState")) { + // Fan speed + uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); + if (fan_speed != prev_fan_speed || cnt % 100 == 0) { + panda->set_fan_speed(fan_speed); + prev_fan_speed = fan_speed; + } + } + if (sm.updated("driverCameraState")) { + auto event = sm["driverCameraState"]; + int cur_integ_lines = event.getDriverCameraState().getIntegLines(); + float cur_gain = event.getDriverCameraState().getGain(); + + cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain); + last_front_frame_t = event.getLogMonoTime(); + + if (cur_integ_lines <= CUTOFF_IL) { + ir_pwr = 100.0 * MIN_IR_POWER; + } else if (cur_integ_lines > SATURATE_IL) { + ir_pwr = 100.0 * MAX_IR_POWER; + } else { + ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_integ_lines - CUTOFF_IL) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_IL - CUTOFF_IL))); + } + } + + // Disable ir_pwr on front frame timeout + uint64_t cur_t = nanos_since_boot(); + if (cur_t - last_front_frame_t > 1e9) { + ir_pwr = 0; + } + + if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0) { + panda->set_ir_pwr(ir_pwr); + prev_ir_pwr = ir_pwr; + } + + // Write to rtc once per minute when no ignition present + if (!ignition && (cnt % 120 == 1)) { + sync_time(panda, SyncTimeDir::TO_PANDA); + } + } +} + +void boardd_main_thread(std::vector serials) { + PubMaster pm({"pandaStates", "peripheralState"}); + LOGW("attempting to connect"); + + if (serials.size() == 0) { + // connect to all + serials = Panda::list(); + + // exit if no pandas are connected + if (serials.size() == 0) { + LOGW("no pandas found, exiting"); + return; + } + } + + // connect to all provided serials + std::vector pandas; + for (int i = 0; i < serials.size() && !do_exit; /**/) { + Panda *p = usb_connect(serials[i], i); + if (!p) { + // send empty pandaState & peripheralState and try again + send_empty_panda_state(&pm); + send_empty_peripheral_state(&pm); + util::sleep_for(500); + continue; + } + + pandas.push_back(p); + ++i; + } + + if (!do_exit) { + LOGW("connected to board"); + Panda *peripheral_panda = pandas[0]; + std::vector threads; + + threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); + threads.emplace_back(peripheral_control_thread, peripheral_panda); + + threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); + threads.emplace_back(can_recv_thread, pandas); + + for (auto &t : threads) t.join(); + } + + // we have exited, clean up pandas + for (Panda *panda : pandas) { + delete panda; + } +} diff --git a/selfdrive/boardd/boardd.h b/selfdrive/boardd/boardd.h new file mode 100644 index 00000000000000..d3c9e1f94a8e2e --- /dev/null +++ b/selfdrive/boardd/boardd.h @@ -0,0 +1,6 @@ +#pragma once + +#include "selfdrive/boardd/panda.h" + +bool safety_setter_thread(std::vector pandas); +void boardd_main_thread(std::vector serials); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py new file mode 100644 index 00000000000000..527f1f4f52dc7d --- /dev/null +++ b/selfdrive/boardd/boardd.py @@ -0,0 +1,12 @@ +# pylint: skip-file + +# Cython, now uses scons to build +from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp +assert can_list_to_can_capnp + +def can_capnp_to_can_list(can, src_filter=None): + ret = [] + for msg in can: + if src_filter is None or msg.src in src_filter: + ret.append((msg.address, msg.busTime, msg.dat, msg.src)) + return ret diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx new file mode 100644 index 00000000000000..0d428a9259c7af --- /dev/null +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -0,0 +1,28 @@ +# distutils: language = c++ +# cython: language_level=3 +from libcpp.vector cimport vector +from libcpp.string cimport string +from libcpp cimport bool + +cdef struct can_frame: + long address + string dat + long busTime + long src + +cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid) + +def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): + cdef vector[can_frame] can_list + can_list.reserve(len(can_msgs)) + + cdef can_frame f + for can_msg in can_msgs: + f.address = can_msg[0] + f.busTime = can_msg[1] + f.dat = can_msg[2] + f.src = can_msg[3] + can_list.push_back(f) + cdef string out + can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid) + return out diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc new file mode 100644 index 00000000000000..faa0e37373546e --- /dev/null +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -0,0 +1,25 @@ +#include "cereal/messaging/messaging.h" +#include "panda.h" + +extern "C" { + +void can_list_to_can_capnp_cpp(const std::vector &can_list, std::string &out, bool sendCan, bool valid) { + MessageBuilder msg; + auto event = msg.initEvent(valid); + + auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size()); + int j = 0; + for (auto it = can_list.begin(); it != can_list.end(); it++, j++) { + auto c = canData[j]; + c.setAddress(it->address); + c.setBusTime(it->busTime); + c.setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size())); + c.setSrc(it->src); + } + const uint64_t msg_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word); + out.resize(msg_size); + kj::ArrayOutputStream output_stream(kj::ArrayPtr((unsigned char *)out.data(), msg_size)); + capnp::writeMessage(output_stream, msg); +} + +} diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc new file mode 100644 index 00000000000000..cb17a584bdb580 --- /dev/null +++ b/selfdrive/boardd/main.cc @@ -0,0 +1,22 @@ +#include + +#include "selfdrive/boardd/boardd.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "system/hardware/hw.h" + +int main(int argc, char *argv[]) { + LOGW("starting boardd"); + + if (!Hardware::PC()) { + int err; + err = util::set_realtime_priority(54); + assert(err == 0); + err = util::set_core_affinity({4}); + assert(err == 0); + } + + std::vector serials(argv + 1, argv + argc); + boardd_main_thread(serials); + return 0; +} diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc new file mode 100644 index 00000000000000..685dabd873df48 --- /dev/null +++ b/selfdrive/boardd/panda.cc @@ -0,0 +1,462 @@ +#include "selfdrive/boardd/panda.h" + +#include + +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "panda/board/dlc_to_len.h" +#include "common/gpio.h" +#include "common/swaglog.h" +#include "common/util.h" + +static int init_usb_ctx(libusb_context **context) { + assert(context != nullptr); + + int err = libusb_init(context); + if (err != 0) { + LOGE("libusb initialization error"); + return err; + } + +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(*context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else + libusb_set_debug(*context, 3); +#endif + + return err; +} + + +Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { + // init libusb + ssize_t num_devices; + libusb_device **dev_list = NULL; + int err = init_usb_ctx(&ctx); + if (err != 0) { goto fail; } + + // connect by serial + num_devices = libusb_get_device_list(ctx, &dev_list); + if (num_devices < 0) { goto fail; } + for (size_t i = 0; i < num_devices; ++i) { + libusb_device_descriptor desc; + libusb_get_device_descriptor(dev_list[i], &desc); + if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { + int ret = libusb_open(dev_list[i], &dev_handle); + if (dev_handle == NULL || ret < 0) { goto fail; } + + unsigned char desc_serial[26] = { 0 }; + ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + if (ret < 0) { goto fail; } + + usb_serial = std::string((char *)desc_serial, ret).c_str(); + if (serial.empty() || serial == usb_serial) { + break; + } + libusb_close(dev_handle); + dev_handle = NULL; + } + } + if (dev_handle == NULL) goto fail; + libusb_free_device_list(dev_list, 1); + dev_list = nullptr; + + if (libusb_kernel_driver_active(dev_handle, 0) == 1) { + libusb_detach_kernel_driver(dev_handle, 0); + } + + err = libusb_set_configuration(dev_handle, 1); + if (err != 0) { goto fail; } + + err = libusb_claim_interface(dev_handle, 0); + if (err != 0) { goto fail; } + + hw_type = get_hw_type(); + + assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) && + (hw_type != cereal::PandaState::PandaType::GREY_PANDA)); + + has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || + (hw_type == cereal::PandaState::PandaType::DOS); + + return; + +fail: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + cleanup(); + throw std::runtime_error("Error connecting to panda"); +} + +Panda::~Panda() { + std::lock_guard lk(usb_lock); + cleanup(); + connected = false; +} + +void Panda::cleanup() { + if (dev_handle) { + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + } + + if (ctx) { + libusb_exit(ctx); + } +} + +std::vector Panda::list() { + // init libusb + ssize_t num_devices; + libusb_context *context = NULL; + libusb_device **dev_list = NULL; + std::vector serials; + + int err = init_usb_ctx(&context); + if (err != 0) { return serials; } + + num_devices = libusb_get_device_list(context, &dev_list); + if (num_devices < 0) { + LOGE("libusb can't get device list"); + goto finish; + } + for (size_t i = 0; i < num_devices; ++i) { + libusb_device *device = dev_list[i]; + libusb_device_descriptor desc; + libusb_get_device_descriptor(device, &desc); + if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { + libusb_device_handle *handle = NULL; + int ret = libusb_open(device, &handle); + if (ret < 0) { goto finish; } + + unsigned char desc_serial[26] = { 0 }; + ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + libusb_close(handle); + if (ret < 0) { goto finish; } + + serials.push_back(std::string((char *)desc_serial, ret).c_str()); + } + } + +finish: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + if (context) { + libusb_exit(context); + } + return serials; +} + +void Panda::handle_usb_issue(int err, const char func[]) { + LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); + if (err == LIBUSB_ERROR_NO_DEVICE) { + LOGE("lost connection"); + connected = false; + } + // TODO: check other errors, is simply retrying okay? +} + +int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(usb_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + + return err; +} + +int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(usb_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + + return err; +} + +int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(usb_lock); + do { + // Try sending can messages. If the receive buffer on the panda is full it will NAK + // and libusb will try again. After 5ms, it will time out. We will drop the messages. + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + LOGW("Transmit buffer full"); + break; + } else if (err != 0 || length != transferred) { + handle_usb_issue(err, __func__); + } + } while(err != 0 && connected); + + return transferred; +} + +int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(usb_lock); + + do { + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + break; // timeout is okay to exit, recv still happened + } else if (err == LIBUSB_ERROR_OVERFLOW) { + comms_healthy = false; + LOGE_100("overflow got 0x%x", transferred); + } else if (err != 0) { + handle_usb_issue(err, __func__); + } + + } while(err != 0 && connected); + + return transferred; +} + +void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { + usb_write(0xdc, (uint16_t)safety_model, safety_param); +} + +void Panda::set_alternative_experience(uint16_t alternative_experience) { + usb_write(0xdf, alternative_experience, 0); +} + +cereal::PandaState::PandaType Panda::get_hw_type() { + unsigned char hw_query[1] = {0}; + + usb_read(0xc1, 0, 0, hw_query, 1); + return (cereal::PandaState::PandaType)(hw_query[0]); +} + +void Panda::set_rtc(struct tm sys_time) { + // tm struct has year defined as years since 1900 + usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); + usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); + usb_write(0xa3, (uint16_t)sys_time.tm_mday, 0); + // usb_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); + usb_write(0xa5, (uint16_t)sys_time.tm_hour, 0); + usb_write(0xa6, (uint16_t)sys_time.tm_min, 0); + usb_write(0xa7, (uint16_t)sys_time.tm_sec, 0); +} + +struct tm Panda::get_rtc() { + struct __attribute__((packed)) timestamp_t { + uint16_t year; // Starts at 0 + uint8_t month; + uint8_t day; + uint8_t weekday; + uint8_t hour; + uint8_t minute; + uint8_t second; + } rtc_time = {0}; + + usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); + + struct tm new_time = { 0 }; + new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 1900 + new_time.tm_mon = rtc_time.month - 1; + new_time.tm_mday = rtc_time.day; + new_time.tm_hour = rtc_time.hour; + new_time.tm_min = rtc_time.minute; + new_time.tm_sec = rtc_time.second; + + return new_time; +} + +void Panda::set_fan_speed(uint16_t fan_speed) { + usb_write(0xb1, fan_speed, 0); +} + +uint16_t Panda::get_fan_speed() { + uint16_t fan_speed_rpm = 0; + usb_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); + return fan_speed_rpm; +} + +void Panda::set_ir_pwr(uint16_t ir_pwr) { + usb_write(0xb0, ir_pwr, 0); +} + +std::optional Panda::get_state() { + health_t health {0}; + int err = usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + return err >= 0 ? std::make_optional(health) : std::nullopt; +} + +void Panda::set_loopback(bool loopback) { + usb_write(0xe5, loopback, 0); +} + +std::optional> Panda::get_firmware_version() { + std::vector fw_sig_buf(128); + int read_1 = usb_read(0xd3, 0, 0, &fw_sig_buf[0], 64); + int read_2 = usb_read(0xd4, 0, 0, &fw_sig_buf[64], 64); + return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt; +} + +std::optional Panda::get_serial() { + char serial_buf[17] = {'\0'}; + int err = usb_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); + return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; +} + +void Panda::set_power_saving(bool power_saving) { + usb_write(0xe7, power_saving, 0); +} + +void Panda::enable_deepsleep() { + usb_write(0xfb, 0, 0); +} + +void Panda::send_heartbeat(bool engaged) { + usb_write(0xf3, engaged, 0); +} + +void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { + usb_write(0xde, bus, (speed * 10)); +} + +void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { + usb_write(0xf9, bus, (speed * 10)); +} + +static uint8_t len_to_dlc(uint8_t len) { + if (len <= 8) { + return len; + } + if (len <= 24) { + return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0); + } else { + return 11 + (len / 16) + ((len % 16) ? 1 : 0); + } +} + +static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) { + for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) { + // Insert counter every 64 bytes (first byte of 64 bytes USB packet) + if (pos % USBPACKET_MAX_SIZE == 0) { + dest[pos] = pos / USBPACKET_MAX_SIZE; + pos++; + } + dest[pos] = src[i]; + } +} + +void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, + std::function write_func) { + int32_t pos = 0; + uint8_t send_buf[2 * USB_TX_SOFT_LIMIT]; + + for (auto cmsg : can_data_list) { + // check if the message is intended for this panda + uint8_t bus = cmsg.getSrc(); + if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) { + continue; + } + auto can_data = cmsg.getDat(); + uint8_t data_len_code = len_to_dlc(can_data.size()); + assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8)); + assert(can_data.size() == dlc_to_len[data_len_code]); + + can_header header; + header.addr = cmsg.getAddress(); + header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; + header.data_len_code = data_len_code; + header.bus = bus - bus_offset; + + write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header)); + write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.size()); + if (pos >= USB_TX_SOFT_LIMIT) { + write_func(send_buf, pos); + pos = 0; + } + } + + // send remaining packets + if (pos > 0) write_func(send_buf, pos); +} + +void Panda::can_send(capnp::List::Reader can_data_list) { + pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) { + usb_bulk_write(3, data, size, 5); + }); +} + +bool Panda::can_receive(std::vector& out_vec) { + uint8_t data[RECV_SIZE]; + int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE); + if (!comms_healthy) { + return false; + } + if (recv == RECV_SIZE) { + LOGW("Panda receive buffer full"); + } + + return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec); +} + +bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) { + recv_buf.clear(); + for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { + if (data[i] != i / USBPACKET_MAX_SIZE) { + LOGE("CAN: MALFORMED USB RECV PACKET"); + comms_healthy = false; + return false; + } + int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); + recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]); + } + + int pos = 0; + while (pos < recv_buf.size()) { + can_header header; + memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE); + + can_frame &canData = out_vec.emplace_back(); + canData.busTime = 0; + canData.address = header.addr; + canData.src = header.bus + bus_offset; + if (header.rejected) { canData.src += CANPACKET_REJECTED; } + if (header.returned) { canData.src += CANPACKET_RETURNED; } + + const uint8_t data_len = dlc_to_len[header.data_len_code]; + canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len); + + pos += CANPACKET_HEAD_SIZE + data_len; + } + return true; +} diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h new file mode 100644 index 00000000000000..1cefc3cb4d9f29 --- /dev/null +++ b/selfdrive/boardd/panda.h @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cereal/gen/cpp/car.capnp.h" +#include "cereal/gen/cpp/log.capnp.h" +#include "panda/board/health.h" + +#define TIMEOUT 0 +#define PANDA_BUS_CNT 4 +#define RECV_SIZE (0x4000U) +#define USB_TX_SOFT_LIMIT (0x100U) +#define USBPACKET_MAX_SIZE (0x40) +#define CANPACKET_HEAD_SIZE 5U +#define CANPACKET_MAX_SIZE 72U +#define CANPACKET_REJECTED (0xC0U) +#define CANPACKET_RETURNED (0x80U) + +struct __attribute__((packed)) can_header { + uint8_t reserved : 1; + uint8_t bus : 3; + uint8_t data_len_code : 4; + uint8_t rejected : 1; + uint8_t returned : 1; + uint8_t extended : 1; + uint32_t addr : 29; +}; + +struct can_frame { + long address; + std::string dat; + long busTime; + long src; +}; + +class Panda { + private: + libusb_context *ctx = NULL; + libusb_device_handle *dev_handle = NULL; + std::mutex usb_lock; + std::vector recv_buf; + void handle_usb_issue(int err, const char func[]); + void cleanup(); + + public: + Panda(std::string serial="", uint32_t bus_offset=0); + ~Panda(); + + std::string usb_serial; + std::atomic connected = true; + std::atomic comms_healthy = true; + cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; + bool has_rtc = false; + const uint32_t bus_offset; + + // Static functions + static std::vector list(); + + // HW communication + int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT); + int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT); + int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + + // Panda functionality + cereal::PandaState::PandaType get_hw_type(); + void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); + void set_alternative_experience(uint16_t alternative_experience); + void set_rtc(struct tm sys_time); + struct tm get_rtc(); + void set_fan_speed(uint16_t fan_speed); + uint16_t get_fan_speed(); + void set_ir_pwr(uint16_t ir_pwr); + std::optional get_state(); + void set_loopback(bool loopback); + std::optional> get_firmware_version(); + std::optional get_serial(); + void set_power_saving(bool power_saving); + void enable_deepsleep(); + void send_heartbeat(bool engaged); + void set_can_speed_kbps(uint16_t bus, uint16_t speed); + void set_data_speed_kbps(uint16_t bus, uint16_t speed); + void can_send(capnp::List::Reader can_data_list); + bool can_receive(std::vector& out_vec); + +protected: + // for unit tests + Panda(uint32_t bus_offset) : bus_offset(bus_offset) {} + void pack_can_buffer(const capnp::List::Reader &can_data_list, + std::function write_func); + bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec); +}; diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py new file mode 100755 index 00000000000000..971756002bc49d --- /dev/null +++ b/selfdrive/boardd/pandad.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +# simple boardd wrapper that updates the panda first +import os +import usb1 +import time +import subprocess +from typing import List, NoReturn +from functools import cmp_to_key + +from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU +from common.basedir import BASEDIR +from common.params import Params +from system.hardware import HARDWARE +from system.swaglog import cloudlog + + +def get_expected_signature(panda: Panda) -> bytes: + fn = DEFAULT_H7_FW_FN if (panda.get_mcu_type() == MCU_TYPE_H7) else DEFAULT_FW_FN + + try: + return Panda.get_signature_from_firmware(fn) + except Exception: + cloudlog.exception("Error computing expected signature") + return b"" + + +def flash_panda(panda_serial: str) -> Panda: + panda = Panda(panda_serial) + + fw_signature = get_expected_signature(panda) + internal_panda = panda.is_internal() and not panda.bootstub + + panda_version = "bootstub" if panda.bootstub else panda.get_version() + 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]}") + + if panda.bootstub or panda_signature != fw_signature: + cloudlog.info("Panda firmware out of date, update required") + panda.flash() + cloudlog.info("Done flashing") + + if panda.bootstub: + bootstub_version = panda.get_version() + cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}") + if internal_panda: + HARDWARE.recover_internal_panda() + panda.recover(reset=(not internal_panda)) + cloudlog.info("Done flashing bootloader") + + if panda.bootstub: + cloudlog.info("Panda still not booting, exiting") + raise AssertionError + + panda_signature = panda.get_signature() + if panda_signature != fw_signature: + cloudlog.info("Version mismatch after flashing, exiting") + raise AssertionError + + return panda + + +def panda_sort_cmp(a: Panda, b: Panda): + a_type = a.get_type() + b_type = b.get_type() + + # make sure the internal one is always first + if a.is_internal() and not b.is_internal(): + return -1 + if not a.is_internal() and b.is_internal(): + return 1 + + # sort by hardware type + if a_type != b_type: + return a_type < b_type + + # last resort: sort by serial number + return a.get_usb_serial() < b.get_usb_serial() + + +def main() -> NoReturn: + first_run = True + params = Params() + + while True: + try: + params.remove("PandaSignatures") + + # Flash all Pandas in DFU mode + for p in PandaDFU.list(): + cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}") + PandaDFU(p).recover() + time.sleep(1) + + panda_serials = Panda.list() + if len(panda_serials) == 0: + if first_run: + cloudlog.info("Resetting internal panda") + HARDWARE.reset_internal_panda() + time.sleep(2) # wait to come back up + continue + + cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}") + + # Flash pandas + pandas: List[Panda] = [] + for serial in panda_serials: + pandas.append(flash_panda(serial)) + + # check health for lost heartbeat + for panda in pandas: + health = panda.health() + if health["heartbeat_lost"]: + params.put_bool("PandaHeartbeatLost", True) + cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) + + if first_run: + cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") + panda.reset() + + # sort pandas to have deterministic order + pandas.sort(key=cmp_to_key(panda_sort_cmp)) + panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) # type: ignore + + # log panda fw versions + params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) + + # close all pandas + for p in pandas: + p.close() + except (usb1.USBErrorNoDevice, usb1.USBErrorPipe): + # a panda was disconnected while setting everything up. let's try again + cloudlog.exception("Panda USB exception while setting up") + continue + + first_run = False + + # run boardd with all connected serials as arguments + os.environ['MANAGER_DAEMON'] = 'boardd' + os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) + subprocess.run(["./boardd", *panda_serials], check=True) + +if __name__ == "__main__": + main() diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py new file mode 100755 index 00000000000000..7d17be4de90d74 --- /dev/null +++ b/selfdrive/boardd/set_time.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import datetime +import os +import struct +import usb1 + +REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE +MIN_DATE = datetime.datetime(year=2021, month=4, day=1) + +def set_time(logger): + sys_time = datetime.datetime.today() + if sys_time > MIN_DATE: + logger.info("System time valid") + return + + try: + ctx = usb1.USBContext() + dev = ctx.openByVendorIDAndProductID(0xbbaa, 0xddcc) + if dev is None: + logger.info("No panda found") + return + + # Set system time from panda RTC time + dat = dev.controlRead(REQUEST_IN, 0xa0, 0, 0, 8) + a = struct.unpack("HBBBBBB", dat) + panda_time = datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6]) + if panda_time > MIN_DATE: + logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") + os.system(f"TZ=UTC date -s '{panda_time}'") + except Exception: + logger.warn("Failed to fetch time from panda") + +if __name__ == "__main__": + import logging + logging.basicConfig(level=logging.INFO) + + set_time(logging) diff --git a/selfdrive/ui/mici/layouts/__init__.py b/selfdrive/boardd/tests/__init__.py similarity index 100% rename from selfdrive/ui/mici/layouts/__init__.py rename to selfdrive/boardd/tests/__init__.py diff --git a/selfdrive/boardd/tests/test_boardd b/selfdrive/boardd/tests/test_boardd new file mode 100755 index 00000000000000..b4455ce67c3a6e Binary files /dev/null and b/selfdrive/boardd/tests/test_boardd differ diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py new file mode 100755 index 00000000000000..e9bbcb4586b45b --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import os +import random +import time +import unittest +from collections import defaultdict + +import cereal.messaging as messaging +from cereal import car +from common.params import Params +from common.spinner import Spinner +from common.timeout import Timeout +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car import make_can_msg +from system.hardware import TICI +from selfdrive.test.helpers import phone_only, with_processes + + +class TestBoardd(unittest.TestCase): + + @classmethod + def setUpClass(cls): + os.environ['STARTED'] = '1' + os.environ['BOARDD_LOOPBACK'] = '1' + cls.spinner = Spinner() + + @classmethod + def tearDownClass(cls): + cls.spinner.close() + + @phone_only + @with_processes(['pandad']) + def test_loopback(self): + # wait for boardd to init + time.sleep(2) + + with Timeout(60, "boardd didn't start"): + sm = messaging.SubMaster(['pandaStates']) + while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0: + sm.update(1000) + + num_pandas = len(sm['pandaStates']) + if TICI: + self.assertGreater(num_pandas, 1, "connect another panda for multipanda tests") + + # boardd blocks on CarVin and CarParams + cp = car.CarParams.new_message() + + safety_config = car.CarParams.SafetyConfig.new_message() + safety_config.safetyModel = car.CarParams.SafetyModel.allOutput + cp.safetyConfigs = [safety_config]*num_pandas + + params = Params() + params.put("CarVin", b"0"*17) + params.put_bool("ControlsReady", True) + params.put("CarParams", cp.to_bytes()) + + sendcan = messaging.pub_sock('sendcan') + can = messaging.sub_sock('can', conflate=False, timeout=100) + time.sleep(0.2) + + n = 200 + for i in range(n): + self.spinner.update(f"boardd loopback {i}/{n}") + + sent_msgs = defaultdict(set) + for _ in range(random.randrange(10)): + to_send = [] + for __ in range(random.randrange(100)): + bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) + addr = random.randrange(1, 1<<29) + dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) + sent_msgs[bus].add((addr, dat)) + to_send.append(make_can_msg(addr, dat, bus)) + sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) + + for _ in range(100 * 2): + recvd = messaging.drain_sock(can, wait_for_one=True) + for msg in recvd: + for m in msg.can: + if m.src >= 128: + key = (m.address, m.dat) + assert key in sent_msgs[m.src-128], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}" + sent_msgs[m.src-128].discard(key) + + if all(len(v) == 0 for v in sent_msgs.values()): + break + + # if a set isn't empty, messages got dropped + for bus in sent_msgs.keys(): + assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages" + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc new file mode 100644 index 00000000000000..6a13cbd71ff0c5 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -0,0 +1,130 @@ +#define CATCH_CONFIG_MAIN +#define CATCH_CONFIG_ENABLE_BENCHMARKING +#include + +#include "catch2/catch.hpp" +#include "cereal/messaging/messaging.h" +#include "selfdrive/boardd/panda.h" + +const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U}; + +int random_int(int min, int max) { + std::random_device dev; + std::mt19937 rng(dev()); + std::uniform_int_distribution dist(min, max); + return dist(rng); +} + +struct PandaTest : public Panda { + PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type); + void test_can_send(); + void test_can_recv(); + + std::map test_data; + int can_list_size = 0; + int total_pakets_size = 0; + MessageBuilder msg; + capnp::List::Reader can_data_list; +}; + +PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) { + this->hw_type = hw_type; + int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8); + // prepare test data + for (int i = 0; i < data_limit; ++i) { + std::random_device rd; + std::independent_bits_engine rbe(rd()); + + int data_len = dlc_to_len[i]; + std::string bytes(data_len, '\0'); + std::generate(bytes.begin(), bytes.end(), std::ref(rbe)); + test_data[data_len] = bytes; + } + + // generate can messages for this panda + auto can_list = msg.initEvent().initSendcan(can_list_size); + for (uint8_t i = 0; i < can_list_size; ++i) { + auto can = can_list[i]; + uint32_t id = random_int(0, std::size(dlc_to_len) - 1); + const std::string &dat = test_data[dlc_to_len[id]]; + can.setAddress(i); + can.setSrc(random_int(0, 3) + bus_offset); + can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); + total_pakets_size += CANPACKET_HEAD_SIZE + dat.size(); + } + + can_data_list = can_list.asReader(); + INFO("test " << can_list_size << " packets, total size " << total_pakets_size); +} + +void PandaTest::test_can_send() { + std::vector unpacked_data; + this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { + int size_left = size; + for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) { + REQUIRE(chunk[i] == counter); + + const int len = std::min(USBPACKET_MAX_SIZE, size_left); + unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]); + size_left -= len; + } + }); + REQUIRE(unpacked_data.size() == total_pakets_size); + + int cnt = 0; + INFO("test can message integrity"); + for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) { + can_header header; + memcpy(&header, &unpacked_data[pos], CANPACKET_HEAD_SIZE); + const uint8_t data_len = dlc_to_len[header.data_len_code]; + pckt_len = CANPACKET_HEAD_SIZE + data_len; + + REQUIRE(header.addr == cnt); + REQUIRE(test_data.find(data_len) != test_data.end()); + const std::string &dat = test_data[data_len]; + REQUIRE(memcmp(dat.data(), &unpacked_data[pos + 5], dat.size()) == 0); + ++cnt; + } + REQUIRE(cnt == can_list_size); +} + +void PandaTest::test_can_recv() { + std::vector frames; + this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) { + this->unpack_can_buffer(data, size, frames); + }); + + REQUIRE(frames.size() == can_list_size); + for (int i = 0; i < frames.size(); ++i) { + REQUIRE(frames[i].address == i); + REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end()); + const std::string &dat = test_data[frames[i].dat.size()]; + REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0); + } +} + +TEST_CASE("send/recv CAN 2.0 packets") { + auto bus_offset = GENERATE(0, 4); + auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); + PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS); + + SECTION("can_send") { + test.test_can_send(); + } + SECTION("can_receive") { + test.test_can_recv(); + } +} + +TEST_CASE("send/recv CAN FD packets") { + auto bus_offset = GENERATE(0, 4); + auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); + PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA); + + SECTION("can_send") { + test.test_can_send(); + } + SECTION("can_receive") { + test.test_can_recv(); + } +} diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md index cd352b2edeb15e..2fce0f9036176e 100644 --- a/selfdrive/car/CARS_template.md +++ b/selfdrive/car/CARS_template.md @@ -1,29 +1,24 @@ -{% set footnote_tag = '[{}](#footnotes)' %} -{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' %} -{% set video_icon = '' %} -{# Force hardware column wider by using a blank image with max width. #} -{% set width_tag = '%s
     ' %} -{% set hardware_col_name = 'Hardware Needed' %} -{% set wide_hardware_col_name = width_tag|format(hardware_col_name) -%} +{% set footnote_tag = '[{}](#footnotes)' -%} +{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' -%} # Supported Cars -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. +A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. -# {{all_car_docs | selectattr('support_type', 'eq', SupportType.UPSTREAM) | list | length}} Supported Cars +# {{all_car_info | length}} Supported Cars -|{{Column | map(attribute='value') | join('|') | replace(hardware_col_name, wide_hardware_col_name)}}| +|{{Column | map(attribute='value') | join('|')}}| |---|---|---|{% for _ in range((Column | length) - 3) %}{{':---:|'}}{% endfor +%} -{% for car_docs in all_car_docs | selectattr('support_type', 'eq', SupportType.UPSTREAM) %} -|{% for column in Column %}{{car_docs.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %} +{% for car_info in all_car_info %} +|{% for column in Column %}{{car_info.get_column(column, star_icon, footnote_tag)}}|{% endfor %} {% endfor %} -### Footnotes + {% for footnote in footnotes %} -{{loop.index}}{{footnote | replace('
    ', '')}}
    +{{loop.index}}{{footnote}}
    {% endfor %} ## Community Maintained Cars @@ -42,8 +37,7 @@ If your car has the following packages or features, then it's a good candidate f | Make | Required Package/Features | | ---- | ------------------------- | -| Acura | Any car with AcuraWatch will work. AcuraWatch comes standard on many newer models. | -| Ford | Any car with Lane Centering will likely work. | +| Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | | Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | | Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | | Nissan | Any car with ProPILOT will likely work. | @@ -53,21 +47,19 @@ If your car has the following packages or features, then it's a good candidate f ### FlexRay -All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. +All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the cars in your computer can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. ### Toyota Security openpilot does not yet support these Toyota models due to a new message authentication method. -[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models. +[Vote](https://comma.ai/shop/products/vote) if you'd like to see openpilot support on these models. * Toyota RAV4 Prime 2021+ * Toyota Sienna 2021+ * Toyota Venza 2021+ * Toyota Sequoia 2023+ * Toyota Tundra 2022+ -* Toyota Highlander 2024+ * Toyota Corolla Cross 2022+ (only US model) -* Toyota Camry 2025+ * Lexus NX 2022+ * Toyota bZ4x 2023+ * Subaru Solterra 2023+ diff --git a/selfdrive/car/README.MD b/selfdrive/car/README.MD new file mode 100644 index 00000000000000..de4db0ee50915e --- /dev/null +++ b/selfdrive/car/README.MD @@ -0,0 +1,11 @@ +## Port structure +##### interface.py +Generic interface to send and receive messages from CAN (controlsd uses this to communicate with car) +##### carcontroller.py +Builds CAN messages to send to car +##### carstate.py +Reads CAN from car and builds openpilot CarState message +##### values.py +Fingerprints and absolute limits +##### radar_interface.py +Radar interface diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index e69de29bb2d1d6..f2d198338d81f4 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -0,0 +1,163 @@ +# functions common among cars +import capnp + +from cereal import car +from common.numpy_fast import clip +from typing import Dict, List + +# kg of standard extra cargo to count for drive, gas, etc... +STD_CARGO_KG = 136. + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName + + +def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], + unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder: + if cur_but != unpressed: + be = car.CarState.ButtonEvent(pressed=True) + but = cur_but + else: + be = car.CarState.ButtonEvent(pressed=False) + but = prev_but + be.type = buttons_dict.get(but, ButtonType.unknown) + return be + + +def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder, pcm_cruise: bool = False) -> List[int]: + events = [] + for b in buttonEvents: + # do enable on both accel and decel buttons + if not pcm_cruise: + if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: + events.append(EventName.buttonEnable) + # do disable on button down + if b.type == ButtonType.cancel and b.pressed: + events.append(EventName.buttonCancel) + return events + + +def gen_empty_fingerprint(): + return {i: {} for i in range(0, 8)} + + +# FIXME: hardcoding honda civic 2016 touring params so they can be used to +# scale unknown params for other cars +class CivicParams: + MASS = 1326. + STD_CARGO_KG + WHEELBASE = 2.70 + CENTER_TO_FRONT = WHEELBASE * 0.4 + CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT + ROTATIONAL_INERTIA = 2500 + TIRE_STIFFNESS_FRONT = 192150 + TIRE_STIFFNESS_REAR = 202500 + + +# TODO: get actual value, for now starting with reasonable value for +# civic and scaling by mass and wheelbase +def scale_rot_inertia(mass, wheelbase): + return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2) + + +# TODO: start from empirically derived lateral slip stiffness for the civic and scale by +# mass and CG position, so all cars will have approximately similar dyn behaviors +def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0): + center_to_rear = wheelbase - center_to_front + tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE) + + tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE) + + return tire_stiffness_front, tire_stiffness_rear + + +def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]: + return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} + + +def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): + + # limits due to driver torque + driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-LIMITS.STEER_MAX, driver_min_torque), 0) + apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), + apply_torque_last + LIMITS.STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP, + min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) + + return int(round(float(apply_torque))) + + +def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): + # limits due to comparison of commanded torque VS motor reported torque + max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX) + min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX) + + apply_torque = clip(apply_torque, min_lim, max_lim) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, + max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), + apply_torque_last + LIMITS.STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, + apply_torque_last - LIMITS.STEER_DELTA_UP, + min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) + + return int(round(float(apply_torque))) + + +def crc8_pedal(data): + crc = 0xFF # standard init value + poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 + size = len(data) + for i in range(size - 1, -1, -1): + crc ^= data[i] + for _ in range(8): + if ((crc & 0x80) != 0): + crc = ((crc << 1) ^ poly) & 0xFF + else: + crc <<= 1 + return crc + + +def create_gas_interceptor_command(packer, gas_amount, idx): + # Common gas pedal msg generator + enable = gas_amount > 0.001 + + values = { + "ENABLE": enable, + "COUNTER_PEDAL": idx & 0xF, + } + + if enable: + values["GAS_COMMAND"] = gas_amount * 255. + values["GAS_COMMAND2"] = gas_amount * 255. + + dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2] + + checksum = crc8_pedal(dat[:-1]) + values["CHECKSUM_PEDAL"] = checksum + + return packer.make_can_msg("GAS_COMMAND", 0, values) + + +def make_can_msg(addr, dat, bus): + return [addr, 0, dat, bus] + + +def get_safety_config(safety_model, safety_param = None): + ret = car.CarParams.SafetyConfig.new_message() + ret.safetyModel = safety_model + if safety_param is not None: + ret.safetyParam = safety_param + return ret diff --git a/selfdrive/ui/onroad/__init__.py b/selfdrive/car/body/__init__.py similarity index 100% rename from selfdrive/ui/onroad/__init__.py rename to selfdrive/car/body/__init__.py diff --git a/selfdrive/car/body/bodycan.py b/selfdrive/car/body/bodycan.py new file mode 100644 index 00000000000000..580e5025ade783 --- /dev/null +++ b/selfdrive/car/body/bodycan.py @@ -0,0 +1,7 @@ +def create_control(packer, torque_l, torque_r): + values = { + "TORQUE_L": torque_l, + "TORQUE_R": torque_r, + } + + return packer.make_can_msg("TORQUE_CMD", 0, values) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py new file mode 100644 index 00000000000000..0d5d780bd313c3 --- /dev/null +++ b/selfdrive/car/body/carcontroller.py @@ -0,0 +1,90 @@ +import numpy as np + +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car.body import bodycan +from selfdrive.car.body.values import SPEED_FROM_RPM +from selfdrive.controls.lib.pid import PIDController + + +MAX_TORQUE = 500 +MAX_TORQUE_RATE = 50 +MAX_ANGLE_ERROR = np.radians(7) +MAX_POS_INTEGRATOR = 0.2 # meters +MAX_TURN_INTEGRATOR = 0.1 # meters + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.frame = 0 + self.packer = CANPacker(dbc_name) + + # Speed, balance and turn PIDs + self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL) + self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL) + self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) + + self.torque_r_filtered = 0. + self.torque_l_filtered = 0. + + @staticmethod + def deadband_filter(torque, deadband): + if torque > 0: + torque += deadband + else: + torque -= deadband + return torque + + def update(self, CC, CS): + + torque_l = 0 + torque_r = 0 + + llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0 + if CC.enabled and llk_valid: + # Read these from the joystick + # TODO: this isn't acceleration, okay? + speed_desired = CC.actuators.accel / 5. + speed_diff_desired = -CC.actuators.steer + + speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. + speed_error = speed_desired - speed_measured + + freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or + (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR)) + angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator) + + # Clip angle error, this is enough to get up from stands + angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR) + angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.) + torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate) + + speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) + turn_error = speed_diff_measured - speed_diff_desired + freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or + (turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR)) + torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator) + + # Combine 2 PIDs outputs + torque_r = torque + torque_diff + torque_l = torque - torque_diff + + # Torque rate limits + self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), + self.torque_r_filtered - MAX_TORQUE_RATE, + self.torque_r_filtered + MAX_TORQUE_RATE) + self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), + self.torque_l_filtered - MAX_TORQUE_RATE, + self.torque_l_filtered + MAX_TORQUE_RATE) + torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) + torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) + + can_sends = [] + can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) + + new_actuators = CC.actuators.copy() + new_actuators.accel = torque_l + new_actuators.steer = torque_r + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py new file mode 100644 index 00000000000000..dbbd85950daf98 --- /dev/null +++ b/selfdrive/car/body/carstate.py @@ -0,0 +1,60 @@ +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.body.values import DBC + +STARTUP_TICKS = 100 + +class CarState(CarStateBase): + def update(self, cp): + ret = car.CarState.new_message() + + ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] + ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R'] + + ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor + + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = False + + ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'], + cp.vl['VAR_VALUES']['FAULT']]) + + ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1 + ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100 + + # irrelevant for non-car + ret.gearShifter = car.CarState.GearShifter.drive + ret.cruiseState.enabled = True + ret.cruiseState.available = True + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("SPEED_L", "MOTORS_DATA"), + ("SPEED_R", "MOTORS_DATA"), + ("ELEC_ANGLE_L", "MOTORS_DATA"), + ("ELEC_ANGLE_R", "MOTORS_DATA"), + ("COUNTER", "MOTORS_DATA"), + ("CHECKSUM", "MOTORS_DATA"), + ("IGNITION", "VAR_VALUES"), + ("ENABLE_MOTORS", "VAR_VALUES"), + ("FAULT", "VAR_VALUES"), + ("MOTOR_ERR_L", "VAR_VALUES"), + ("MOTOR_ERR_R", "VAR_VALUES"), + ("MCU_TEMP", "BODY_DATA"), + ("BATT_VOLTAGE", "BODY_DATA"), + ("BATT_PERCENTAGE", "BODY_DATA"), + ("CHARGER_CONNECTED", "BODY_DATA"), + ] + + checks = [ + ("MOTORS_DATA", 100), + ("VAR_VALUES", 10), + ("BODY_DATA", 1), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py new file mode 100644 index 00000000000000..ae7ab89aab497a --- /dev/null +++ b/selfdrive/car/body/interface.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import math +from cereal import car +from common.realtime import DT_CTRL +from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.body.values import SPEED_FROM_RPM + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=None, car_fw=None, experimental_long=False): + + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.notCar = True + ret.carName = "body" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] + + ret.minSteerSpeed = -math.inf + ret.maxLateralAccel = math.inf # TODO: set to a reasonable value + ret.steerRatio = 0.5 + ret.steerLimitTimer = 1.0 + ret.steerActuatorDelay = 0. + + ret.mass = 9 + ret.wheelbase = 0.406 + ret.wheelSpeedFactor = SPEED_FROM_RPM + ret.centerToFront = ret.wheelbase * 0.44 + + ret.radarOffCan = True + ret.openpilotLongitudinalControl = True + ret.steerControlType = car.CarParams.SteerControlType.angle + + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + def _update(self, c): + ret = self.CS.update(self.cp) + + # wait for everything to init first + if self.frame > int(5. / DT_CTRL): + # body always wants to enable + ret.init('events', 1) + ret.events[0].name = car.CarEvent.EventName.pcmEnable + ret.events[0].enable = True + self.frame += 1 + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/body/radar_interface.py b/selfdrive/car/body/radar_interface.py new file mode 100644 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/body/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py new file mode 100644 index 00000000000000..66f1b947a8e0fe --- /dev/null +++ b/selfdrive/car/body/values.py @@ -0,0 +1,53 @@ +from typing import Dict + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu + +SPEED_FROM_RPM = 0.008587 + + +class CarControllerParams: + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower + STEER_THRESHOLD = 1.0 + + +class CAR: + BODY = "COMMA BODY" + + +CAR_INFO: Dict[str, CarInfo] = { + CAR.BODY: CarInfo("comma body", package="All"), +} + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + bus=0, + ), + ], +) + +FW_VERSIONS = { + CAR.BODY: { + (Ecu.engine, 0x720, None): [ + b'0.0.01', + b'02/27/2022' + ], + (Ecu.debug, 0x721, None): [ + b'166bd860' # git hash of the firmware used + ], + }, +} + +DBC = { + CAR.BODY: dbc_dict('comma_body', None), +} diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py new file mode 100644 index 00000000000000..4f098fadb5272a --- /dev/null +++ b/selfdrive/car/car_helpers.py @@ -0,0 +1,189 @@ +import os +from typing import Dict, List + +from cereal import car +from common.params import Params +from common.basedir import BASEDIR +from system.version import is_comma_remote, is_tested_branch +from selfdrive.car.interfaces import get_interface_attr +from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars +from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN +from selfdrive.car.fw_versions import get_fw_versions_ordered, match_fw_to_car, get_present_ecus +from system.swaglog import cloudlog +import cereal.messaging as messaging +from selfdrive.car import gen_empty_fingerprint + +EventName = car.CarEvent.EventName + + +def get_startup_event(car_recognized, controller_available, fw_seen): + if is_comma_remote() and is_tested_branch(): + event = EventName.startup + else: + event = EventName.startupMaster + + if not car_recognized: + if fw_seen: + event = EventName.startupNoCar + else: + event = EventName.startupNoFw + elif car_recognized and not controller_available: + event = EventName.startupNoControl + return event + + +def get_one_can(logcan): + while True: + can = messaging.recv_one_retry(logcan) + if len(can.can) > 0: + return can + + +def load_interfaces(brand_names): + ret = {} + for brand_name in brand_names: + path = f'selfdrive.car.{brand_name}' + CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface + + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'): + CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState + else: + CarState = None + + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): + CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController + else: + CarController = None + + for model_name in brand_names[brand_name]: + ret[model_name] = (CarInterface, CarController, CarState) + return ret + + +def _get_interface_names() -> Dict[str, List[str]]: + # returns a dict of brand name and its respective models + brand_names = {} + for brand_name, model_names in get_interface_attr("CAR").items(): + model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")] + brand_names[brand_name] = model_names + + return brand_names + + +# imports from directory selfdrive/car// +interface_names = _get_interface_names() +interfaces = load_interfaces(interface_names) + + +# **** for use live only **** +def fingerprint(logcan, sendcan): + fixed_fingerprint = os.environ.get('FINGERPRINT', "") + skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) + ecu_rx_addrs = set() + + if not fixed_fingerprint and not skip_fw_query: + # Vin query only reliably works through OBDII + bus = 1 + + cached_params = Params().get("CarParamsCache") + if cached_params is not None: + cached_params = car.CarParams.from_bytes(cached_params) + if cached_params.carName == "mock": + cached_params = None + + if cached_params is not None and len(cached_params.carFw) > 0 and cached_params.carVin is not VIN_UNKNOWN: + cloudlog.warning("Using cached CarParams") + vin, vin_rx_addr = cached_params.carVin, 0 + car_fw = list(cached_params.carFw) + else: + cloudlog.warning("Getting VIN & FW versions") + _, vin_rx_addr, vin = get_vin(logcan, sendcan, bus) + ecu_rx_addrs = get_present_ecus(logcan, sendcan) + car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs) + + exact_fw_match, fw_candidates = match_fw_to_car(car_fw) + else: + vin, vin_rx_addr = VIN_UNKNOWN, 0 + exact_fw_match, fw_candidates, car_fw = True, set(), [] + + if not is_valid_vin(vin): + cloudlog.event("Malformed VIN", vin=vin, error=True) + vin = VIN_UNKNOWN + cloudlog.warning("VIN %s", vin) + Params().put("CarVin", vin) + + finger = gen_empty_fingerprint() + candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 + frame = 0 + frame_fingerprint = 100 # 1s + car_fingerprint = None + done = False + + # drain CAN socket so we always get the latest messages + messaging.drain_sock_raw(logcan) + + while not done: + a = get_one_can(logcan) + + for can in a.can: + # The fingerprint dict is generated for all buses, this way the car interface + # can use it to detect a (valid) multipanda setup and initialize accordingly + if can.src < 128: + if can.src not in finger: + finger[can.src] = {} + finger[can.src][can.address] = len(can.dat) + + for b in candidate_cars: + # Ignore extended messages and VIN query response. + if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8): + candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) + + # if we only have one car choice and the time since we got our first + # message has elapsed, exit + for b in candidate_cars: + if len(candidate_cars[b]) == 1 and frame > frame_fingerprint: + # fingerprint done + car_fingerprint = candidate_cars[b][0] + + # bail if no cars left or we've been waiting for more than 2s + failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200 + succeeded = car_fingerprint is not None + done = failed or succeeded + + frame += 1 + + exact_match = True + source = car.CarParams.FingerprintSource.can + + # If FW query returns exactly 1 candidate, use it + if len(fw_candidates) == 1: + car_fingerprint = list(fw_candidates)[0] + source = car.CarParams.FingerprintSource.fw + exact_match = exact_fw_match + + if fixed_fingerprint: + car_fingerprint = fixed_fingerprint + source = car.CarParams.FingerprintSource.fixed + + cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, + fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True) + return car_fingerprint, finger, vin, car_fw, source, exact_match + + +def get_car(logcan, sendcan): + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan) + + if candidate is None: + cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) + candidate = "mock" + + experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled") + + CarInterface, CarController, CarState = interfaces[candidate] + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long) + CP.carVin = vin + CP.carFw = car_fw + CP.fingerprintSource = source + CP.fuzzyFingerprint = not exact_match + + return CarInterface(CP, CarController, CarState), CP diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py deleted file mode 100644 index 86494afc7a0dae..00000000000000 --- a/selfdrive/car/car_specific.py +++ /dev/null @@ -1,187 +0,0 @@ -from cereal import car, log -from opendbc.car import DT_CTRL, structs -from opendbc.car.car_helpers import interfaces -from opendbc.car.interfaces import MAX_CTRL_SPEED -from opendbc.car.toyota.values import ToyotaFlags - -from openpilot.selfdrive.selfdrived.events import Events - -ButtonType = structs.CarState.ButtonEvent.Type -GearShifter = structs.CarState.GearShifter -EventName = log.OnroadEvent.EventName -NetworkLocation = structs.CarParams.NetworkLocation - - -class CarSpecificEvents: - def __init__(self, CP: structs.CarParams): - self.CP = CP - - self.steering_unpressed = 0 - self.low_speed_alert = False - self.no_steer_warning = False - self.silent_steer_warning = True - - def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl): - if self.CP.brand in ('body', 'mock'): - return Events() - - events = self.create_common_events(CS, CS_prev) - - if self.CP.brand == 'chrysler': - # Low speed steer alert hysteresis logic - if self.CP.minSteerSpeed > 0. and CS.vEgo < (self.CP.minSteerSpeed + 0.5): - self.low_speed_alert = True - elif CS.vEgo > (self.CP.minSteerSpeed + 1.): - self.low_speed_alert = False - if self.low_speed_alert: - events.add(EventName.belowSteerSpeed) - - elif self.CP.brand == 'honda': - if self.CP.pcmCruise and CS.vEgo < self.CP.minEnableSpeed: - events.add(EventName.belowEngageSpeed) - - if self.CP.pcmCruise: - # we engage when pcm is active (rising edge) - if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled: - events.add(EventName.pcmEnable) - elif not CS.cruiseState.enabled and (CC.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl): - # it can happen that car cruise disables while comma system is enabled: need to - # keep braking if needed or if the speed is very low - if CS.vEgo < self.CP.minEnableSpeed + 2.: - # non loud alert if cruise disables below 25mph as expected (+ a little margin) - events.add(EventName.speedTooLow) - else: - events.add(EventName.cruiseDisabled) - if self.CP.minEnableSpeed > 0 and CS.vEgo < 0.001: - events.add(EventName.manualRestart) - - elif self.CP.brand == 'toyota': - # TODO: when we check for unexpected disengagement, check gear not S1, S2, S3 - if self.CP.openpilotLongitudinalControl: - # Only can leave standstill when planner wants to move - if CS.cruiseState.standstill and not CS.brakePressed and (CC.cruiseControl.resume or self.CP.flags & ToyotaFlags.HYBRID.value): - events.add(EventName.resumeRequired) - if CS.vEgo < self.CP.minEnableSpeed: - events.add(EventName.belowEngageSpeed) - if CC.actuators.accel > 0.3: - # some margin on the actuator to not false trigger cancellation while stopping - events.add(EventName.speedTooLow) - if CS.vEgo < 0.001: - # while in standstill, send a user alert - events.add(EventName.manualRestart) - - elif self.CP.brand == 'gm': - # Enabling at a standstill with brake is allowed - # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs - if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brake >= 20 and - self.CP.networkLocation == NetworkLocation.fwdCamera): - events.add(EventName.belowEngageSpeed) - if CS.cruiseState.standstill: - events.add(EventName.resumeRequired) - - elif self.CP.brand == 'volkswagen': - if self.CP.openpilotLongitudinalControl: - if CS.vEgo < self.CP.minEnableSpeed + 0.5: - events.add(EventName.belowEngageSpeed) - if CC.enabled and CS.vEgo < self.CP.minEnableSpeed: - events.add(EventName.speedTooLow) - - # TODO: this needs to be implemented generically in carState struct - # if CC.eps_timer_soft_disable_alert: - # events.add(EventName.steerTimeLimit) - - return events - - def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState): - events = Events() - - CI = interfaces[self.CP.carFingerprint] - # TODO: cleanup the honda-specific logic - pcm_enable = self.CP.pcmCruise and self.CP.brand != 'honda' - # TODO: on some hyundai cars, the cancel button is also the pause/resume button, - # so only use it for cancel when running openpilot longitudinal - allow_button_cancel = self.CP.brand != 'hyundai' - - if CS.doorOpen: - events.add(EventName.doorOpen) - if CS.seatbeltUnlatched: - events.add(EventName.seatbeltNotLatched) - if CS.gearShifter != GearShifter.drive and CS.gearShifter not in CI.DRIVABLE_GEARS: - events.add(EventName.wrongGear) - if CS.gearShifter == GearShifter.reverse: - events.add(EventName.reverseGear) - if not CS.cruiseState.available: - events.add(EventName.wrongCarMode) - if CS.espDisabled: - events.add(EventName.espDisabled) - if CS.espActive: - events.add(EventName.espActive) - if CS.stockFcw: - events.add(EventName.stockFcw) - if CS.stockAeb: - events.add(EventName.stockAeb) - if CS.stockLkas: - events.add(EventName.stockLkas) - if CS.vEgo > MAX_CTRL_SPEED: - events.add(EventName.speedTooHigh) - if CS.cruiseState.nonAdaptive: - events.add(EventName.wrongCruiseMode) - if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl: - events.add(EventName.brakeHold) - if CS.parkingBrake: - events.add(EventName.parkBrake) - if CS.accFaulted: - events.add(EventName.accFaulted) - if CS.steeringPressed: - events.add(EventName.steerOverride) - if CS.steeringDisengage and not CS_prev.steeringDisengage: - events.add(EventName.steerDisengage) - if CS.brakePressed and CS.standstill: - events.add(EventName.preEnableStandstill) - if CS.gasPressed: - events.add(EventName.gasPressedOverride) - if CS.vehicleSensorsInvalid: - events.add(EventName.vehicleSensorsInvalid) - if CS.invalidLkasSetting: - events.add(EventName.invalidLkasSetting) - if CS.lowSpeedAlert: - events.add(EventName.belowSteerSpeed) - if CS.buttonEnable: - events.add(EventName.buttonEnable) - - # Handle cancel button presses - for b in CS.buttonEvents: - # Disable on rising and falling edge of cancel for both stock and OP long - # TODO: only check the cancel button with openpilot longitudinal on all brands to match panda safety - if b.type == ButtonType.cancel and (allow_button_cancel or not self.CP.pcmCruise): - events.add(EventName.buttonCancel) - - # Handle permanent and temporary steering faults - self.steering_unpressed = 0 if CS.steeringPressed else self.steering_unpressed + 1 - if CS.steerFaultTemporary: - if CS.steeringPressed and (not CS_prev.steerFaultTemporary or self.no_steer_warning): - self.no_steer_warning = True - else: - self.no_steer_warning = False - - # if the user overrode recently, show a less harsh alert - if self.silent_steer_warning or CS.standstill or self.steering_unpressed < int(1.5 / DT_CTRL): - self.silent_steer_warning = True - events.add(EventName.steerTempUnavailableSilent) - else: - events.add(EventName.steerTempUnavailable) - else: - self.no_steer_warning = False - self.silent_steer_warning = False - if CS.steerFaultPermanent: - events.add(EventName.steerUnavailable) - - # we engage when pcm is active (rising edge) - # enabling can optionally be blocked by the car interface - if pcm_enable: - if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled and not CS.blockPcmEnable: - events.add(EventName.pcmEnable) - elif not CS.cruiseState.enabled: - events.add(EventName.pcmDisable) - - return events diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py deleted file mode 100755 index 12b831347100a8..00000000000000 --- a/selfdrive/car/card.py +++ /dev/null @@ -1,281 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import threading - -import cereal.messaging as messaging - -from cereal import car, log - -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper -from openpilot.common.swaglog import cloudlog, ForwardingHandler - -from opendbc.car import DT_CTRL, structs -from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable -from opendbc.car.carlog import carlog -from opendbc.car.fw_versions import ObdCallback -from opendbc.car.car_helpers import get_car, interfaces -from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase -from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp -from openpilot.selfdrive.car.cruise import VCruiseHelper - -REPLAY = "REPLAY" in os.environ - -EventName = log.OnroadEvent.EventName - -# forward -carlog.addHandler(ForwardingHandler(cloudlog)) - - -def obd_callback(params: Params) -> ObdCallback: - def set_obd_multiplexing(obd_multiplexing: bool): - if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: - cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") - params.remove("ObdMultiplexingChanged") - params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) - params.get_bool("ObdMultiplexingChanged", block=True) - cloudlog.warning("OBD multiplexing set successfully") - return set_obd_multiplexing - - -def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket) -> tuple[CanRecvCallable, CanSendCallable]: - def can_recv(wait_for_one: bool = False) -> list[list[CanData]]: - """ - wait_for_one: wait the normal logcan socket timeout for a CAN packet, may return empty list if nothing comes - - Returns: CAN packets comprised of CanData objects for easy access - """ - ret = [] - for can in messaging.drain_sock(logcan, wait_for_one=wait_for_one): - ret.append([CanData(msg.address, msg.dat, msg.src) for msg in can.can]) - return ret - - def can_send(msgs: list[CanData]) -> None: - sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) - - return can_recv, can_send - - -class Car: - CI: CarInterfaceBase - RI: RadarInterfaceBase - CP: car.CarParams - - def __init__(self, CI=None, RI=None) -> None: - self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) - - self.can_rcv_cum_timeout_counter = 0 - - self.CC_prev = car.CarControl.new_message() - self.CS_prev = car.CarState.new_message() - self.initialized_prev = False - - self.last_actuators_output = structs.CarControl.Actuators() - - self.params = Params() - - self.can_callbacks = can_comm_callbacks(self.can_sock, self.pm.sock['sendcan']) - - is_release = self.params.get_bool("IsReleaseBranch") - - if CI is None: - # wait for one pandaState and one CAN packet - print("Waiting for CAN messages...") - while True: - can = messaging.recv_one_retry(self.can_sock) - if len(can.can) > 0: - break - - alpha_long_allowed = self.params.get_bool("AlphaLongitudinalEnabled") - num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - - cached_params = None - cached_params_raw = self.params.get("CarParamsCache") - if cached_params_raw is not None: - with car.CarParams.from_bytes(cached_params_raw) as _cached_params: - cached_params = _cached_params - - self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, cached_params) - self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP) - self.CP = self.CI.CP - - # continue onto next fingerprinting step in pandad - self.params.put_bool("FirmwareQueryDone", True) - else: - self.CI, self.CP = CI, CI.CP - self.RI = RI - - self.CP.alternativeExperience = 0 - openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") - controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly - self.CP.passive = not controller_available or self.CP.dashcamOnly - if self.CP.passive: - safety_config = structs.CarParams.SafetyConfig() - safety_config.safetyModel = structs.CarParams.SafetyModel.noOutput - self.CP.safetyConfigs = [safety_config] - - if self.CP.secOcRequired: - # Copy user key if available - try: - with open("/cache/params/SecOCKey") as f: - user_key = f.readline().strip() - if len(user_key) == 32: - self.params.put("SecOCKey", user_key) - except Exception: - pass - - secoc_key = self.params.get("SecOCKey") - if secoc_key is not None: - saved_secoc_key = bytes.fromhex(secoc_key.strip()) - if len(saved_secoc_key) == 16: - self.CP.secOcKeyAvailable = True - self.CI.CS.secoc_key = saved_secoc_key - if controller_available: - self.CI.CC.secoc_key = saved_secoc_key - else: - cloudlog.warning("Saved SecOC key is invalid") - - # Write previous route's CarParams - prev_cp = self.params.get("CarParamsPersistent") - if prev_cp is not None: - self.params.put("CarParamsPrevRoute", prev_cp) - - # Write CarParams for controls and radard - cp_bytes = self.CP.to_bytes() - self.params.put("CarParams", cp_bytes) - self.params.put_nonblocking("CarParamsCache", cp_bytes) - self.params.put_nonblocking("CarParamsPersistent", cp_bytes) - - self.v_cruise_helper = VCruiseHelper(self.CP) - - self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") - - # card is driven by can recv, expected at 100Hz - self.rk = Ratekeeper(100, print_delay_threshold=None) - - def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]: - """carState update loop, driven by can""" - - can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - can_list = can_capnp_to_list(can_strs) - - # Update carState from CAN - CS = self.CI.update(can_list) - - # Update radar tracks from CAN - RD: structs.RadarDataT | None = self.RI.update(can_list) - - self.sm.update(0) - - can_rcv_valid = len(can_strs) > 0 - - # Check for CAN timeout - if not can_rcv_valid: - self.can_rcv_cum_timeout_counter += 1 - - if can_rcv_valid and REPLAY: - self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - - 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 - self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode) - - # TODO: mirror the carState.cruiseState struct? - CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) - CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) - - return CS, RD - - def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None): - """carState and carParams publish loop""" - - # carParams - logged every 50 seconds (> 1 per segment) - if self.sm.frame % int(50. / DT_CTRL) == 0: - cp_send = messaging.new_message('carParams') - cp_send.valid = True - cp_send.carParams = self.CP - self.pm.send('carParams', cp_send) - - # publish new carOutput - co_send = messaging.new_message('carOutput') - co_send.valid = self.sm.all_checks(['carControl']) - co_send.carOutput.actuatorsOutput = self.last_actuators_output - self.pm.send('carOutput', co_send) - - # kick off controlsd step while we actuate the latest carControl packet - cs_send = messaging.new_message('carState') - cs_send.valid = CS.canValid - cs_send.carState = CS - cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter - cs_send.carState.cumLagMs = -self.rk.remaining * 1000. - self.pm.send('carState', cs_send) - - if RD is not None: - tracks_msg = messaging.new_message('liveTracks') - tracks_msg.valid = not any(RD.errors.to_dict().values()) - tracks_msg.liveTracks = RD - self.pm.send('liveTracks', tracks_msg) - - def controls_update(self, CS: car.CarState, CC: car.CarControl): - """control update loop, driven by carControl""" - - if not self.initialized_prev: - # Initialize CarInterface, once controls are ready - # TODO: this can make us miss at least a few cycles when doing an ECU knockout - self.CI.init(self.CP, *self.can_callbacks) - # signal pandad to switch to car safety mode - self.params.put_bool_nonblocking("ControlsReady", True) - - if self.sm.all_alive(['carControl']): - # send car controls over can - now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) - self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) - - self.CC_prev = CC - - def step(self): - CS, RD = self.state_update() - - self.state_publish(CS, RD) - - initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and - self.sm.seen['onroadEvents']) - if not self.CP.passive and initialized: - self.controls_update(CS, self.sm['carControl']) - - self.initialized_prev = initialized - self.CS_prev = CS - - def params_thread(self, evt): - while not evt.is_set(): - self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl - time.sleep(0.1) - - def card_thread(self): - e = threading.Event() - t = threading.Thread(target=self.params_thread, args=(e, )) - try: - t.start() - while True: - self.step() - self.rk.monitor_time() - finally: - e.set() - t.join() - - -def main(): - config_realtime_process(4, Priority.CTRL_HIGH) - car = Car() - car.card_thread() - - -if __name__ == "__main__": - main() diff --git a/selfdrive/ui/widgets/__init__.py b/selfdrive/car/chrysler/__init__.py similarity index 100% rename from selfdrive/ui/widgets/__init__.py rename to selfdrive/car/chrysler/__init__.py diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py new file mode 100644 index 00000000000000..5a2d90c64c2410 --- /dev/null +++ b/selfdrive/car/chrysler/carcontroller.py @@ -0,0 +1,82 @@ +from opendbc.can.packer import CANPacker +from common.realtime import DT_CTRL +from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons +from selfdrive.car.chrysler.values import CAR, RAM_CARS, CarControllerParams + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.apply_steer_last = 0 + self.frame = 0 + + self.hud_count = 0 + self.last_lkas_falling_edge = 0 + self.lkas_control_bit_prev = False + self.last_button_frame = 0 + + self.packer = CANPacker(dbc_name) + self.params = CarControllerParams(CP) + + def update(self, CC, CS): + can_sends = [] + + lkas_active = CC.latActive and self.lkas_control_bit_prev + + # cruise buttons + if (self.frame - self.last_button_frame)*DT_CTRL > 0.05: + das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0 + + # ACC cancellation + if CC.cruiseControl.cancel: + self.last_button_frame = self.frame + can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True)) + + # ACC resume from standstill + elif CC.cruiseControl.resume: + self.last_button_frame = self.frame + can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True)) + + # HUD alerts + if self.frame % 25 == 0: + if CS.lkas_car_model != -1: + can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) + self.hud_count += 1 + + # steering + if self.frame % 2 == 0: + + # TODO: can we make this more sane? why is it different for all the cars? + lkas_control_bit = self.lkas_control_bit_prev + if CS.out.vEgo > self.CP.minSteerSpeed: + lkas_control_bit = True + elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0): + lkas_control_bit = False + elif self.CP.carFingerprint in RAM_CARS: + if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5): + lkas_control_bit = False + + # EPS faults if LKAS re-enables too quickly + lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200) + + if not lkas_control_bit and self.lkas_control_bit_prev: + self.last_lkas_falling_edge = self.frame + self.lkas_control_bit_prev = lkas_control_bit + + # steer torque + new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) + if not lkas_active or not lkas_control_bit: + apply_steer = 0 + self.apply_steer_last = apply_steer + + can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit)) + + self.frame += 1 + + new_actuators = CC.actuators.copy() + new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + + return new_actuators, can_sends diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py new file mode 100644 index 00000000000000..0f0d30782aa2e3 --- /dev/null +++ b/selfdrive/car/chrysler/carstate.py @@ -0,0 +1,205 @@ +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.CP = CP + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + self.auto_high_beam = 0 + self.button_counter = 0 + self.lkas_car_model = -1 + + if CP.carFingerprint in RAM_CARS: + self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"] + else: + self.shifter_values = can_define.dv["GEAR"]["PRNDL"] + + def update(self, cp, cp_cam): + + ret = car.CarState.new_message() + + # lock info + ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], + cp.vl["BCM_1"]["DOOR_OPEN_FR"], + cp.vl["BCM_1"]["DOOR_OPEN_RL"], + cp.vl["BCM_1"]["DOOR_OPEN_RR"]]) + ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1 + + # brake pedal + ret.brake = 0 + ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch + + # gas pedal + ret.gas = cp.vl["ECM_5"]["Accelerator_Position"] + ret.gasPressed = ret.gas > 1e-5 + + # car speed + if self.CP.carFingerprint in RAM_CARS: + ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None)) + else: + ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2. + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None)) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = not ret.vEgoRaw > 0.001 + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["ESP_6"]["WHEEL_SPEED_FL"], + cp.vl["ESP_6"]["WHEEL_SPEED_FR"], + cp.vl["ESP_6"]["WHEEL_SPEED_RL"], + cp.vl["ESP_6"]["WHEEL_SPEED_RR"], + unit=1, + ) + + # button presses + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, + cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2) + ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1 + + # steering wheel + ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"] + ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"] + ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"] + ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + + # cruise state + cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp + + ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1 + ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS + ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet + ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1 + ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0 + + if self.CP.carFingerprint in RAM_CARS: + self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON'] # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message + ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1 + else: + ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4 + + # blindspot sensors + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1 + ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1 + + self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] + self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] + + return ret + + @staticmethod + def get_cruise_signals(): + signals = [ + ("ACC_AVAILABLE", "DAS_3"), + ("ACC_ACTIVE", "DAS_3"), + ("ACC_FAULTED", "DAS_3"), + ("ACC_STANDSTILL", "DAS_3"), + ("COUNTER", "DAS_3"), + ("ACC_SET_SPEED_KPH", "DAS_4"), + ("ACC_STATE", "DAS_4"), + ] + checks = [ + ("DAS_3", 50), + ("DAS_4", 50), + ] + return signals, checks + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("DOOR_OPEN_FL", "BCM_1"), + ("DOOR_OPEN_FR", "BCM_1"), + ("DOOR_OPEN_RL", "BCM_1"), + ("DOOR_OPEN_RR", "BCM_1"), + ("Brake_Pedal_State", "ESP_1"), + ("Accelerator_Position", "ECM_5"), + ("WHEEL_SPEED_FL", "ESP_6"), + ("WHEEL_SPEED_RR", "ESP_6"), + ("WHEEL_SPEED_RL", "ESP_6"), + ("WHEEL_SPEED_FR", "ESP_6"), + ("STEERING_ANGLE", "STEERING"), + ("STEERING_ANGLE_HP", "STEERING"), + ("STEERING_RATE", "STEERING"), + ("TURN_SIGNALS", "STEERING_LEVERS"), + ("HIGH_BEAM_PRESSED", "STEERING_LEVERS"), + ("SEATBELT_DRIVER_UNLATCHED", "ORC_1"), + ("COUNTER", "EPS_2",), + ("COLUMN_TORQUE", "EPS_2"), + ("EPS_TORQUE_MOTOR", "EPS_2"), + ("LKAS_STATE", "EPS_2"), + ("COUNTER", "CRUISE_BUTTONS"), + ] + + checks = [ + # sig_address, frequency + ("ESP_1", 50), + ("EPS_2", 100), + ("ESP_6", 50), + ("STEERING", 100), + ("ECM_5", 50), + ("CRUISE_BUTTONS", 50), + ("STEERING_LEVERS", 10), + ("ORC_1", 2), + ("BCM_1", 1), + ] + + if CP.enableBsm: + signals += [ + ("RIGHT_STATUS", "BSM_1"), + ("LEFT_STATUS", "BSM_1"), + ] + checks.append(("BSM_1", 2)) + + if CP.carFingerprint in RAM_CARS: + signals += [ + ("DASM_FAULT", "EPS_3"), + ("Vehicle_Speed", "ESP_8"), + ("Gear_State", "Transmission_Status"), + ] + checks += [ + ("ESP_8", 50), + ("EPS_3", 50), + ("Transmission_Status", 50), + ] + else: + signals += [ + ("PRNDL", "GEAR"), + ("SPEED_LEFT", "SPEED_1"), + ("SPEED_RIGHT", "SPEED_1"), + ] + checks += [ + ("GEAR", 50), + ("SPEED_1", 100), + ] + signals += CarState.get_cruise_signals()[0] + checks += CarState.get_cruise_signals()[1] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("CAR_MODEL", "DAS_6"), + ] + checks = [ + ("DAS_6", 4), + ] + + if CP.carFingerprint in RAM_CARS: + signals += [ + ("AUTO_HIGH_BEAM_ON", "DAS_6"), + ] + signals += CarState.get_cruise_signals()[0] + checks += CarState.get_cruise_signals()[1] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py new file mode 100644 index 00000000000000..10ed73e9f2bcfe --- /dev/null +++ b/selfdrive/car/chrysler/chryslercan.py @@ -0,0 +1,71 @@ +from cereal import car +from selfdrive.car.chrysler.values import RAM_CARS + +GearShifter = car.CarState.GearShifter +VisualAlert = car.CarControl.HUDControl.VisualAlert + +def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): + # LKAS_HUD - Controls what lane-keeping icon is displayed + + # == Color == + # 0 hidden? + # 1 white + # 2 green + # 3 ldw + + # == Lines == + # 03 white Lines + # 04 grey lines + # 09 left lane close + # 0A right lane close + # 0B left Lane very close + # 0C right Lane very close + # 0D left cross cross + # 0E right lane cross + + # == Alerts == + # 7 Normal + # 6 lane departure place hands on wheel + + color = 2 if lkas_active else 1 + lines = 3 if lkas_active else 0 + alerts = 7 if lkas_active else 0 + + if hud_count < (1 * 4): # first 3 seconds, 4Hz + alerts = 1 + + if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired): + color = 4 + lines = 0 + alerts = 6 + + values = { + "LKAS_ICON_COLOR": color, + "CAR_MODEL": car_model, + "LKAS_LANE_LINES": lines, + "LKAS_ALERTS": alerts, + } + + if CP.carFingerprint in RAM_CARS: + values['AUTO_HIGH_BEAM_ON'] = auto_high_beam + + return packer.make_can_msg("DAS_6", 0, values) + + +def create_lkas_command(packer, CP, apply_steer, lkas_control_bit): + # LKAS_COMMAND Lane-keeping signal to turn the wheel + enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1 + values = { + "STEERING_TORQUE": apply_steer, + "LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0, + } + return packer.make_can_msg("LKAS_COMMAND", 0, values) + + +def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False): + values = { + "ACC_Cancel": cancel, + "ACC_Resume": resume, + "COUNTER": frame % 0x10, + } + return packer.make_can_msg("CRUISE_BUTTONS", bus, values) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py new file mode 100755 index 00000000000000..245e10650c1aaa --- /dev/null +++ b/selfdrive/car/chrysler/interface.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT +from selfdrive.car.interfaces import CarInterfaceBase + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "chrysler" + + ret.dashcamOnly = candidate in RAM_HD + + ret.radarOffCan = DBC[candidate]['radar'] is None + + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.4 + + # safety config + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)] + if candidate in RAM_HD: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_HD + elif candidate in RAM_DT: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT + + ret.minSteerSpeed = 3.8 # m/s + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + # TODO: allow 2019 cars to steer down to 13 m/s if already engaged. + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + + # Chrysler + if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020): + ret.mass = 2242. + STD_CARGO_KG + ret.wheelbase = 3.089 + ret.steerRatio = 16.2 # Pacifica Hybrid 2017 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 + + # Jeep + elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): + ret.mass = 1778 + STD_CARGO_KG + ret.wheelbase = 2.71 + ret.steerRatio = 16.7 + ret.steerActuatorDelay = 0.2 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 + + # Ram + elif candidate == CAR.RAM_1500: + ret.steerActuatorDelay = 0.2 + ret.wheelbase = 3.88 + ret.steerRatio = 16.3 + ret.mass = 2493. + STD_CARGO_KG + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + ret.minSteerSpeed = 14.5 + if car_fw is not None: + for fw in car_fw: + if fw.ecu == 'eps' and fw.fwVersion[:8] in (b"68312176", b"68273275"): + ret.minSteerSpeed = 0. + + elif candidate == CAR.RAM_HD: + ret.steerActuatorDelay = 0.2 + ret.wheelbase = 3.785 + ret.steerRatio = 15.61 + ret.mass = 3405. + STD_CARGO_KG + ret.minSteerSpeed = 16 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False) + + else: + raise ValueError(f"Unsupported car: {candidate}") + + ret.centerToFront = ret.wheelbase * 0.44 + + # starting with reasonable value for civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + ret.enableBsm = 720 in fingerprint[0] + + return ret + + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + # events + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) + + # Low speed steer alert hysteresis logic + if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5): + self.low_speed_alert = True + elif ret.vEgo > (self.CP.minSteerSpeed + 1.): + self.low_speed_alert = False + if self.low_speed_alert: + events.add(car.CarEvent.EventName.belowSteerSpeed) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py new file mode 100755 index 00000000000000..348e3c3632dfb4 --- /dev/null +++ b/selfdrive/car/chrysler/radar_interface.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +from opendbc.can.parser import CANParser +from cereal import car +from selfdrive.car.interfaces import RadarInterfaceBase +from selfdrive.car.chrysler.values import DBC + +RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724 +RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages +LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D) +NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) + +def _create_radar_can_parser(car_fingerprint): + dbc = DBC[car_fingerprint]['radar'] + if dbc is None: + return None + + msg_n = len(RADAR_MSGS_C) + # list of [(signal name, message name or number), (...)] + # [('RADAR_STATE', 1024), + # ('LONG_DIST', 1072), + # ('LONG_DIST', 1073), + # ('LONG_DIST', 1074), + # ('LONG_DIST', 1075), + + signals = list(zip(['LONG_DIST'] * msg_n + + ['LAT_DIST'] * msg_n + + ['REL_SPEED'] * msg_n, + RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST + RADAR_MSGS_D)) # REL_SPEED + + checks = list(zip(RADAR_MSGS_C + + RADAR_MSGS_D, + [20] * msg_n + # 20Hz (0.05s) + [20] * msg_n)) # 20Hz (0.05s) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) + +def _address_to_track(address): + if address in RADAR_MSGS_C: + return (address - RADAR_MSGS_C[0]) // 2 + if address in RADAR_MSGS_D: + return (address - RADAR_MSGS_D[0]) // 2 + raise ValueError("radar received unexpected address %d" % address) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.rcp = _create_radar_can_parser(CP.carFingerprint) + self.updated_messages = set() + self.trigger_msg = LAST_MSG + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + for ii in self.updated_messages: # ii should be the message ID as a number + cpt = self.rcp.vl[ii] + trackId = _address_to_track(ii) + + if trackId not in self.pts: + self.pts[trackId] = car.RadarData.RadarPoint.new_message() + self.pts[trackId].trackId = trackId + self.pts[trackId].aRel = float('nan') + self.pts[trackId].yvRel = float('nan') + self.pts[trackId].measured = True + + if 'LONG_DIST' in cpt: # c_* message + self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car + # our lat_dist is positive to the right in car's frame. + # TODO what does yRel want? + self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive + else: # d_* message + self.pts[trackId].vRel = cpt['REL_SPEED'] + + # We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid. + ret.points = [x for x in self.pts.values() if x.dRel != 0] + + self.updated_messages.clear() + return ret \ No newline at end of file diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py new file mode 100644 index 00000000000000..3b3fc6e558ee1d --- /dev/null +++ b/selfdrive/car/chrysler/values.py @@ -0,0 +1,280 @@ +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Optional, Union + +from cereal import car +from panda.python import uds +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 + +Ecu = car.CarParams.Ecu + + +class CAR: + # Chrysler + PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" + PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" + PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" + PACIFICA_2018 = "CHRYSLER PACIFICA 2018" + PACIFICA_2020 = "CHRYSLER PACIFICA 2020" + + # Jeep + JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk + JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk + + # Ram + RAM_1500 = "RAM 1500 5TH GEN" + RAM_HD = "RAM HD 5TH GEN" + + +class CarControllerParams: + def __init__(self, CP): + self.STEER_ERROR_MAX = 80 + if CP.carFingerprint in RAM_HD: + self.STEER_DELTA_UP = 14 + self.STEER_DELTA_DOWN = 14 + self.STEER_MAX = 361 # higher than this faults the EPS + elif CP.carFingerprint in RAM_DT: + self.STEER_DELTA_UP = 6 + self.STEER_DELTA_DOWN = 6 + self.STEER_MAX = 261 # EPS allows more, up to 350? + else: + self.STEER_DELTA_UP = 3 + self.STEER_DELTA_DOWN = 3 + self.STEER_MAX = 261 # higher than this faults the EPS + +STEER_THRESHOLD = 120 + +RAM_DT = {CAR.RAM_1500, } +RAM_HD = {CAR.RAM_HD, } +RAM_CARS = RAM_DT | RAM_HD + +@dataclass +class ChryslerCarInfo(CarInfo): + package: str = "Adaptive Cruise Control" + harness: Enum = Harness.fca + +CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { + CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"), + CAR.PACIFICA_2018_HYBRID: None, # same platforms + CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-22"), + CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), + CAR.PACIFICA_2020: [ + ChryslerCarInfo("Chrysler Pacifica 2019-20"), + ChryslerCarInfo("Chrysler Pacifica 2021", package="All"), + ], + CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), + CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), + CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram), + CAR.RAM_HD: [ + ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram), + ChryslerCarInfo("Ram 3500 2020-22", harness=Harness.ram), + ], +} + +# Unique CAN messages: +# Only the hybrids have 270: 8 +# Only the gas have 55: 8, 416: 7 +# For 564, All 2017 have length 4, whereas 2018-19 have length 8. +# For 924, Pacifica 2017 has length 3, whereas all 2018-19 have length 8. +# For 560, All 2019 have length 8, whereas all 2017-18 have length 4. + +# Jeep Grand Cherokee unique messages: +# 2017 Trailhawk: 618: 8 +# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8. + +FINGERPRINTS = { + CAR.PACIFICA_2017_HYBRID: [{ + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8 + }], + CAR.PACIFICA_2018: [{ + 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 + }, + { + 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 + }], + CAR.PACIFICA_2020: [{ + 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8 + }], + CAR.PACIFICA_2018_HYBRID: [{ + 68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 + }, + # based on 9ae7821dc4e92455|2019-07-01--16-42-55 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }], + CAR.PACIFICA_2019_HYBRID: [{ + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8 + }, + # Based on 0607d2516fc2148f|2019-02-13--23-03-16 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8 + }, + # Based on 3c7ce223e3571b54|2019-05-11--20-16-14 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 + }, + # Based on "8190c7275a24557b|2020-02-24--09-57-23" + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }], + CAR.JEEP_CHEROKEE: [{ + 55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + # Based on c88f65eeaee4003a|2022-08-04--15-37-16 + { + 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 678: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1235: 8, 1242: 8, 1252: 8, 1792: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1892: 8, 1927: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], + CAR.JEEP_CHEROKEE_2019: [{ + # Jeep Grand Cherokee 2019, including most 2020 models + 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], +} + +CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf132) +CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xf132) + +CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) +CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) + +CHRYSLER_RX_OFFSET = -0x280 + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [CHRYSLER_VERSION_REQUEST], + [CHRYSLER_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.gateway, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter], + rx_offset=CHRYSLER_RX_OFFSET, + ), + Request( + [CHRYSLER_VERSION_REQUEST], + [CHRYSLER_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.hcp, Ecu.engine, Ecu.transmission], + ), + Request( + [CHRYSLER_SOFTWARE_VERSION_REQUEST], + [CHRYSLER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.transmission], + ), + ], +) + +FW_VERSIONS = { + CAR.PACIFICA_2019_HYBRID: { + (Ecu.hcp, 0x7e2, None): [], + (Ecu.abs, 0x7e4, None): [], + }, + + CAR.RAM_1500: { + (Ecu.combinationMeter, 0x742, None): [ + b'68294063AH', + b'68294063AG', + b'68434860AC', + b'68527375AD', + b'68453503AC', + ], + (Ecu.srs, 0x744, None): [ + b'68441329AB', + b'68490898AA', + b'68428609AB', + b'68500728AA', + ], + (Ecu.abs, 0x747, None): [ + b'68432418AD', + b'68432418AB', + b'68436004AE', + b'68438454AD', + b'68436004AD', + b'68535469AB', + b'68438454AC', + ], + (Ecu.fwdRadar, 0x753, None): [ + b'68320950AL', + b'68320950AJ', + b'68454268AB', + b'68475160AG', + b'04672892AB', + b'68475160AE', + ], + (Ecu.eps, 0x75A, None): [ + b'68273275AG', + b'68469901AA', + b'68552788AA', + ], + (Ecu.engine, 0x7e0, None): [ + b'68448163AJ', + b'68500630AD', + b'68539650AD', + b'68378758AM ', + ], + (Ecu.transmission, 0x7e1, None): [ + b'68360078AL', + b'68384328AD', + b'68360085AL', + b'68360081AM', + b'68502994AD', + b'68445533AB', + b'68540431AB', + b'68484467AC', + ], + (Ecu.gateway, 0x18DACBF1, None): [ + b'68402660AB', + b'68445283AB', + b'68533631AB', + b'68500483AB', + ], + }, + + CAR.RAM_HD: { + (Ecu.combinationMeter, 0x742, None): [ + b'68361606AH', + b'68492693AD', + ], + (Ecu.srs, 0x744, None): [ + b'68399794AC', + b'68428503AA', + b'68428505AA', + ], + (Ecu.abs, 0x747, None): [ + b'68334977AH', + b'68504022AB', + b'68530686AB', + ], + (Ecu.fwdRadar, 0x753, None): [ + b'04672895AB', + b'56029827AG', + b'68484694AE', + ], + (Ecu.eps, 0x761, None): [ + b'68421036AC', + b'68507906AB', + ], + (Ecu.engine, 0x7e0, None): [ + b'52421132AF', + b'M2370131MB', + b'M2421132MB', + ], + (Ecu.gateway, 0x18DACBF1, None): [ + b'68488419AB', + b'68535476AB', + ], + }, +} + +DBC = { + CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.JEEP_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.JEEP_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None), + CAR.RAM_HD: dbc_dict('chrysler_ram_hd_generated', None), +} diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py deleted file mode 100644 index 0d761844b53810..00000000000000 --- a/selfdrive/car/cruise.py +++ /dev/null @@ -1,138 +0,0 @@ -import math -import numpy as np - -from cereal import car -from openpilot.common.constants import CV - - -# WARNING: this value was determined based on the model's training distribution, -# model predictions above this speed can be unpredictable -# V_CRUISE's are in kph -V_CRUISE_MIN = 8 -V_CRUISE_MAX = 145 -V_CRUISE_UNSET = 255 -V_CRUISE_INITIAL = 40 -V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 -IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1) # round here to avoid rounding errors incrementing set speed - -ButtonEvent = car.CarState.ButtonEvent -ButtonType = car.CarState.ButtonEvent.Type -CRUISE_LONG_PRESS = 50 -CRUISE_NEAREST_FUNC = { - ButtonType.accelCruise: math.ceil, - ButtonType.decelCruise: math.floor, -} -CRUISE_INTERVAL_SIGN = { - ButtonType.accelCruise: +1, - ButtonType.decelCruise: -1, -} - - -class VCruiseHelper: - def __init__(self, CP): - self.CP = CP - self.v_cruise_kph = V_CRUISE_UNSET - self.v_cruise_cluster_kph = V_CRUISE_UNSET - self.v_cruise_kph_last = 0 - self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} - self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} - - @property - def v_cruise_initialized(self): - return self.v_cruise_kph != V_CRUISE_UNSET - - def update_v_cruise(self, CS, enabled, is_metric): - self.v_cruise_kph_last = self.v_cruise_kph - - if CS.cruiseState.available: - if not self.CP.pcmCruise: - # 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.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS, enabled) - else: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - if CS.cruiseState.speed == 0: - self.v_cruise_kph = V_CRUISE_UNSET - self.v_cruise_cluster_kph = V_CRUISE_UNSET - elif CS.cruiseState.speed == -1: - self.v_cruise_kph = -1 - self.v_cruise_cluster_kph = -1 - else: - self.v_cruise_kph = V_CRUISE_UNSET - self.v_cruise_cluster_kph = V_CRUISE_UNSET - - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): - # handle button presses. TODO: this should be in state_control, but a decelCruise press - # would have the effect of both enabling and changing speed is checked after the state transition - if not enabled: - return - - long_press = False - button_type = None - - v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT - - for b in CS.buttonEvents: - if b.type.raw in self.button_timers and not b.pressed: - if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: - return # end long press - button_type = b.type.raw - break - else: - for k, timer in self.button_timers.items(): - if timer and timer % CRUISE_LONG_PRESS == 0: - button_type = k - long_press = True - break - - if button_type is None: - return - - # Don't adjust speed when pressing resume to exit standstill - cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill - if button_type == ButtonType.accelCruise and cruise_standstill: - return - - # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) - if not self.button_change_states[button_type]["enabled"]: - return - - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) - 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 - else: - self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - - # If set is pressed while overriding, clip cruise speed to minimum of vEgo - 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) - - def update_button_timers(self, CS, enabled): - # increment timer for buttons still pressed - for k in self.button_timers: - if self.button_timers[k] > 0: - self.button_timers[k] += 1 - - for b in CS.buttonEvents: - if b.type.raw in self.button_timers: - # Start/end timer and store current state on change of button pressed - self.button_timers[b.type.raw] = 1 if b.pressed else 0 - self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: - # initializing is handled by the PCM - if self.CP.pcmCruise: - return - - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL - - if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: - self.v_cruise_kph = self.v_cruise_kph_last - else: - self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) - - self.v_cruise_cluster_kph = self.v_cruise_kph diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py new file mode 100755 index 00000000000000..cd3e93fa80c1bd --- /dev/null +++ b/selfdrive/car/disable_ecu.py @@ -0,0 +1,48 @@ +from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from system.swaglog import cloudlog + +EXT_DIAG_REQUEST = b'\x10\x03' +EXT_DIAG_RESPONSE = b'\x50\x03' + +COM_CONT_RESPONSE = b'' + + +def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False): + """Silence an ECU by disabling sending and receiving messages using UDS 0x28. + The ECU will stay silent as long as openpilot keeps sending Tester Present. + + This is used to disable the radar in some cars. Openpilot will emulate the radar. + WARNING: THIS DISABLES AEB!""" + cloudlog.warning(f"ecu disable {hex(addr)} ...") + + for i in range(retry): + try: + query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug) + + for _, _ in query.get_data(timeout).items(): + cloudlog.warning("communication control disable tx/rx ...") + + query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [com_cont_req], [COM_CONT_RESPONSE], debug=debug) + query.get_data(0) + + cloudlog.warning("ecu disabled") + return True + + except Exception: + cloudlog.exception("ecu disable exception") + + print(f"ecu disable retry ({i+1}) ...") + cloudlog.warning("ecu disable failed") + return False + + +if __name__ == "__main__": + import time + import cereal.messaging as messaging + sendcan = messaging.pub_sock('sendcan') + logcan = messaging.sub_sock('can') + time.sleep(1) + + # honda bosch radar disable + disabled = disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5, debug=False) + print(f"disabled: {disabled}") diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py index f807fc320edbe0..e948698cb45f35 100755 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -1,13 +1,69 @@ #!/usr/bin/env python3 import argparse +from collections import defaultdict +import jinja2 import os +from enum import Enum +from natsort import natsorted +from typing import Dict, List + +from common.basedir import BASEDIR +from selfdrive.car import gen_empty_fingerprint +from selfdrive.car.docs_definitions import CarInfo, Column +from selfdrive.car.car_helpers import interfaces, get_interface_attr + + +def get_all_footnotes() -> Dict[Enum, int]: + all_footnotes = [] + for footnotes in get_interface_attr("Footnote", ignore_none=True).values(): + all_footnotes.extend(footnotes) + return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} -from openpilot.common.basedir import BASEDIR -from opendbc.car.docs import get_all_car_docs, generate_cars_md CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") + +def get_all_car_info() -> List[CarInfo]: + all_car_info: List[CarInfo] = [] + footnotes = get_all_footnotes() + for model, car_info in get_interface_attr("CAR_INFO", combine_brands=True).items(): + CP = interfaces[model][0].get_params(model, fingerprint=gen_empty_fingerprint(), experimental_long=True) + + if CP.dashcamOnly or car_info is None: + continue + + # A platform can include multiple car models + if not isinstance(car_info, list): + car_info = (car_info,) + + for _car_info in car_info: + if not hasattr(_car_info, "row"): + _car_info.init(CP, footnotes) + all_car_info.append(_car_info) + + # Sort cars by make and model + year + sorted_cars: List[CarInfo] = natsorted(all_car_info, key=lambda car: car.name.lower()) + return sorted_cars + + +def group_by_make(all_car_info: List[CarInfo]) -> Dict[str, List[CarInfo]]: + sorted_car_info = defaultdict(list) + for car_info in all_car_info: + sorted_car_info[car_info.make].append(car_info) + return dict(sorted_car_info) + + +def generate_cars_md(all_car_info: List[CarInfo], template_fn: str) -> str: + with open(template_fn, "r") as f: + template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True) + + footnotes = [fn.value.text for fn in get_all_footnotes()] + cars_md: str = template.render(all_car_info=all_car_info, group_by_make=group_by_make, + footnotes=footnotes, Column=Column) + return cars_md + + if __name__ == "__main__": parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", formatter_class=argparse.ArgumentDefaultsHelpFormatter) @@ -17,5 +73,5 @@ args = parser.parse_args() with open(args.out, 'w') as f: - f.write(generate_cars_md(get_all_car_docs(), args.template)) + f.write(generate_cars_md(get_all_car_info(), args.template)) print(f"Generated and written to {args.out}") diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py new file mode 100644 index 00000000000000..015877dcb4030e --- /dev/null +++ b/selfdrive/car/docs_definitions.py @@ -0,0 +1,193 @@ +import re +from collections import namedtuple +from dataclasses import dataclass, field +from enum import Enum +from typing import Dict, List, Optional, Tuple, Union + +from cereal import car +from common.conversions import Conversions as CV + +GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2 +MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)" + + +class Column(Enum): + MAKE = "Make" + MODEL = "Model" + PACKAGE = "Supported Package" + LONGITUDINAL = "ACC" + FSR_LONGITUDINAL = "No ACC accel below" + FSR_STEERING = "No ALC below" + STEERING_TORQUE = "Steering Torque" + AUTO_RESUME = "Resume from stop" + HARNESS = "Harness" + + +class Star(Enum): + FULL = "full" + HALF = "half" + EMPTY = "empty" + + +class Harness(Enum): + nidec = "Honda Nidec" + bosch_a = "Honda Bosch A" + bosch_b = "Honda Bosch B" + toyota = "Toyota" + subaru_a = "Subaru A" + subaru_b = "Subaru B" + fca = "FCA" + ram = "Ram" + vw = "VW" + j533 = "J533" + hyundai_a = "Hyundai A" + hyundai_b = "Hyundai B" + hyundai_c = "Hyundai C" + hyundai_d = "Hyundai D" + hyundai_e = "Hyundai E" + hyundai_f = "Hyundai F" + hyundai_g = "Hyundai G" + hyundai_h = "Hyundai H" + hyundai_i = "Hyundai I" + hyundai_j = "Hyundai J" + hyundai_k = "Hyundai K" + hyundai_l = "Hyundai L" + hyundai_m = "Hyundai M" + hyundai_n = "Hyundai N" + hyundai_o = "Hyundai O" + hyundai_p = "Hyundai P" + hyundai_q = "Hyundai Q" + custom = "Developer" + obd_ii = "OBD-II" + gm = "GM" + nissan_a = "Nissan A" + nissan_b = "Nissan B" + mazda = "Mazda" + ford_q3 = "Ford Q3" + ford_q4 = "Ford Q4" + none = "None" + + +CarFootnote = namedtuple("CarFootnote", ["text", "column"], defaults=[None]) + + +def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]: + # Returns applicable footnotes given current column + return [fn for fn in footnotes if fn.value.column == column] + + +# TODO: store years as a list +def get_year_list(years): + years_list = [] + if len(years) == 0: + return years_list + + for year in years.split(','): + year = year.strip() + if len(year) == 4: + years_list.append(str(year)) + elif "-" in year and len(year) == 7: + start, end = year.split("-") + years_list.extend(map(str, range(int(start), int(f"20{end}") + 1))) + else: + raise Exception(f"Malformed year string: {years}") + return years_list + + +def split_name(name: str) -> Tuple[str, str, str]: + make, model = name.split(" ", 1) + years = "" + match = re.search(MODEL_YEARS_RE, model) + if match is not None: + years = model[match.start():] + model = model[:match.start() - 1] + return make, model, years + + +@dataclass +class CarInfo: + name: str + package: str + video_link: Optional[str] = None + footnotes: List[Enum] = field(default_factory=list) + min_steer_speed: Optional[float] = None + min_enable_speed: Optional[float] = None + harness: Enum = Harness.none + + def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): + # TODO: set all the min steer speeds in carParams and remove this + if self.min_steer_speed is not None: + assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams" + else: + self.min_steer_speed = CP.minSteerSpeed + + # TODO: set all the min enable speeds in carParams correctly and remove this + if self.min_enable_speed is None: + self.min_enable_speed = CP.minEnableSpeed + + self.car_name = CP.carName + self.car_fingerprint = CP.carFingerprint + self.make, self.model, self.years = split_name(self.name) + self.row = { + Column.MAKE: self.make, + Column.MODEL: self.model, + Column.PACKAGE: self.package, + Column.LONGITUDINAL: "openpilot" if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable else "Stock", + Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph", + Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph", + Column.STEERING_TORQUE: Star.EMPTY, + Column.AUTO_RESUME: Star.FULL if CP.autoResumeSng else Star.EMPTY, + Column.HARNESS: self.harness.value, + } + + # Set steering torque star from max lateral acceleration + assert CP.maxLateralAccel > 0.1 + if CP.maxLateralAccel >= GOOD_TORQUE_THRESHOLD: + self.row[Column.STEERING_TORQUE] = Star.FULL + + self.all_footnotes = all_footnotes + self.year_list = get_year_list(self.years) + self.detail_sentence = self.get_detail_sentence(CP) + + return self + + def get_detail_sentence(self, CP): + if not CP.notCar: + sentence_builder = "openpilot upgrades your {car_model} with automated lane centering{alc} and adaptive cruise control{acc}." + + if self.min_steer_speed > self.min_enable_speed: + alc = f" above {self.min_steer_speed * CV.MS_TO_MPH:.0f} mph," if self.min_steer_speed > 0 else " at all speeds," + else: + alc = "" + + # Exception for cars which do not auto-resume yet + acc = "" + if self.min_enable_speed > 0: + acc = f" while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph" + elif CP.autoResumeSng: + acc = " that automatically resumes from a stop" + + if self.row[Column.STEERING_TORQUE] != Star.FULL: + sentence_builder += " This car may not be able to take tight turns on its own." + + return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc) + + else: + if CP.carFingerprint == "COMMA BODY": + return "The body is a robotics dev kit that can run openpilot. Learn more." + else: + raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}") + + def get_column(self, column: Column, star_icon: str, footnote_tag: str) -> str: + item: Union[str, Star] = self.row[column] + if isinstance(item, Star): + item = star_icon.format(item.value) + elif column == Column.MODEL and len(self.years): + item += f" {self.years}" + + footnotes = get_footnotes(self.footnotes, column) + if len(footnotes): + sups = sorted([self.all_footnotes[fn] for fn in footnotes]) + item += footnote_tag.format(f'{",".join(map(str, sups))}') + + return item diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py new file mode 100755 index 00000000000000..267701509a6926 --- /dev/null +++ b/selfdrive/car/ecu_addrs.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 +import capnp +import time +import traceback +from typing import Optional, Set, Tuple + +import cereal.messaging as messaging +from panda.python.uds import SERVICE_TYPE +from selfdrive.car import make_can_msg +from selfdrive.boardd.boardd import can_list_to_can_capnp +from system.swaglog import cloudlog + + +def make_tester_present_msg(addr, bus, subaddr=None): + dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0] + if subaddr is not None: + dat.insert(0, subaddr) + + dat.extend([0x0] * (8 - len(dat))) + return make_can_msg(addr, bytes(dat), bus) + + +def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool: + # ISO-TP messages are always padded to 8 bytes + # tester present response is always a single frame + dat_offset = 1 if subaddr is not None else 0 + if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7: + # success response + if msg.dat[dat_offset + 1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40): + return True + # error response + if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == SERVICE_TYPE.TESTER_PRESENT: + return True + return False + + +def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]: + addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] + queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list} + responses = queries + return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) + + +def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]], + responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]: + ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),) + try: + msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries] + + messaging.drain_sock_raw(logcan) + sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) + start_time = time.monotonic() + while time.monotonic() - start_time < timeout: + can_packets = messaging.drain_sock(logcan, wait_for_one=True) + for packet in can_packets: + for msg in packet.can: + subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0] + if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr): + if debug: + print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}") + if (msg.address, subaddr, msg.src) in ecu_responses: + print(f"Duplicate ECU address: {hex(msg.address)}") + ecu_responses.add((msg.address, subaddr, msg.src)) + except Exception: + cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}") + return ecu_responses + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description='Get addresses of all ECUs') + parser.add_argument('--debug', action='store_true') + args = parser.parse_args() + + logcan = messaging.sub_sock('can') + sendcan = messaging.pub_sock('sendcan') + + time.sleep(1.0) + + print("Getting ECU addresses ...") + ecu_addrs = get_all_ecu_addrs(logcan, sendcan, 1, debug=args.debug) + + print() + print("Found ECUs on addresses:") + for addr, subaddr, bus in ecu_addrs: + msg = f" 0x{hex(addr)}" + if subaddr is not None: + msg += f" (sub-address: 0x{hex(subaddr)})" + print(msg) diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py new file mode 100644 index 00000000000000..1a9bb8c4e76cd2 --- /dev/null +++ b/selfdrive/car/fingerprints.py @@ -0,0 +1,48 @@ +from selfdrive.car.interfaces import get_interface_attr + + +FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True) +_FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True) + +_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes + + +def is_valid_for_fingerprint(msg, car_fingerprint): + adr = msg.address + # ignore addresses that are more than 11 bits + return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800 + + +def eliminate_incompatible_cars(msg, candidate_cars): + """Removes cars that could not have sent msg. + + Inputs: + msg: A cereal/log CanData message from the car. + candidate_cars: A list of cars to consider. + + Returns: + A list containing the subset of candidate_cars that could have sent msg. + """ + compatible_cars = [] + + for car_name in candidate_cars: + car_fingerprints = _FINGERPRINTS[car_name] + + for fingerprint in car_fingerprints: + fingerprint.update(_DEBUG_ADDRESS) # add alien debug address + + if is_valid_for_fingerprint(msg, fingerprint): + compatible_cars.append(car_name) + break + + return compatible_cars + + +def all_known_cars(): + """Returns a list of all known car strings.""" + return list({*FW_VERSIONS.keys(), *_FINGERPRINTS.keys()}) + + +def all_legacy_fingerprint_cars(): + """Returns a list of all known car strings, FPv1 only.""" + return list(_FINGERPRINTS.keys()) diff --git a/system/athena/__init__.py b/selfdrive/car/ford/__init__.py similarity index 100% rename from system/athena/__init__.py rename to selfdrive/car/ford/__init__.py diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py new file mode 100644 index 00000000000000..592d8586ca29b0 --- /dev/null +++ b/selfdrive/car/ford/carcontroller.py @@ -0,0 +1,105 @@ +import math +from cereal import car +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.ford import fordcan +from selfdrive.car.ford.values import CarControllerParams + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): + # rate limit + steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN + max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) + apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) + + # absolute limit (LatCtlPath_An_Actl) + apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO + apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240) + apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO + + return apply_angle + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.VM = VM + self.packer = CANPacker(dbc_name) + self.frame = 0 + + self.apply_angle_last = 0 + self.main_on_last = False + self.lkas_enabled_last = False + self.steer_alert_last = False + + def update(self, CC, CS): + can_sends = [] + + actuators = CC.actuators + hud_control = CC.hudControl + + main_on = CS.out.cruiseState.available + steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + + ### acc buttons ### + if CC.cruiseControl.cancel: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True)) + elif CC.cruiseControl.resume: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True)) + + # if stock lane centering is active or in standby, toggle it off + # the stock system checks for steering pressed, and eventually disengages cruise control + if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0: + can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True)) + + + ### lateral control ### + if CC.latActive: + apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) + else: + apply_angle = CS.out.steeringAngleDeg + + # send steering commands at 20Hz + if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: + lca_rq = 1 if CC.latActive else 0 + + # use LatCtlPath_An_Actl to actuate steering + # path angle is the car wheel angle, not the steering wheel angle + path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO + + # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately + # TODO: try slower ramp speed when driver torque detected + ramp_type = 3 + precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) + + offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094) + + self.apply_angle_last = apply_angle + can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0)) + can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, + 0, path_angle, 0, offset_roll_compensation_curvature)) + + + ### ui ### + send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) + + # send lkas ui command at 1Hz or if ui state changes + if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: + can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + + # send acc ui command at 20Hz or if ui state changes + if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: + can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values)) + + self.main_on_last = main_on + self.lkas_enabled_last = CC.latActive + self.steer_alert_last = steer_alert + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py new file mode 100644 index 00000000000000..a7ea19effcd5fa --- /dev/null +++ b/selfdrive/car/ford/carstate.py @@ -0,0 +1,257 @@ +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams + +GearShifter = car.CarState.GearShifter +TransmissionType = car.CarParams.TransmissionType + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + # car speed + ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] + ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 + + # gas pedal + ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. + ret.gasPressed = ret.gas > 1e-6 + + # brake pedal + ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm + ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2 + ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2) + + # steering wheel + ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] + ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] + ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 + ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) + # ret.espDisabled = False # TODO: find traction control signal + + # cruise state + ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS + ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) + ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) + ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 + ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3 + + # gear + if self.CP.transmissionType == TransmissionType.automatic: + gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None) + ret.gearShifter = self.parse_gear_shifter(gear) + elif self.CP.transmissionType == TransmissionType.manual: + ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 + if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]): + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.drive + + # safety + ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) + ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled + + # button presses + ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 + ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 + # TODO: block this going to the camera otherwise it will enable stock TJA + ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + + # lock info + ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], + cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]]) + ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2 + + # blindspot sensors + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 + ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 + + # Stock steering buttons so that we can passthru blinkers etc. + self.buttons_stock_values = cp.vl["Steering_Data_FD1"] + # Stock values from IPMA so that we can retain some stock functionality + self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] + self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + # Use stock sensor values + self.yaw_data = cp.vl["Yaw_Data_FD1"] + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) + ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) + ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped + ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status + ("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct) + ("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm) + ("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed + ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) + # The units might change with IPC settings? + ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status + ("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill + ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable + ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) + # Calculates steering angle (and offset) from pinion + # angle and driving measurements. + # StePinRelInit_An_Sns is the pinion angle, initialised + # to zero at the beginning of the drive. + ("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) + ("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status + ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch + ("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle + ("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver + ("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger + ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left + ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right + ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver + ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons + ("WiprFront_D_Stat", "Steering_Data_FD1"), + ("LghtAmb_D_Sns", "Steering_Data_FD1"), + ("AccButtnGapDecPress", "Steering_Data_FD1"), + ("AccButtnGapIncPress", "Steering_Data_FD1"), + ("AslButtnOnOffCnclPress", "Steering_Data_FD1"), + ("AslButtnOnOffPress", "Steering_Data_FD1"), + ("CcAslButtnCnclPress", "Steering_Data_FD1"), + ("LaSwtchPos_D_Stat", "Steering_Data_FD1"), + ("CcAslButtnCnclResPress", "Steering_Data_FD1"), + ("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"), + ("CcAslButtnIndxDecPress", "Steering_Data_FD1"), + ("CcAslButtnIndxIncPress", "Steering_Data_FD1"), + ("CcAslButtnOffCnclPress", "Steering_Data_FD1"), + ("CcAslButtnOnOffCncl", "Steering_Data_FD1"), + ("CcAslButtnOnPress", "Steering_Data_FD1"), + ("CcAslButtnResDecPress", "Steering_Data_FD1"), + ("CcAslButtnResIncPress", "Steering_Data_FD1"), + ("CcAslButtnSetDecPress", "Steering_Data_FD1"), + ("CcAslButtnSetIncPress", "Steering_Data_FD1"), + ("CcAslButtnSetPress", "Steering_Data_FD1"), + ("CcAsllButtnResPress", "Steering_Data_FD1"), + ("CcButtnOffPress", "Steering_Data_FD1"), + ("CcButtnOnOffCnclPress", "Steering_Data_FD1"), + ("CcButtnOnOffPress", "Steering_Data_FD1"), + ("CcButtnOnPress", "Steering_Data_FD1"), + ("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"), + ("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"), + ("AhbStat_B_Dsply", "Steering_Data_FD1"), + ("AccButtnGapTogglePress", "Steering_Data_FD1"), + ("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"), + ("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"), + ] + + checks = [ + # sig_address, frequency + ("EngVehicleSpThrottle2", 50), + ("Yaw_Data_FD1", 100), + ("DesiredTorqBrk", 50), + ("EngVehicleSpThrottle", 100), + ("BrakeSnData_4", 50), + ("EngBrakeData", 10), + ("Cluster_Info1_FD1", 10), + ("SteeringPinion_Data", 100), + ("EPAS_INFO", 50), + ("Lane_Assist_Data3_FD1", 33), + ("Steering_Data_FD1", 10), + ("BodyInfo_3_FD1", 2), + ("RCMStatusMessage2_FD1", 10), + ] + + if CP.transmissionType == TransmissionType.automatic: + signals += [ + ("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position + ] + checks += [ + ("Gear_Shift_by_Wire_FD1", 10), + ] + elif CP.transmissionType == TransmissionType.manual: + signals += [ + ("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct) + ("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light + ] + checks += [ + ("Engine_Clutch_Data", 33), + ("BCM_Lamp_Stat_FD1", 1), + ] + + if CP.enableBsm: + signals += [ + ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left + ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right + ] + checks += [ + ("Side_Detect_L_Stat", 5), + ("Side_Detect_R_Stat", 5), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address + ("HaDsply_No_Cs", "ACCDATA_3"), + ("HaDsply_No_Cnt", "ACCDATA_3"), + ("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message + ("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance + ("AccStopRes_B_Dsply", "ACCDATA_3"), + ("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning + ("Tja_D_Stat", "ACCDATA_3"), # TJA status + ("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text + ("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon + ("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text + ("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled + ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting + ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting + ("CadsAlignIncplt_B_Actl", "ACCDATA_3"), + ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting + ("CadsRadrBlck_B_Actl", "ACCDATA_3"), + ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status + ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting + ("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting + ("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text + ("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning + ("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert + ("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert + ("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap + ("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting + ("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting + + ("FeatConfigIpmaActl", "IPMA_Data"), + ("FeatNoIpmaActl", "IPMA_Data"), + ("PersIndexIpma_D_Actl", "IPMA_Data"), + ("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping + ("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) + ("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error + ("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime + ("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? + ("CamraStats_D_Dsply", "IPMA_Data"), # Camera status + ("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level + ("DasStats_D_Dsply", "IPMA_Data"), # DAS status + ("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning + ("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status + ("Passthru_63", "IPMA_Data"), + ("Passthru_48", "IPMA_Data"), + ] + + checks = [ + # sig_address, frequency + ("ACCDATA_3", 5), + ("IPMA_Data", 1), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py new file mode 100644 index 00000000000000..b42561df21a707 --- /dev/null +++ b/selfdrive/car/ford/fordcan.py @@ -0,0 +1,157 @@ +from cereal import car +from selfdrive.car.ford.values import CANBUS + +HUDControl = car.CarControl.HUDControl + + +def create_lka_command(packer, angle_deg: float, curvature: float): + """ + Creates a CAN message for the Ford LKAS Command. + + This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the + PSCM lockout. + + Frequency is 20Hz. + """ + + values = { + "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] + "LkaActvStats_D2_Req": 0, # action [0|7] + "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees + "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick + "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter + "LdwActvStats_D_Req": 0, # LDW status [0|7] + "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength + } + return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values) + + +def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): + """ + Creates a CAN message for the Ford TJA/LCA Command. + + This command can apply "Lane Centering" manoeuvres: continuous lane centering + for traffic jam assist and highway driving. It is not subject to the PSCM + lockout. + + The PSCM should be configured to accept TJA/LCA commands before these + commands will be processed. This can be done using tools such as Forscan. + + Frequency is 20Hz. + """ + + values = { + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": path_angle, # Path angle [-0.4995|0.5240] radians + "LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter + } + return packer.make_can_msg("LateralMotionControl", CANBUS.main, values) + + +def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict): + """ + Creates a CAN message for the Ford IPC IPMA/LKAS status. + + Show the LKAS status with the "driver assist" lines in the IPC. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 1Hz. + """ + + # LaActvStats_D_Dsply + # R Intvn Warn Supprs Avail No + # L + # Intvn 24 19 14 9 4 + # Warn 23 18 13 8 3 + # Supprs 22 17 12 7 2 + # Avail 21 16 11 6 1 + # No 20 15 10 5 0 + # + # TODO: test suppress state + if enabled: + lines = 0 # NoLeft_NoRight + if hud_control.leftLaneDepart: + lines += 4 + elif hud_control.leftLaneVisible: + lines += 1 + if hud_control.rightLaneDepart: + lines += 20 + elif hud_control.rightLaneVisible: + lines += 5 + elif main_on: + lines = 0 + else: + if hud_control.leftLaneDepart: + lines = 3 # WarnLeft_NoRight + elif hud_control.rightLaneDepart: + lines = 15 # NoLeft_WarnRight + else: + lines = 30 # LA_Off + + # TODO: use level 1 for no sound when less severe? + hands_on_wheel_dsply = 2 if steer_alert else 0 + + values = { + **stock_values, + "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] + "LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed + } + return packer.make_can_msg("IPMA_Data", CANBUS.main, values) + + +def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict): + """ + Creates a CAN message for the Ford IPC adaptive cruise, forward collision + warning and traffic jam assist status. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 20Hz. + """ + + # Tja_D_Stat + if enabled: + if hud_control.leftLaneDepart: + status = 3 # ActiveInterventionLeft + elif hud_control.rightLaneDepart: + status = 4 # ActiveInterventionRight + else: + status = 2 # Active + elif main_on: + if hud_control.leftLaneDepart: + status = 5 # ActiveWarningLeft + elif hud_control.rightLaneDepart: + status = 6 # ActiveWarningRight + else: + status = 1 # Standby + else: + status = 0 # Off + + values = { + **stock_values, + "Tja_D_Stat": status, + } + return packer.make_can_msg("ACCDATA_3", CANBUS.main, values) + + +def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera): + """ + Creates a CAN message for the Ford SCCM buttons/switches. + + Includes cruise control buttons, turn lights and more. + """ + + values = { + **stock_values, + "CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button + "CcAsllButtnResPress": 1 if resume else 0, # CC resume button + "TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button + } + return packer.make_can_msg("Steering_Data_FD1", bus, values) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py new file mode 100644 index 00000000000000..7d4c9eb94c8d13 --- /dev/null +++ b/selfdrive/car/ford/interface.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter +from selfdrive.car.interfaces import CarInterfaceBase + + +EventName = car.CarEvent.EventName + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + if car_fw is None: + car_fw = [] + + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "ford" + ret.dashcamOnly = True + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] + + # Angle-based steering + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.steerActuatorDelay = 0.4 + ret.steerLimitTimer = 1.0 + tire_stiffness_factor = 1.0 + + if candidate == CAR.ESCAPE_MK4: + ret.wheelbase = 2.71 + ret.steerRatio = 14.3 # Copied from Focus + ret.mass = 1750 + STD_CARGO_KG + + elif candidate == CAR.EXPLORER_MK6: + ret.wheelbase = 3.025 + ret.steerRatio = 16.8 # learned + ret.mass = 2050 + STD_CARGO_KG + + elif candidate == CAR.FOCUS_MK4: + ret.wheelbase = 2.7 + ret.steerRatio = 13.8 # learned + ret.mass = 1350 + STD_CARGO_KG + + else: + raise ValueError(f"Unsupported car: ${candidate}") + + # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 + found_ecus = [fw.ecu for fw in car_fw] + if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]: + ret.transmissionType = TransmissionType.automatic + else: + ret.transmissionType = TransmissionType.manual + ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS + + # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat + # TODO: detect bsm in car_fw? + ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] + + # LCA can steer down to zero + ret.minSteerSpeed = 0. + + ret.autoResumeSng = ret.minEnableSpeed == -1. + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.44 + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + return ret + + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py new file mode 100644 index 00000000000000..c942703002ff29 --- /dev/null +++ b/selfdrive/car/ford/radar_interface.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +from math import cos, sin +from cereal import car +from opendbc.can.parser import CANParser +from common.conversions import Conversions as CV +from selfdrive.car.ford.values import CANBUS, DBC, RADAR +from selfdrive.car.interfaces import RadarInterfaceBase + +DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540)) + +DELPHI_MRR_RADAR_START_ADDR = 0x120 +DELPHI_MRR_RADAR_MSG_COUNT = 64 + + +def _create_delphi_esr_radar_can_parser(): + msg_n = len(DELPHI_ESR_RADAR_MSGS) + signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, + DELPHI_ESR_RADAR_MSGS * 3)) + checks = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n)) + + return CANParser(RADAR.DELPHI_ESR, signals, checks, CANBUS.radar) + + +def _create_delphi_mrr_radar_can_parser(): + signals = [] + checks = [] + + for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1): + msg = f"MRR_Detection_{i:03d}" + signals += [ + (f"CAN_DET_VALID_LEVEL_{i:02d}", msg), + (f"CAN_DET_AZIMUTH_{i:02d}", msg), + (f"CAN_DET_RANGE_{i:02d}", msg), + (f"CAN_DET_RANGE_RATE_{i:02d}", msg), + (f"CAN_DET_AMPLITUDE_{i:02d}", msg), + (f"CAN_SCAN_INDEX_2LSB_{i:02d}", msg), + ] + checks += [(msg, 20)] + + return CANParser(RADAR.DELPHI_MRR, signals, checks, CANBUS.radar) + + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + + self.updated_messages = set() + self.track_id = 0 + self.radar = DBC[CP.carFingerprint]['radar'] + if self.radar is None: + self.rcp = None + elif self.radar == RADAR.DELPHI_ESR: + self.rcp = _create_delphi_esr_radar_can_parser() + self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1] + self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS} + elif self.radar == RADAR.DELPHI_MRR: + self.rcp = _create_delphi_mrr_radar_can_parser() + self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1 + else: + raise ValueError(f"Unsupported radar: {self.radar}") + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + if self.radar == RADAR.DELPHI_ESR: + self._update_delphi_esr() + elif self.radar == RADAR.DELPHI_MRR: + self._update_delphi_mrr() + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret + + def _update_delphi_esr(self): + for ii in sorted(self.updated_messages): + cpt = self.rcp.vl[ii] + + if cpt['X_Rel'] > 0.00001: + self.valid_cnt[ii] = 0 # reset counter + + if cpt['X_Rel'] > 0.00001: + self.valid_cnt[ii] += 1 + else: + self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) + #print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle'] + + # radar point only valid if there have been enough valid measurements + if self.valid_cnt[ii] > 0: + if ii not in self.pts: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['X_Rel'] # from front of car + self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['V_Rel'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + else: + if ii in self.pts: + del self.pts[ii] + + def _update_delphi_mrr(self): + for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1): + msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"] + + # SCAN_INDEX rotates through 0..3 on each message + # treat these as separate points + scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"] + i = (ii - 1) * 4 + scanIndex + + if i not in self.pts: + self.pts[i] = car.RadarData.RadarPoint.new_message() + self.pts[i].trackId = self.track_id + self.pts[i].aRel = float('nan') + self.pts[i].yvRel = float('nan') + self.track_id += 1 + + valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"]) + amplitude = msg[f"CAN_DET_AMPLITUDE_{ii:02d}"] # dBsm [-64|63] + + if valid and 0 < amplitude <= 15: + azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964] + dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984] + distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984] + + # *** openpilot radar point *** + self.pts[i].dRel = cos(azimuth) * dist # m from front of car + self.pts[i].yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive + self.pts[i].vRel = distRate # m/s + + self.pts[i].measured = True + + else: + del self.pts[i] diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py new file mode 100644 index 00000000000000..5820b5c9fd3bcd --- /dev/null +++ b/selfdrive/car/ford/values.py @@ -0,0 +1,152 @@ +from collections import namedtuple +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu +TransmissionType = car.CarParams.TransmissionType +GearShifter = car.CarState.GearShifter + +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) + + +class CarControllerParams: + # Messages: Lane_Assist_Data1, LateralMotionControl + LKAS_STEER_STEP = 5 + # Message: IPMA_Data + LKAS_UI_STEP = 100 + # Message: ACCDATA_3 + ACC_UI_STEP = 5 + + STEER_RATIO = 2.75 + STEER_DRIVER_ALLOWANCE = 0.8 + + RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + + +class RADAR: + DELPHI_ESR = 'ford_fusion_2018_adas' + DELPHI_MRR = 'FORD_CADS' + + +class CANBUS: + main = 0 + radar = 1 + camera = 2 + + +class CAR: + ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" + EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" + FOCUS_MK4 = "FORD FOCUS 4TH GEN" + + +@dataclass +class FordCarInfo(CarInfo): + package: str = "Co-Pilot360 Assist+" + harness: Enum = Harness.ford_q3 + + +CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { + CAR.ESCAPE_MK4: [ + FordCarInfo("Ford Escape 2020"), + FordCarInfo("Ford Kuga EU", "Driver Assistance Pack"), + ], + CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-21"), + CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"), +} + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine], + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + bus=0, + whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], + ), + ], +) + +FW_VERSIONS = { + CAR.ESCAPE_MK4: { + (Ecu.eps, 0x730, None): [ + b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + ], + }, + CAR.EXPLORER_MK6: { + (Ecu.eps, 0x730, None): [ + b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.FOCUS_MK4: { + (Ecu.eps, 0x730, None): [ + b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.shiftByWire, 0x732, None): [ + ], + }, +} + + +DBC = { + CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), + CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), + CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR), +} diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py new file mode 100755 index 00000000000000..c3b74da92049e5 --- /dev/null +++ b/selfdrive/car/fw_query_definitions.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import capnp +from dataclasses import dataclass, field +import struct +from typing import Dict, List + +import panda.python.uds as uds + + +def p16(val): + return struct.pack("!H", val) + + +class StdQueries: + # FW queries + TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0]) + TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0]) + + SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT]) + SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) + + DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.DEFAULT]) + DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) + + EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) + EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) + + MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + + OBD_VERSION_REQUEST = b'\x09\x04' + OBD_VERSION_RESPONSE = b'\x49\x04' + + # VIN queries + OBD_VIN_REQUEST = b'\x09\x02' + OBD_VIN_RESPONSE = b'\x49\x02\x01' + + UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + + +@dataclass +class Request: + request: List[bytes] + response: List[bytes] + whitelist_ecus: List[int] = field(default_factory=list) + rx_offset: int = 0x8 + bus: int = 1 + + +@dataclass +class FwQueryConfig: + requests: List[Request] + # Overrides and removes from essential ecus for specific models and ecus (exact matching) + non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py new file mode 100755 index 00000000000000..9c0c406f14991c --- /dev/null +++ b/selfdrive/car/fw_versions.py @@ -0,0 +1,327 @@ +#!/usr/bin/env python3 +import traceback +from collections import defaultdict +from typing import Any, Optional, Set, Tuple +from tqdm import tqdm + +import panda.python.uds as uds +from cereal import car +from selfdrive.car.ecu_addrs import get_ecu_addrs +from selfdrive.car.interfaces import get_interface_attr +from selfdrive.car.fingerprints import FW_VERSIONS +from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from system.swaglog import cloudlog + +Ecu = car.CarParams.Ecu +ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] + +FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True) +VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) + +MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e} +REQUESTS = [(brand, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests] + + +def chunks(l, n=128): + for i in range(0, len(l), n): + yield l[i:i + n] + + +def build_fw_dict(fw_versions, filter_brand=None): + fw_versions_dict = defaultdict(set) + for fw in fw_versions: + if filter_brand is None or fw.brand == filter_brand: + addr = fw.address + sub_addr = fw.subAddress if fw.subAddress != 0 else None + fw_versions_dict[(addr, sub_addr)].add(fw.fwVersion) + return dict(fw_versions_dict) + + +def get_brand_addrs(): + brand_addrs = defaultdict(set) + for brand, cars in VERSIONS.items(): + for fw in cars.values(): + brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()} + return brand_addrs + + +def match_fw_to_car_fuzzy(fw_versions_dict, log=True, exclude=None): + """Do a fuzzy FW match. This function will return a match, and the number of firmware version + that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars + the match is rejected.""" + + # These ECUs are known to be shared between models (EPS only between hybrid/ICE version) + # Getting this exactly right isn't crucial, but excluding camera and radar makes it almost + # impossible to get 3 matching versions, even if two models with shared parts are released at the same + # time and only one is in our database. + exclude_types = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps, Ecu.debug] + + # Build lookup table from (addr, sub_addr, fw) to list of candidate cars + all_fw_versions = defaultdict(list) + for candidate, fw_by_addr in FW_VERSIONS.items(): + if candidate == exclude: + continue + + for addr, fws in fw_by_addr.items(): + if addr[0] in exclude_types: + continue + for f in fws: + all_fw_versions[(addr[1], addr[2], f)].append(candidate) + + match_count = 0 + candidate = None + for addr, versions in fw_versions_dict.items(): + for version in versions: + # All cars that have this FW response on the specified address + candidates = all_fw_versions[(addr[0], addr[1], version)] + + if len(candidates) == 1: + match_count += 1 + if candidate is None: + candidate = candidates[0] + # We uniquely matched two different cars. No fuzzy match possible + elif candidate != candidates[0]: + return set() + + if match_count >= 2: + if log: + cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {match_count} matching ECUs") + return {candidate} + else: + return set() + + +def match_fw_to_car_exact(fw_versions_dict): + """Do an exact FW match. Returns all cars that match the given + FW versions for a list of "essential" ECUs. If an ECU is not considered + essential the FW version can be missing to get a fingerprint, but if it's present it + needs to match the database.""" + invalid = [] + candidates = FW_VERSIONS + + for candidate, fws in candidates.items(): + for ecu, expected_versions in fws.items(): + config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]] + ecu_type = ecu[0] + addr = ecu[1:] + + found_versions = fw_versions_dict.get(addr, set()) + if not len(found_versions): + # Some models can sometimes miss an ecu, or show on two different addresses + if candidate in config.non_essential_ecus.get(ecu_type, []): + continue + + # Ignore non essential ecus + if ecu_type not in ESSENTIAL_ECUS: + continue + + # Virtual debug ecu doesn't need to match the database + if ecu_type == Ecu.debug: + continue + + if not any([found_version in expected_versions for found_version in found_versions]): + invalid.append(candidate) + break + + return set(candidates.keys()) - set(invalid) + + +def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): + # Try exact matching first + exact_matches = [] + if allow_exact: + exact_matches = [(True, match_fw_to_car_exact)] + if allow_fuzzy: + exact_matches.append((False, match_fw_to_car_fuzzy)) + + for exact_match, match_func in exact_matches: + # For each brand, attempt to fingerprint using all FW returned from its queries + matches = set() + for brand in VERSIONS.keys(): + fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand) + matches |= match_func(fw_versions_dict) + + if len(matches): + return exact_match, matches + + return True, set() + + +def get_present_ecus(logcan, sendcan): + queries = list() + parallel_queries = list() + responses = set() + + for brand, r in REQUESTS: + for brand_versions in VERSIONS[brand].values(): + for ecu_type, addr, sub_addr in brand_versions: + # Only query ecus in whitelist if whitelist is not empty + if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus: + a = (addr, sub_addr, r.bus) + # Build set of queries + if sub_addr is None: + if a not in parallel_queries: + parallel_queries.append(a) + else: # subaddresses must be queried one by one + if [a] not in queries: + queries.append([a]) + + # Build set of expected responses to filter + response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset) + responses.add((response_addr, sub_addr, r.bus)) + + queries.insert(0, parallel_queries) + + ecu_responses: Set[Tuple[int, Optional[int], int]] = set() + for query in queries: + ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) + return ecu_responses + + +def get_brand_ecu_matches(ecu_rx_addrs): + """Returns dictionary of brands and matches with ECUs in their FW versions""" + + brand_addrs = get_brand_addrs() + brand_matches = {brand: set() for brand, _ in REQUESTS} + + brand_rx_offsets = set((brand, r.rx_offset) for brand, r in REQUESTS) + for addr, sub_addr, _ in ecu_rx_addrs: + # Since we can't know what request an ecu responded to, add matches for all possible rx offsets + for brand, rx_offset in brand_rx_offsets: + a = (uds.get_rx_addr_for_tx_addr(addr, -rx_offset), sub_addr) + if a in brand_addrs[brand]: + brand_matches[brand].add(a) + + return brand_matches + + +def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, debug=False, progress=False): + """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" + + all_car_fw = [] + brand_matches = get_brand_ecu_matches(ecu_rx_addrs) + + for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True): + car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, debug=debug, progress=progress) + all_car_fw.extend(car_fw) + # Try to match using FW returned from this brand only + matches = match_fw_to_car_exact(build_fw_dict(car_fw)) + if len(matches) == 1: + break + + return all_car_fw + + +def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, debug=False, progress=False): + versions = VERSIONS.copy() + if query_brand is not None: + versions = {query_brand: versions[query_brand]} + + if extra is not None: + versions.update(extra) + + # Extract ECU addresses to query from fingerprints + # ECUs using a subaddress need be queried one by one, the rest can be done in parallel + addrs = [] + parallel_addrs = [] + ecu_types = {} + + for brand, brand_versions in versions.items(): + for c in brand_versions.values(): + for ecu_type, addr, sub_addr in c.keys(): + a = (brand, addr, sub_addr) + if a not in ecu_types: + ecu_types[a] = ecu_type + + if sub_addr is None: + if a not in parallel_addrs: + parallel_addrs.append(a) + else: + if [a] not in addrs: + addrs.append([a]) + + addrs.insert(0, parallel_addrs) + + # Get versions and build capnp list to put into CarParams + car_fw = [] + requests = [(brand, r) for brand, r in REQUESTS if query_brand is None or brand == query_brand] + for addr in tqdm(addrs, disable=not progress): + for addr_chunk in chunks(addr): + for brand, r in requests: + try: + addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and + (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] + + if addrs: + query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug) + for (addr, rx_addr), version in query.get_data(timeout).items(): + f = car.CarParams.CarFw.new_message() + + f.ecu = ecu_types.get((brand, addr[0], addr[1]), Ecu.unknown) + f.fwVersion = version + f.address = addr[0] + f.responseAddress = rx_addr + f.request = r.request + f.brand = brand + f.bus = r.bus + + if addr[1] is not None: + f.subAddress = addr[1] + + car_fw.append(f) + except Exception: + cloudlog.warning(f"FW query exception: {traceback.format_exc()}") + + return car_fw + + +if __name__ == "__main__": + import time + import argparse + import cereal.messaging as messaging + from selfdrive.car.vin import get_vin + + parser = argparse.ArgumentParser(description='Get firmware version of ECUs') + parser.add_argument('--scan', action='store_true') + parser.add_argument('--debug', action='store_true') + parser.add_argument('--brand', help='Only query addresses/with requests for this brand') + args = parser.parse_args() + + logcan = messaging.sub_sock('can') + sendcan = messaging.pub_sock('sendcan') + + extra: Any = None + if args.scan: + extra = {} + # Honda + for i in range(256): + extra[(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = [] + extra[(Ecu.unknown, 0x700 + i, None)] = [] + extra[(Ecu.unknown, 0x750, i)] = [] + extra = {"any": {"debug": extra}} + + time.sleep(1.) + + t = time.time() + print("Getting vin...") + addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug) + print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}') + print(f"Getting VIN took {time.time() - t:.3f} s") + print() + + t = time.time() + fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, debug=args.debug, progress=True) + _, candidates = match_fw_to_car(fw_vers) + + print() + print("Found FW versions") + print("{") + padding = max([len(fw.brand) for fw in fw_vers] or [0]) + for version in fw_vers: + subaddr = None if version.subAddress == 0 else hex(version.subAddress) + print(f" Brand: {version.brand:{padding}}, bus: {version.bus} - (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]") + print("}") + + print() + print("Possible matches:", candidates) + print(f"Getting fw took {time.time() - t:.3f} s") diff --git a/system/athena/tests/__init__.py b/selfdrive/car/gm/__init__.py similarity index 100% rename from system/athena/tests/__init__.py rename to selfdrive/car/gm/__init__.py diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py new file mode 100644 index 00000000000000..977d20c5b3c2e1 --- /dev/null +++ b/selfdrive/car/gm/carcontroller.py @@ -0,0 +1,137 @@ +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import interp +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.gm import gmcan +from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, EV_CAR + +VisualAlert = car.CarControl.HUDControl.VisualAlert +NetworkLocation = car.CarParams.NetworkLocation + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.start_time = 0. + self.apply_steer_last = 0 + self.apply_gas = 0 + self.apply_brake = 0 + self.frame = 0 + self.last_button_frame = 0 + + self.lka_steering_cmd_counter_last = -1 + self.lka_icon_status_last = (False, False) + + self.params = CarControllerParams() + + self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt']) + self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) + self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + hud_alert = hud_control.visualAlert + hud_v_cruise = hud_control.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 + + # Send CAN commands. + can_sends = [] + + # Steering (50Hz) + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the + # next Panda loopback confirmation in the current CS frame. + if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: + self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter + elif (self.frame % self.params.STEER_STEP) == 0: + if CC.latActive: + new_steer = int(round(actuators.steer * self.params.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + else: + apply_steer = 0 + + self.apply_steer_last = apply_steer + # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the + # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. + idx = (CS.lka_steering_cmd_counter + 1) % 4 + + can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) + + if self.CP.openpilotLongitudinalControl: + # Gas/regen, brakes, and UI commands - all at 25Hz + if self.frame % 4 == 0: + if not CC.longActive: + # Stock ECU sends max regen when not enabled + self.apply_gas = self.params.MAX_ACC_REGEN + self.apply_brake = 0 + else: + if self.CP.carFingerprint in EV_CAR: + self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + else: + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + + idx = (self.frame // 4) % 4 + + at_full_stop = CC.longActive and CS.out.standstill + near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) + # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) + + # Send dashboard UI commands (ACC status) + send_fcw = hud_alert == VisualAlert.fcw + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, + hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) + + # Radar needs to know current speed and yaw rate (50hz), + # and that ADAS is alive (10hz) + if not self.CP.radarOffCan: + tt = self.frame * DT_CTRL + time_and_headlights_step = 10 + if self.frame % time_and_headlights_step == 0: + idx = (self.frame // time_and_headlights_step) % 4 + can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) + + speed_and_accelerometer_step = 2 + if self.frame % speed_and_accelerometer_step == 0: + idx = (self.frame // speed_and_accelerometer_step) % 4 + can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) + + if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: + can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) + + else: + # Stock longitudinal, integrated at camera + if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: + if CC.cruiseControl.cancel: + self.last_button_frame = self.frame + can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL)) + + # Show green icon when LKA torque is applied, and + # alarming orange icon when approaching torque limit. + # If not sent again, LKA icon disappears in about 5 seconds. + # Conveniently, sending camera message periodically also works as a keepalive. + lka_active = CS.lkas_status == 1 + lka_critical = lka_active and abs(actuators.steer) > 0.9 + lka_icon_status = (lka_active, lka_critical) + + # SW_GMLAN not yet on cam harness, no HUD alerts + if self.CP.networkLocation != NetworkLocation.fwdCamera and (self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last): + steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) + can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) + self.lka_icon_status_last = lka_icon_status + + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.gas = self.apply_gas + new_actuators.brake = self.apply_brake + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py new file mode 100644 index 00000000000000..0bba1d29b8c01f --- /dev/null +++ b/selfdrive/car/gm/carstate.py @@ -0,0 +1,165 @@ +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import mean +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD + +TransmissionType = car.CarParams.TransmissionType +NetworkLocation = car.CarParams.NetworkLocation + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] + self.lka_steering_cmd_counter = 0 + self.buttons_counter = 0 + + def update(self, pt_cp, cam_cp, loopback_cp): + ret = car.CarState.new_message() + + self.prev_cruise_buttons = self.cruise_buttons + self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] + self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] + + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], + pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], + pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"], + pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"], + ) + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 + + if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: + ret.gearShifter = self.parse_gear_shifter("T") + else: + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) + + # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. + ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 + ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 + + # Regen braking is braking + if self.CP.transmissionType == TransmissionType.direct: + ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 + + ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. + ret.gasPressed = ret.gas > 1e-5 + + ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] + ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] + ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] + ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + + # 0 inactive, 1 active, 2 temporarily limited, 3 failed + self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] + ret.steerFaultTemporary = self.lkas_status == 2 + ret.steerFaultPermanent = self.lkas_status == 3 + + # 1 - open, 0 - closed + ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1) + + # 1 - latched + ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 + ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 + ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 + + ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 + ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 + ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 + ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED + + ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF + ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL + if self.CP.networkLocation == NetworkLocation.fwdCamera: + ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS + + return ret + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [] + if CP.networkLocation == NetworkLocation.fwdCamera: + signals.append(("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus")) + checks.append(("ASCMActiveCruiseControlStatus", 25)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.CAMERA) + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("FrontLeftDoor", "BCMDoorBeltStatus"), + ("FrontRightDoor", "BCMDoorBeltStatus"), + ("RearLeftDoor", "BCMDoorBeltStatus"), + ("RearRightDoor", "BCMDoorBeltStatus"), + ("LeftSeatBelt", "BCMDoorBeltStatus"), + ("RightSeatBelt", "BCMDoorBeltStatus"), + ("TurnSignals", "BCMTurnSignals"), + ("AcceleratorPedal2", "AcceleratorPedal2"), + ("CruiseState", "AcceleratorPedal2"), + ("ACCButtons", "ASCMSteeringButton"), + ("RollingCounter", "ASCMSteeringButton"), + ("SteeringWheelAngle", "PSCMSteeringAngle"), + ("SteeringWheelRate", "PSCMSteeringAngle"), + ("FLWheelSpd", "EBCMWheelSpdFront"), + ("FRWheelSpd", "EBCMWheelSpdFront"), + ("RLWheelSpd", "EBCMWheelSpdRear"), + ("RRWheelSpd", "EBCMWheelSpdRear"), + ("PRNDL2", "ECMPRDNL2"), + ("ManualMode", "ECMPRDNL2"), + ("LKADriverAppldTrq", "PSCMStatus"), + ("LKATorqueDelivered", "PSCMStatus"), + ("LKATorqueDeliveredStatus", "PSCMStatus"), + ("TractionControlOn", "ESPStatus"), + ("ParkBrake", "VehicleIgnitionAlt"), + ("CruiseMainOn", "ECMEngineStatus"), + ] + + checks = [ + ("BCMTurnSignals", 1), + ("ECMPRDNL2", 10), + ("PSCMStatus", 10), + ("ESPStatus", 10), + ("BCMDoorBeltStatus", 10), + ("VehicleIgnitionAlt", 10), + ("EBCMWheelSpdFront", 20), + ("EBCMWheelSpdRear", 20), + ("AcceleratorPedal2", 33), + ("ASCMSteeringButton", 33), + ("ECMEngineStatus", 100), + ("PSCMSteeringAngle", 100), + ("EBCMBrakePedalPosition", 100), + ] + + if CP.transmissionType == TransmissionType.direct: + signals.append(("RegenPaddle", "EBCMRegenPaddle")) + checks.append(("EBCMRegenPaddle", 50)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) + + @staticmethod + def get_loopback_can_parser(CP): + signals = [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + + checks = [ + ("ASCMLKASteeringCmd", 10), # 10 Hz is the stock inactive rate (every 100ms). + # While active 50 Hz (every 20 ms) is normal + # EPS will tolerate around 200ms when active before faulting + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py new file mode 100644 index 00000000000000..20e4c4ab6efb69 --- /dev/null +++ b/selfdrive/car/gm/gmcan.py @@ -0,0 +1,131 @@ +from selfdrive.car import make_can_msg + +def create_buttons(packer, bus, idx, button): + values = { + "ACCButtons": button, + "RollingCounter": idx, + } + return packer.make_can_msg("ASCMSteeringButton", bus, values) + +def create_steering_control(packer, bus, apply_steer, idx, lkas_active): + + values = { + "LKASteeringCmdActive": lkas_active, + "LKASteeringCmd": apply_steer, + "RollingCounter": idx, + "LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx + } + + return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) + +def create_adas_keepalive(bus): + dat = b"\x00\x00\x00\x00\x00\x00\x00" + return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] + +def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): + values = { + "GasRegenCmdActive": acc_engaged, + "RollingCounter": idx, + "GasRegenCmdActiveInv": 1 - acc_engaged, + "GasRegenCmd": throttle, + "GasRegenFullStopActive": at_full_stop, + "GasRegenAlwaysOne": 1, + "GasRegenAlwaysOne2": 1, + "GasRegenAlwaysOne3": 1, + } + + dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] + values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \ + (((0xff - dat[2]) & 0xff) << 8) | \ + ((0x100 - dat[3] - idx) & 0xff) + + return packer.make_can_msg("ASCMGasRegenCmd", bus, values) + +def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop): + mode = 0x1 + if apply_brake > 0: + mode = 0xa + if at_full_stop: + mode = 0xd + + # TODO: this is to have GM bringing the car to complete stop, + # but currently it conflicts with OP controls, so turned off. + #elif near_stop: + # mode = 0xb + + brake = (0x1000 - apply_brake) & 0xfff + checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff + + values = { + "RollingCounter" : idx, + "FrictionBrakeMode" : mode, + "FrictionBrakeChecksum": checksum, + "FrictionBrakeCmd" : -apply_brake + } + + return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) + +def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lead_car_in_sight, fcw): + target_speed = min(target_speed_kph, 255) + + values = { + "ACCAlwaysOne" : 1, + "ACCResumeButton" : 0, + "ACCSpeedSetpoint" : target_speed, + "ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive" + "ACCCmdActive" : acc_engaged, + "ACCAlwaysOne2" : 1, + "ACCLeadCar" : lead_car_in_sight, + "FCWAlert": 0x3 if fcw else 0 + } + + return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) + +def create_adas_time_status(bus, tt, idx): + dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, + ((tt & 0xf) << 4) + (idx << 2)] + chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] + chksum = chksum & 0xfff + dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] + return make_can_msg(0xa1, bytes(dat), bus) + +def create_adas_steering_status(bus, idx): + dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] + chksum = 0x60 + sum(dat) + dat += [chksum >> 8, chksum & 0xff] + return make_can_msg(0x306, bytes(dat), bus) + +def create_adas_accelerometer_speed_status(bus, speed_ms, idx): + spd = int(speed_ms * 16) & 0xfff + accel = 0 & 0xfff + # 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L + #stick = 0x08 + near_range_cutoff = 0x27 + near_range_mode = 1 if spd <= near_range_cutoff else 0 + far_range_mode = 1 - near_range_mode + dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] + chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] + dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] + return make_can_msg(0x308, bytes(dat), bus) + +def create_adas_headlights_status(packer, bus): + values = { + "Always42": 0x42, + "Always4": 0x4, + } + return packer.make_can_msg("ASCMHeadlight", bus, values) + +def create_lka_icon_command(bus, active, critical, steer): + if active and steer == 1: + if critical: + dat = b"\x50\xc0\x14" + else: + dat = b"\x50\x40\x18" + elif active: + if critical: + dat = b"\x40\xc0\x14" + else: + dat = b"\x40\x40\x18" + else: + dat = b"\x00\x00\x00" + return make_can_msg(0x104c006c, dat, bus) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py new file mode 100755 index 00000000000000..be288904840676 --- /dev/null +++ b/selfdrive/car/gm/interface.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 +from cereal import car +from math import fabs +from panda import Panda + +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR +from selfdrive.car.interfaces import CarInterfaceBase + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter +TransmissionType = car.CarParams.TransmissionType +NetworkLocation = car.CarParams.NetworkLocation +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + params = CarControllerParams() + return params.ACCEL_MIN, params.ACCEL_MAX + + # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. + @staticmethod + def get_steer_feedforward_volt(desired_angle, v_ego): + desired_angle *= 0.02904609 + sigmoid = desired_angle / (1 + fabs(desired_angle)) + return 0.10006696 * sigmoid * (v_ego + 3.12485927) + + @staticmethod + def get_steer_feedforward_acadia(desired_angle, v_ego): + desired_angle *= 0.09760208 + sigmoid = desired_angle / (1 + fabs(desired_angle)) + return 0.04689655 * sigmoid * (v_ego + 10.028217) + + def get_steer_feedforward_function(self): + if self.CP.carFingerprint == CAR.VOLT: + return self.get_steer_feedforward_volt + elif self.CP.carFingerprint == CAR.ACADIA: + return self.get_steer_feedforward_acadia + else: + return CarInterfaceBase.get_steer_feedforward_default + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "gm" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] + ret.autoResumeSng = False + + if candidate in EV_CAR: + ret.transmissionType = TransmissionType.direct + else: + ret.transmissionType = TransmissionType.automatic + + if candidate in CAMERA_ACC_CAR: + ret.openpilotLongitudinalControl = False + ret.networkLocation = NetworkLocation.fwdCamera + ret.radarOffCan = True # no radar + ret.pcmCruise = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM + else: # ASCM, OBD-II harness + ret.openpilotLongitudinalControl = True + ret.networkLocation = NetworkLocation.gateway + ret.radarOffCan = False + ret.pcmCruise = False # stock non-adaptive cruise control is kept off + + # These cars have been put into dashcam only due to both a lack of users and test coverage. + # These cars likely still work fine. Once a user confirms each car works and a test route is + # added to selfdrive/car/tests/routes.py, we can remove it from this list. + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} + + # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. + ret.minSteerSpeed = 10 * CV.KPH_TO_MS + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] + ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 + ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + tire_stiffness_factor = 0.444 # not optimized yet + + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.36] + + ret.steerLimitTimer = 0.4 + ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz + + # supports stop and go, but initial engage must (conservatively) be above 18mph + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + + if candidate == CAR.VOLT: + ret.mass = 1607. + STD_CARGO_KG + ret.wheelbase = 2.69 + ret.steerRatio = 17.7 # Stock 15.7, LiveParameters + tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters + ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh + + ret.lateralTuning.pid.kpBP = [0., 40.] + ret.lateralTuning.pid.kpV = [0., 0.17] + ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kiV = [0.] + ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() + ret.steerActuatorDelay = 0.2 + + elif candidate == CAR.MALIBU: + ret.mass = 1496. + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 15.8 + ret.centerToFront = ret.wheelbase * 0.4 # wild guess + + elif candidate == CAR.HOLDEN_ASTRA: + ret.mass = 1363. + STD_CARGO_KG + ret.wheelbase = 2.662 + # Remaining parameters copied from Volt for now + ret.centerToFront = ret.wheelbase * 0.4 + ret.steerRatio = 15.7 + + elif candidate == CAR.ACADIA: + ret.minEnableSpeed = -1. # engage speed is decided by pcm + ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.86 + ret.steerRatio = 14.4 # end to end is 13.46 + ret.centerToFront = ret.wheelbase * 0.4 + ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() + ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking + + elif candidate == CAR.BUICK_REGAL: + ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 + ret.wheelbase = 2.83 # 111.4 inches in meters + ret.steerRatio = 14.4 # guess for tourx + ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx + + elif candidate == CAR.CADILLAC_ATS: + ret.mass = 1601. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 15.3 + ret.centerToFront = ret.wheelbase * 0.49 + + elif candidate == CAR.ESCALADE_ESV: + ret.minEnableSpeed = -1. # engage speed is decided by pcm + ret.mass = 2739. + STD_CARGO_KG + ret.wheelbase = 3.302 + ret.steerRatio = 17.3 + ret.centerToFront = ret.wheelbase * 0.49 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] + ret.lateralTuning.pid.kf = 0.000045 + tire_stiffness_factor = 1.0 + + elif candidate == CAR.BOLT_EUV: + ret.minEnableSpeed = -1 + ret.mass = 1669. + STD_CARGO_KG + ret.wheelbase = 2.675 + ret.steerRatio = 16.8 + ret.centerToFront = ret.wheelbase * 0.4 + tire_stiffness_factor = 1.0 + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + elif candidate == CAR.SILVERADO: + ret.minEnableSpeed = -1 + ret.mass = 2200. + STD_CARGO_KG + ret.wheelbase = 3.75 + ret.steerRatio = 16.3 + ret.centerToFront = ret.wheelbase * 0.5 + tire_stiffness_factor = 1.0 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) + + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: + be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) + + # Suppress resume button if we're resuming from stop so we don't adjust speed. + if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): + be.type = ButtonType.unknown + + ret.buttonEvents = [be] + + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, + GearShifter.eco, GearShifter.manumatic], + pcm_enable=self.CP.pcmCruise) + + if ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + if ret.cruiseState.standstill: + events.add(EventName.resumeRequired) + if ret.vEgo < self.CP.minSteerSpeed: + events.add(car.CarEvent.EventName.belowSteerSpeed) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py new file mode 100755 index 00000000000000..6904e6f899f914 --- /dev/null +++ b/selfdrive/car/gm/radar_interface.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +import math +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from selfdrive.car.gm.values import DBC, CAR, CanBus +from selfdrive.car.interfaces import RadarInterfaceBase + +RADAR_HEADER_MSG = 1120 +SLOT_1_MSG = RADAR_HEADER_MSG + 1 +NUM_SLOTS = 20 + +# Actually it's 0x47f, but can parser only reports +# messages that are present in DBC +LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS + + +def create_radar_can_parser(car_fingerprint): + if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): + return None + + # C1A-ARS3-A by Continental + radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) + signals = list(zip(['FLRRNumValidTargets', + 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', + 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', + 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, + [RADAR_HEADER_MSG] * 7 + radar_targets * 6)) + + checks = list({(s[1], 14) for s in signals}) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + + self.rcp = create_radar_can_parser(CP.carFingerprint) + + self.trigger_msg = LAST_RADAR_MSG + self.updated_messages = set() + self.radar_ts = CP.radarTimeStep + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + header = self.rcp.vl[RADAR_HEADER_MSG] + fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ + header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \ + header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt'] + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + if fault: + errors.append("fault") + ret.errors = errors + + currentTargets = set() + num_targets = header['FLRRNumValidTargets'] + + # Not all radar messages describe targets, + # no need to monitor all of the self.rcp.msgs_upd + for ii in self.updated_messages: + if ii == RADAR_HEADER_MSG: + continue + + if num_targets == 0: + break + + cpt = self.rcp.vl[ii] + # Zero distance means it's an empty target slot + if cpt['TrkRange'] > 0.0: + targetId = cpt['TrkObjectID'] + currentTargets.add(targetId) + if targetId not in self.pts: + self.pts[targetId] = car.RadarData.RadarPoint.new_message() + self.pts[targetId].trackId = targetId + distance = cpt['TrkRange'] + self.pts[targetId].dRel = distance # from front of car + # From driver's pov, left is positive + self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance + self.pts[targetId].vRel = cpt['TrkRangeRate'] + self.pts[targetId].aRel = float('nan') + self.pts[targetId].yvRel = float('nan') + + for oldTarget in list(self.pts.keys()): + if oldTarget not in currentTargets: + del self.pts[oldTarget] + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py new file mode 100644 index 00000000000000..21ede171e04202 --- /dev/null +++ b/selfdrive/car/gm/values.py @@ -0,0 +1,178 @@ +from collections import defaultdict +from dataclasses import dataclass, field +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output + STEER_STEP = 2 # Control frames per command (50hz) + STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness + STEER_DELTA_DOWN = 17 + STEER_DRIVER_ALLOWANCE = 50 + STEER_DRIVER_MULTIPLIER = 4 + STEER_DRIVER_FACTOR = 100 + NEAR_STOP_BRAKE_PHASE = 0.5 # m/s + + # Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera" + ADAS_KEEPALIVE_STEP = 100 + CAMERA_KEEPALIVE_STEP = 100 + + # Volt gasbrake lookups + # TODO: These values should be confirmed on non-Volt vehicles + MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. + ZERO_GAS = 2048 # Coasting + MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen + MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen + + # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we + # perform the closed loop control, and might need some + # to apply some more braking if we're on a downhill slope. + # Our controller should still keep the 2 second average above + # -3.5 m/s^2 as per planner limits + ACCEL_MAX = 2. # m/s^2 + ACCEL_MIN = -4. # m/s^2 + + EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] + EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] + + # ICE has much less engine braking force compared to regen in EVs, + # lower threshold removes some braking deadzone + GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] + BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] + + GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] + BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] + + +class CAR: + HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" + VOLT = "CHEVROLET VOLT PREMIER 2017" + CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" + MALIBU = "CHEVROLET MALIBU PREMIER 2017" + ACADIA = "GMC ACADIA DENALI 2018" + BUICK_REGAL = "BUICK REGAL ESSENCE 2018" + ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" + BOLT_EUV = "CHEVROLET BOLT EUV 2022" + SILVERADO = "CHEVROLET SILVERADO 1500 2020" + + +class Footnote(Enum): + OBD_II = CarFootnote( + 'Requires a community built ASCM harness. ' + + 'NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).', + Column.MODEL) + + +@dataclass +class GMCarInfo(CarInfo): + package: str = "Adaptive Cruise Control" + harness: Enum = Harness.obd_ii + footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II]) + + +CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { + CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), + CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0), + CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), + CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), + CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), + CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), + CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), + CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm), + CAR.SILVERADO: [ + GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II", footnotes=[], harness=Harness.gm), + GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", footnotes=[], harness=Harness.gm), + ], +} + + +class CruiseButtons: + INIT = 0 + UNPRESS = 1 + RES_ACCEL = 2 + DECEL_SET = 3 + MAIN = 5 + CANCEL = 6 + +class AccState: + OFF = 0 + ACTIVE = 1 + FAULTED = 3 + STANDSTILL = 4 + +class CanBus: + POWERTRAIN = 0 + OBSTACLE = 1 + CAMERA = 2 + CHASSIS = 2 + SW_GMLAN = 3 + LOOPBACK = 128 + DROPPED = 192 + +FINGERPRINTS = { + CAR.HOLDEN_ASTRA: [ + # Astra BK MY17, ASCM unplugged + { + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7, + }], + CAR.VOLT: [ + # Volt Premier w/ ACC 2017 + { + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 + }, + # Volt Premier w/ ACC 2018 + { + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8 + }], + CAR.BUICK_REGAL : [ + # Regal TourX Essence w/ ACC 2018 + { + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8 + }], + CAR.CADILLAC_ATS: [ + # Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], + CAR.MALIBU: [ + # Malibu Premier w/ ACC 2017 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8, + }], + CAR.ACADIA: [ + # Acadia Denali w/ACC 2018 + { + 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7 + }, + # Acadia Denali w/ /ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], + CAR.ESCALADE_ESV: [ + { + 309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8 + }], + CAR.BOLT_EUV: [ + { + 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1930: 7 + }], + CAR.SILVERADO: [ + { + 190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1930: 7 + }], +} + +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) + +EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} + +# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) +CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO} + +STEER_THRESHOLD = 1.0 diff --git a/system/hardware/tests/__init__.py b/selfdrive/car/honda/__init__.py similarity index 100% rename from system/hardware/tests/__init__.py rename to selfdrive/car/honda/__init__.py diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py new file mode 100644 index 00000000000000..5fa475fe083d12 --- /dev/null +++ b/selfdrive/car/honda/carcontroller.py @@ -0,0 +1,255 @@ +from collections import namedtuple + +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car import create_gas_interceptor_command +from selfdrive.car.honda import hondacan +from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams +from selfdrive.controls.lib.drive_helpers import rate_limit + +VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState + + +def compute_gb_honda_bosch(accel, speed): + # TODO returns 0s, is unused + return 0.0, 0.0 + + +def compute_gb_honda_nidec(accel, speed): + creep_brake = 0.0 + creep_speed = 2.3 + creep_brake_value = 0.15 + if speed < creep_speed: + creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + gb = float(accel) / 4.8 - creep_brake + return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0) + + +def compute_gas_brake(accel, speed, fingerprint): + if fingerprint in HONDA_BOSCH: + return compute_gb_honda_bosch(accel, speed) + else: + return compute_gb_honda_nidec(accel, speed) + + +# TODO not clear this does anything useful +def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint): + # hyst params + brake_hyst_on = 0.02 # to activate brakes exceed this value + brake_hyst_off = 0.005 # to deactivate brakes below this value + brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value + + # *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger + if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off: + brake = 0. + braking = brake > 0. + + # for small brake oscillations within brake_hyst_gap, don't change the brake command + if brake == 0.: + brake_steady = 0. + elif brake > brake_steady + brake_hyst_gap: + brake_steady = brake - brake_hyst_gap + elif brake < brake_steady - brake_hyst_gap: + brake_steady = brake + brake_hyst_gap + brake = brake_steady + + return brake, braking, brake_steady + + +def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts): + pump_on = False + + # reset pump timer if: + # - there is an increment in brake request + # - we are applying steady state brakes and we haven't been running the pump + # for more than 20s (to prevent pressure bleeding) + if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0): + last_pump_ts = ts + + # once the pump is on, run it for at least 0.2s + if ts - last_pump_ts < 0.2 and apply_brake > 0: + pump_on = True + + return pump_on, last_pump_ts + + +def process_hud_alert(hud_alert): + # initialize to no alert + fcw_display = 0 + steer_required = 0 + acc_alert = 0 + + # priority is: FCW, steer required, all others + if hud_alert == VisualAlert.fcw: + fcw_display = VISUAL_HUD[hud_alert.raw] + elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw): + steer_required = VISUAL_HUD[hud_alert.raw] + else: + acc_alert = VISUAL_HUD[hud_alert.raw] + + return fcw_display, steer_required, acc_alert + + +HUDData = namedtuple("HUDData", + ["pcm_accel", "v_cruise", "lead_visible", + "lanes_visible", "fcw", "acc_alert", "steer_required"]) + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.packer = CANPacker(dbc_name) + self.params = CarControllerParams(CP) + self.frame = 0 + + self.braking = False + self.brake_steady = 0. + self.brake_last = 0. + self.apply_brake_last = 0 + self.last_pump_ts = 0. + + self.accel = 0.0 + self.speed = 0.0 + self.gas = 0.0 + self.brake = 0.0 + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255 + pcm_cancel_cmd = CC.cruiseControl.cancel + + if CC.longActive: + accel = actuators.accel + gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint) + else: + accel = 0.0 + gas, brake = 0.0, 0.0 + + # *** apply brake hysteresis *** + pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady, + CS.out.vEgo, self.CP.carFingerprint) + + # *** rate limit after the enable check *** + self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) + + # vehicle hud display, wait for one update from 10Hz 0x304 msg + fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert) + + # **** process the car messages **** + + # steer torque is converted back to CAN reference (positive when steering right) + apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX, + self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) + + # Send CAN commands + can_sends = [] + + # tester present - w/ no response (keeps radar disabled) + if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl: + if self.frame % 10 == 0: + can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) + + # Send steering command. + can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint, + CS.CP.openpilotLongitudinalControl)) + + # wind brake from air resistance decel at high speed + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + # all of this is only relevant for HONDA NIDEC + max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V) + # TODO this 1.44 is just to maintain previous behavior + pcm_speed_BP = [-wind_brake, + -wind_brake * (3 / 4), + 0.0, + 0.5] + # The Honda ODYSSEY seems to have different PCM_ACCEL + # msgs, is it other cars too? + if self.CP.enableGasInterceptor or not CC.longActive: + pcm_speed = 0.0 + pcm_accel = int(0.0) + elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL: + pcm_speed_V = [0.0, + clip(CS.out.vEgo - 3.0, 0.0, 100.0), + clip(CS.out.vEgo + 0.0, 0.0, 100.0), + clip(CS.out.vEgo + 5.0, 0.0, 100.0)] + pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) + pcm_accel = int(1.0 * 0xc6) + else: + pcm_speed_V = [0.0, + clip(CS.out.vEgo - 2.0, 0.0, 100.0), + clip(CS.out.vEgo + 2.0, 0.0, 100.0), + clip(CS.out.vEgo + 5.0, 0.0, 100.0)] + pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) + pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6) + + if not self.CP.openpilotLongitudinalControl: + if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message + can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint)) + # If using stock ACC, spam cancel command to kill gas when OP disengages. + if pcm_cancel_cmd: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint)) + elif CC.cruiseControl.resume: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) + + else: + # Send gas and brake commands. + if self.frame % 2 == 0: + ts = self.frame * DT_CTRL + + if self.CP.carFingerprint in HONDA_BOSCH: + self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) + self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) + + stopping = actuators.longControlState == LongCtrlState.stopping + can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, + stopping, self.CP.carFingerprint)) + else: + apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) + apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1)) + pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) + + pcm_override = True + can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, + pcm_override, pcm_cancel_cmd, fcw_display, + self.CP.carFingerprint, CS.stock_brake)) + self.apply_brake_last = apply_brake + self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX + + if self.CP.enableGasInterceptor: + # way too aggressive at low speed without this + gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected + # when you do enable. + if CC.longActive: + self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.) + else: + self.gas = 0.0 + can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2)) + + # Send dashboard UI commands. + if self.frame % 10 == 0: + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, + hud_control.lanesVisible, fcw_display, acc_alert, steer_required) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) + + if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: + self.speed = pcm_speed + + if not self.CP.enableGasInterceptor: + self.gas = pcm_accel / 0xc6 + + new_actuators = actuators.copy() + new_actuators.speed = self.speed + new_actuators.accel = self.accel + new_actuators.gas = self.gas + new_actuators.brake = self.brake + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py new file mode 100644 index 00000000000000..4696bec82e3418 --- /dev/null +++ b/selfdrive/car/honda/carstate.py @@ -0,0 +1,344 @@ +from collections import defaultdict + +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import interp +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.honda.hondacan import get_pt_bus +from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS +from selfdrive.car.interfaces import CarStateBase + +TransmissionType = car.CarParams.TransmissionType + + +def get_can_signals(CP, gearbox_msg, main_on_sig_msg): + signals = [ + ("XMISSION_SPEED", "ENGINE_DATA"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("STEER_ANGLE", "STEERING_SENSORS"), + ("STEER_ANGLE_RATE", "STEERING_SENSORS"), + ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), + ("STEER_TORQUE_SENSOR", "STEER_STATUS"), + ("IMPERIAL_UNIT", "CAR_SPEED"), + ("LEFT_BLINKER", "SCM_FEEDBACK"), + ("RIGHT_BLINKER", "SCM_FEEDBACK"), + ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"), + ("BRAKE_PRESSED", "POWERTRAIN_DATA"), + ("BRAKE_SWITCH", "POWERTRAIN_DATA"), + ("CRUISE_BUTTONS", "SCM_BUTTONS"), + ("ESP_DISABLED", "VSA_STATUS"), + ("USER_BRAKE", "VSA_STATUS"), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS"), + ("STEER_STATUS", "STEER_STATUS"), + ("GEAR_SHIFTER", gearbox_msg), + ("GEAR", gearbox_msg), + ("PEDAL_GAS", "POWERTRAIN_DATA"), + ("CRUISE_SETTING", "SCM_BUTTONS"), + ("ACC_STATUS", "POWERTRAIN_DATA"), + ("MAIN_ON", main_on_sig_msg), + ] + + checks = [ + ("ENGINE_DATA", 100), + ("WHEEL_SPEEDS", 50), + ("STEERING_SENSORS", 100), + ("SEATBELT_STATUS", 10), + ("CRUISE", 10), + ("POWERTRAIN_DATA", 100), + ("CAR_SPEED", 10), + ("VSA_STATUS", 50), + ("STEER_STATUS", 100), + ("STEER_MOTOR_TORQUE", 0), # TODO: not on every car + ] + + if CP.carFingerprint == CAR.ODYSSEY_CHN: + checks += [ + ("SCM_FEEDBACK", 25), + ("SCM_BUTTONS", 50), + ] + else: + checks += [ + ("SCM_FEEDBACK", 10), + ("SCM_BUTTONS", 25), + ] + + if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): + checks.append((gearbox_msg, 50)) + else: + checks.append((gearbox_msg, 100)) + + if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL: + signals.append(("BRAKE_PRESSED", "BRAKE_MODULE")) + checks.append(("BRAKE_MODULE", 50)) + + if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): + signals.append(("EPB_STATE", "EPB_STATUS")) + checks.append(("EPB_STATUS", 50)) + + if CP.carFingerprint in HONDA_BOSCH: + # these messages are on camera bus on radarless cars + if not CP.openpilotLongitudinalControl and CP.carFingerprint not in HONDA_BOSCH_RADARLESS: + signals += [ + ("CRUISE_CONTROL_LABEL", "ACC_HUD"), + ("CRUISE_SPEED", "ACC_HUD"), + ("ACCEL_COMMAND", "ACC_CONTROL"), + ("AEB_STATUS", "ACC_CONTROL"), + ] + checks += [ + ("ACC_HUD", 10), + ("ACC_CONTROL", 50), + ] + else: # Nidec signals + signals += [("CRUISE_SPEED_PCM", "CRUISE"), + ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")] + + if CP.carFingerprint == CAR.ODYSSEY_CHN: + checks.append(("CRUISE_PARAMS", 10)) + else: + checks.append(("CRUISE_PARAMS", 50)) + + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK")) + elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): + signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS")) + else: + signals += [("DOOR_OPEN_FL", "DOORS_STATUS"), + ("DOOR_OPEN_FR", "DOORS_STATUS"), + ("DOOR_OPEN_RL", "DOORS_STATUS"), + ("DOOR_OPEN_RR", "DOORS_STATUS")] + checks.append(("DOORS_STATUS", 3)) + + # add gas interceptor reading if we are using it + if CP.enableGasInterceptor: + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) + checks.append(("GAS_SENSOR", 50)) + + if CP.openpilotLongitudinalControl: + signals += [ + ("BRAKE_ERROR_1", "STANDSTILL"), + ("BRAKE_ERROR_2", "STANDSTILL") + ] + checks.append(("STANDSTILL", 50)) + + return signals, checks + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.gearbox_msg = "GEARBOX" + if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt: + self.gearbox_msg = "GEARBOX_15T" + + self.main_on_sig_msg = "SCM_FEEDBACK" + if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES: + self.main_on_sig_msg = "SCM_BUTTONS" + + self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"] + self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"]) + + self.brake_error = False + self.brake_switch_prev = False + self.brake_switch_active = False + self.cruise_setting = 0 + self.v_cruise_pcm_prev = 0 + + def update(self, cp, cp_cam, cp_body): + ret = car.CarState.new_message() + + # car params + v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping + v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero + + # update prevs, update must run once per loop + self.prev_cruise_buttons = self.cruise_buttons + self.prev_cruise_setting = self.cruise_setting + self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] + self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"] + + # used for car hud message + self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"] + + # ******************* parse out can ******************* + # STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED + # panda checks if the signal is non-zero + ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 + # TODO: find a common signal across all cars + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022): + ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) + elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): + ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) + else: + ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"], + cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]]) + ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"]) + + steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]] + ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT") + # LOW_SPEED_LOCKOUT is not worth a warning + # NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver + ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2") + + if self.CP.openpilotLongitudinalControl: + self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"] + ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 + + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], + ) + v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0 + + # blend in transmission speed at low speed, since it has more low speed accuracy + v_weight = interp(v_wheel, v_weight_bp, v_weight_v) + ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + + ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"] + ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"] + + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk( + 250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"]) + ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1 + + # TODO: set for all cars + if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): + ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0 + + gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) + + if self.CP.enableGasInterceptor: + # Same threshold as panda, equivalent to 1e-5 with previous DBC scaling + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 + ret.gasPressed = ret.gas > 492 + else: + ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] + ret.gasPressed = ret.gas > 1e-5 + + ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"] + ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200) + + if self.CP.carFingerprint in HONDA_BOSCH: + if not self.CP.openpilotLongitudinalControl: + # ACC_HUD is on camera bus on radarless cars + acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"] + ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0 + ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252. + + # on certain cars, CRUISE_SPEED changes to imperial with car's unit setting + conversion_factor = CV.MPH_TO_MS if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS and not self.is_metric else CV.KPH_TO_MS + # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. + ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion_factor + self.v_cruise_pcm_prev = ret.cruiseState.speed + else: + ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS + + if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL: + ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 + else: + # brake switch has shown some single time step noise, so only considered when + # switch is on for at least 2 consecutive CAN samples + # brake switch rises earlier than brake pressed but is never 1 when in park + brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"] + if len(brake_switch_vals): + brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0 + if len(brake_switch_vals) > 1: + self.brake_switch_prev = brake_switch_vals[-2] != 0 + self.brake_switch_active = brake_switch and self.brake_switch_prev + self.brake_switch_prev = brake_switch + ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active + + ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"] + ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0 + ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) + + # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models + if self.CP.carFingerprint in (CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE): + if ret.brake > 0.1: + ret.brakePressed = True + + if self.CP.carFingerprint in HONDA_BOSCH: + # TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts + if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: + ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) + else: + ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) + + self.acc_hud = False + self.lkas_hud = False + if self.CP.carFingerprint not in HONDA_BOSCH: + ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0 + self.acc_hud = cp_cam.vl["ACC_HUD"] + self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + self.lkas_hud = cp_cam.vl["LKAS_HUD"] + + if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, ): + # BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0 + # more info here: https://github.com/commaai/openpilot/pull/1867 + ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1 + ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 + + return ret + + def get_can_parser(self, CP): + signals, checks = get_can_signals(CP, self.gearbox_msg, self.main_on_sig_msg) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_pt_bus(CP.carFingerprint)) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [ + ("STEERING_CONTROL", 100), + ] + + if CP.carFingerprint in HONDA_BOSCH_RADARLESS: + signals.append(("LKAS_PROBLEM", "LKAS_HUD")) + checks.append(("LKAS_HUD", 10)) + if not CP.openpilotLongitudinalControl: + signals += [ + ("CRUISE_SPEED", "ACC_HUD"), + ("CRUISE_CONTROL_LABEL", "ACC_HUD"), + ] + checks.append(("ACC_HUD", 10)) + + elif CP.carFingerprint not in HONDA_BOSCH: + signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND"), + ("AEB_REQ_1", "BRAKE_COMMAND"), + ("FCW", "BRAKE_COMMAND"), + ("CHIME", "BRAKE_COMMAND"), + ("FCM_OFF", "ACC_HUD"), + ("FCM_OFF_2", "ACC_HUD"), + ("FCM_PROBLEM", "ACC_HUD"), + ("ICONS", "ACC_HUD")] + checks += [ + ("ACC_HUD", 10), + ("BRAKE_COMMAND", 50), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_body_can_parser(CP): + if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G: + signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"), + ("BSM_ALERT", "BSM_STATUS_LEFT")] + + checks = [ + ("BSM_STATUS_LEFT", 3), + ("BSM_STATUS_RIGHT", 3), + ] + bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) + return CANParser(DBC[CP.carFingerprint]["body"], signals, checks, bus_body) + return None diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py new file mode 100644 index 00000000000000..87f8e6c5de3635 --- /dev/null +++ b/selfdrive/car/honda/hondacan.py @@ -0,0 +1,176 @@ +from common.conversions import Conversions as CV +from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams + +# CAN bus layout with relay +# 0 = ACC-CAN - radar side +# 1 = F-CAN B - powertrain +# 2 = ACC-CAN - camera side +# 3 = F-CAN A - OBDII port + + +def get_pt_bus(car_fingerprint): + return 1 if car_fingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) else 0 + + +def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): + if radar_disabled: + # when radar is disabled, steering commands are sent directly to powertrain bus + return get_pt_bus(car_fingerprint) + # normally steering commands are sent to radar, which forwards them to powertrain bus + return 0 + + +def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): + # TODO: do we loose pressure if we keep pump off for long? + brakelights = apply_brake > 0 + brake_rq = apply_brake > 0 + pcm_fault_cmd = False + + values = { + "COMPUTER_BRAKE": apply_brake, + "BRAKE_PUMP_REQUEST": pump_on, + "CRUISE_OVERRIDE": pcm_override, + "CRUISE_FAULT_CMD": pcm_fault_cmd, + "CRUISE_CANCEL_CMD": pcm_cancel_cmd, + "COMPUTER_BRAKE_REQUEST": brake_rq, + "SET_ME_1": 1, + "BRAKE_LIGHTS": brakelights, + "CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw + "FCW": fcw << 1, # TODO: Why are there two bits for fcw? + "AEB_REQ_1": 0, + "AEB_REQ_2": 0, + "AEB_STATUS": 0, + } + bus = get_pt_bus(car_fingerprint) + return packer.make_can_msg("BRAKE_COMMAND", bus, values) + + +def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_fingerprint): + commands = [] + bus = get_pt_bus(car_fingerprint) + min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0] + + control_on = 5 if enabled else 0 + gas_command = gas if active and accel > min_gas_accel else -30000 + accel_command = accel if active else 0 + braking = 1 if active and accel < min_gas_accel else 0 + standstill = 1 if active and stopping else 0 + standstill_release = 1 if active and not stopping else 0 + + acc_control_values = { + # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1 + "CONTROL_ON": control_on, + "GAS_COMMAND": gas_command, # used for gas + "ACCEL_COMMAND": accel_command, # used for brakes + "BRAKE_LIGHTS": braking, + "BRAKE_REQUEST": braking, + "STANDSTILL": standstill, + "STANDSTILL_RELEASE": standstill_release, + } + commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values)) + + acc_control_on_values = { + "SET_TO_3": 0x03, + "CONTROL_ON": enabled, + "SET_TO_FF": 0xff, + "SET_TO_75": 0x75, + "SET_TO_30": 0x30, + } + commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values)) + + return commands + + +def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled): + values = { + "STEER_TORQUE": apply_steer if lkas_active else 0, + "STEER_TORQUE_REQUEST": lkas_active, + } + bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled) + return packer.make_can_msg("STEERING_CONTROL", bus, values) + + +def create_bosch_supplemental_1(packer, car_fingerprint): + # non-active params + values = { + "SET_ME_X04": 0x04, + "SET_ME_X80": 0x80, + "SET_ME_X10": 0x10, + } + bus = get_lkas_cmd_bus(car_fingerprint) + return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) + + +def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): + commands = [] + bus_pt = get_pt_bus(CP.carFingerprint) + radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl + bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) + + if CP.openpilotLongitudinalControl: + acc_hud_values = { + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': 1, + 'HUD_DISTANCE': 0, # max distance setting on display + 'IMPERIAL_UNIT': int(not is_metric), + 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, + 'SET_ME_X01_2': 1, + } + + if CP.carFingerprint in HONDA_BOSCH: + acc_hud_values['ACC_ON'] = int(enabled) + acc_hud_values['FCM_OFF'] = 1 + acc_hud_values['FCM_OFF_2'] = 1 + else: + acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH + acc_hud_values['PCM_GAS'] = hud.pcm_accel + acc_hud_values['SET_ME_X01'] = 1 + acc_hud_values['FCM_OFF'] = acc_hud['FCM_OFF'] + acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2'] + acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM'] + acc_hud_values['ICONS'] = acc_hud['ICONS'] + commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values)) + + lkas_hud_values = { + 'SET_ME_X41': 0x41, + 'STEERING_REQUIRED': hud.steer_required, + 'SOLID_LANES': hud.lanes_visible, + 'BEEP': 0, + } + + if CP.carFingerprint in HONDA_BOSCH_RADARLESS: + lkas_hud_values['LANE_LINES'] = 3 + lkas_hud_values['DASHED_LANES'] = hud.lanes_visible + # car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera + lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM'] + + if not (CP.flags & HondaFlags.BOSCH_EXT_HUD): + lkas_hud_values['SET_ME_X48'] = 0x48 + + if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl: + commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values)) + commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values)) + else: + commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values)) + + if radar_disabled and CP.carFingerprint in HONDA_BOSCH: + radar_hud_values = { + 'CMBS_OFF': 0x01, + 'SET_TO_1': 0x01, + } + commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values)) + + if CP.carFingerprint == CAR.CIVIC_BOSCH: + commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {})) + + return commands + + +def spam_buttons_command(packer, button_val, car_fingerprint): + values = { + 'CRUISE_BUTTONS': button_val, + 'CRUISE_SETTING': 0, + } + # send buttons to camera on radarless cars + bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint) + return packer.make_can_msg("SCM_BUTTONS", bus, values) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py new file mode 100755 index 00000000000000..c884f586a00bb6 --- /dev/null +++ b/selfdrive/car/honda/interface.py @@ -0,0 +1,363 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from common.conversions import Conversions as CV +from common.numpy_fast import interp +from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS +from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.disable_ecu import disable_ecu + + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +TransmissionType = car.CarParams.TransmissionType +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + if CP.carFingerprint in HONDA_BOSCH: + return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX + else: + # NIDECs don't allow acceleration near cruise_speed, + # so limit limits of pid to prevent windup + ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2] + ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2] + return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "honda" + + if candidate in HONDA_BOSCH: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] + ret.radarOffCan = True + + if candidate not in HONDA_BOSCH_RADARLESS: + # Disable the radar and let openpilot control longitudinal + # WARNING: THIS DISABLES AEB! + ret.experimentalLongitudinalAvailable = True + ret.openpilotLongitudinalControl = experimental_long + + ret.pcmCruise = not ret.openpilotLongitudinalControl + else: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)] + ret.enableGasInterceptor = 0x201 in fingerprint[0] + ret.openpilotLongitudinalControl = True + + ret.pcmCruise = not ret.enableGasInterceptor + + if candidate == CAR.CRV_5G: + ret.enableBsm = 0x12f8bfa7 in fingerprint[0] + + # Detect Bosch cars with new HUD msgs + if any(0x33DA in f for f in fingerprint.values()): + ret.flags |= HondaFlags.BOSCH_EXT_HUD.value + + # Accord 1.5T CVT has different gearbox message + if candidate == CAR.ACCORD and 0x191 in fingerprint[1]: + ret.transmissionType = TransmissionType.cvt + + # Certain Hondas have an extra steering sensor at the bottom of the steering rack, + # which improves controls quality as it removes the steering column torsion from feedback. + # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. + # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward + + if candidate in HONDA_BOSCH: + ret.longitudinalTuning.kpV = [0.25] + ret.longitudinalTuning.kiV = [0.05] + ret.longitudinalActuatorDelayUpperBound = 0.5 # s + else: + # default longitudinal tuning for all hondas + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] + + eps_modified = False + for fw in car_fw: + if fw.ecu == "eps" and b"," in fw.fwVersion: + eps_modified = True + + if candidate == CAR.CIVIC: + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT + ret.steerRatio = 15.38 # 10.93 is end-to-end spec + if eps_modified: + # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE + # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 + # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180 + # stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108 + # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 + # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] + tire_stiffness_factor = 1. + + elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022): + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT + ret.steerRatio = 15.38 # 10.93 is end-to-end spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 1. + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + + elif candidate in (CAR.ACCORD, CAR.ACCORDH): + ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 16.33 # 11.82 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.8467 + + if eps_modified: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] + else: + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + + elif candidate == CAR.ACURA_ILX: + ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.37 + ret.steerRatio = 18.61 # 15.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.72 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + + elif candidate in (CAR.CRV, CAR.CRV_EU): + ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.62 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 16.89 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.444 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + ret.wheelSpeedFactor = 1.025 + + elif candidate == CAR.CRV_5G: + ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.66 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 16.0 # 12.3 is spec end-to-end + if eps_modified: + # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F + # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 + # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] + tire_stiffness_factor = 0.677 + ret.wheelSpeedFactor = 1.025 + + elif candidate == CAR.CRV_HYBRID: + ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg + ret.wheelbase = 2.66 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 16.0 # 12.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.677 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.wheelSpeedFactor = 1.025 + + elif candidate == CAR.FIT: + ret.mass = 2644. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.53 + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 13.06 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.75 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] + + elif candidate == CAR.FREED: + ret.mass = 3086. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.74 + # the remaining parameters were copied from FIT + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 13.06 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] + tire_stiffness_factor = 0.75 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] + + elif candidate == CAR.HRV: + ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.61 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 15.2 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] + ret.wheelSpeedFactor = 1.025 + + elif candidate == CAR.ACURA_RDX: + ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.68 + ret.centerToFront = ret.wheelbase * 0.38 + ret.steerRatio = 15.0 # as spec + tire_stiffness_factor = 0.444 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + + elif candidate == CAR.ACURA_RDX_3G: + ret.mass = 4068. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.75 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 11.95 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] + tire_stiffness_factor = 0.677 + + elif candidate == CAR.ODYSSEY: + ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.00 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 14.35 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] + + elif candidate == CAR.ODYSSEY_CHN: + ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg + ret.wheelbase = 2.90 + ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY + ret.steerRatio = 14.35 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] + + elif candidate in (CAR.PILOT, CAR.PASSPORT): + ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight + ret.wheelbase = 2.82 + ret.centerToFront = ret.wheelbase * 0.428 + ret.steerRatio = 17.25 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.444 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] + + elif candidate == CAR.RIDGELINE: + ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.18 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 15.59 # as spec + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.444 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] + + elif candidate == CAR.INSIGHT: + ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 15.0 # 12.58 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + + elif candidate == CAR.HONDA_E: + ret.mass = 3338.8 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.5 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 16.71 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning + + else: + raise ValueError(f"unsupported car {candidate}") + + # These cars use alternate user brake msg (0x1BE) + if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE + + # These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON) + if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_NIDEC_ALT + + if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG + + if candidate in HONDA_BOSCH_RADARLESS: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not + # conflict with PCM acc + stop_and_go = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor + ret.minEnableSpeed = -1. if stop_and_go else 25.5 * CV.MPH_TO_MS + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.8 + + return ret + + @staticmethod + def init(CP, logcan, sendcan): + if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl: + disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + + buttonEvents = [] + + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: + buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT)) + + if self.CS.cruise_setting != self.CS.prev_cruise_setting: + buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1})) + + ret.buttonEvents = buttonEvents + + # events + events = self.create_common_events(ret, pcm_enable=False) + if self.CS.brake_error: + events.add(EventName.brakeUnavailable) + + if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + + if self.CP.pcmCruise: + # we engage when pcm is active (rising edge) + if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: + events.add(EventName.pcmEnable) + elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl): + # it can happen that car cruise disables while comma system is enabled: need to + # keep braking if needed or if the speed is very low + if ret.vEgo < self.CP.minEnableSpeed + 2.: + # non loud alert if cruise disables below 25mph as expected (+ a little margin) + events.add(EventName.speedTooLow) + else: + events.add(EventName.cruiseDisabled) + if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: + events.add(EventName.manualRestart) + + ret.events = events.to_msg() + + return ret + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py new file mode 100755 index 00000000000000..629ab01d4cfe92 --- /dev/null +++ b/selfdrive/car/honda/radar_interface.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import RadarInterfaceBase +from selfdrive.car.honda.values import DBC + + +def _create_nidec_can_parser(car_fingerprint): + radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446)) + signals = list(zip(['RADAR_STATE'] + + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + ['REL_SPEED'] * 16, + [0x400] + radar_messages[1:] * 4)) + checks = [(s[1], 20) for s in signals] + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) + + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.track_id = 0 + self.radar_fault = False + self.radar_wrong_config = False + self.radar_off_can = CP.radarOffCan + self.radar_ts = CP.radarTimeStep + + self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar + + # Nidec + if self.radar_off_can: + self.rcp = None + else: + self.rcp = _create_nidec_can_parser(CP.carFingerprint) + self.trigger_msg = 0x445 + self.updated_messages = set() + + def update(self, can_strings): + # in Bosch radar and we are only steering for now, so sleep 0.05s to keep + # radard at 20Hz and return no points + if self.radar_off_can: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + rr = self._update(self.updated_messages) + self.updated_messages.clear() + return rr + + def _update(self, updated_messages): + ret = car.RadarData.new_message() + + for ii in sorted(updated_messages): + cpt = self.rcp.vl[ii] + if ii == 0x400: + # check for radar faults + self.radar_fault = cpt['RADAR_STATE'] != 0x79 + self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69 + elif cpt['LONG_DIST'] < 255: + if ii not in self.pts or cpt['NEW_TRACK']: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car + self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['REL_SPEED'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + else: + if ii in self.pts: + del self.pts[ii] + + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + if self.radar_fault: + errors.append("fault") + if self.radar_wrong_config: + errors.append("wrongConfig") + ret.errors = errors + + ret.points = list(self.pts.values()) + + return ret diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py new file mode 100644 index 00000000000000..43c3c773694f1d --- /dev/null +++ b/selfdrive/car/honda/values.py @@ -0,0 +1,1494 @@ +from dataclasses import dataclass +from enum import Enum, IntFlag +from typing import Dict, List, Optional, Union + +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +class CarControllerParams: + # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we + # perform the closed loop control, and might need some + # to apply some more braking if we're on a downhill slope. + # Our controller should still keep the 2 second average above + # -3.5 m/s^2 as per planner limits + NIDEC_ACCEL_MIN = -4.0 # m/s^2 + NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons + + NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6] + NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0] + + NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6] + NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.] + + NIDEC_BRAKE_MAX = 1024 // 4 + + BOSCH_ACCEL_MIN = -3.5 # m/s^2 + BOSCH_ACCEL_MAX = 2.0 # m/s^2 + + BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0] # 2m/s^2 + BOSCH_GAS_LOOKUP_V = [0, 1600] + + def __init__(self, CP): + self.STEER_MAX = CP.lateralParams.torqueBP[-1] + # mirror of list (assuming first item is zero) for interp of signed request values + assert(CP.lateralParams.torqueBP[0] == 0) + assert(CP.lateralParams.torqueBP[0] == 0) + self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) + self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) + + +class HondaFlags(IntFlag): + # Bosch models with alternate set of LKAS_HUD messages + BOSCH_EXT_HUD = 1 + + +# Car button codes +class CruiseButtons: + RES_ACCEL = 4 + DECEL_SET = 3 + CANCEL = 2 + MAIN = 1 + + +# See dbc files for info on values +VISUAL_HUD = { + VisualAlert.none: 0, + VisualAlert.fcw: 1, + VisualAlert.steerRequired: 1, + VisualAlert.ldw: 1, + VisualAlert.brakePressed: 10, + VisualAlert.wrongGear: 6, + VisualAlert.seatbeltUnbuckled: 5, + VisualAlert.speedTooHigh: 8 +} + + +class CAR: + ACCORD = "HONDA ACCORD 2018" + ACCORDH = "HONDA ACCORD HYBRID 2018" + CIVIC = "HONDA CIVIC 2016" + CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019" + CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019" + CIVIC_2022 = "HONDA CIVIC 2022" + ACURA_ILX = "ACURA ILX 2016" + CRV = "HONDA CR-V 2016" + CRV_5G = "HONDA CR-V 2017" + CRV_EU = "HONDA CR-V EU 2016" + CRV_HYBRID = "HONDA CR-V HYBRID 2019" + FIT = "HONDA FIT 2018" + FREED = "HONDA FREED 2020" + HRV = "HONDA HRV 2019" + ODYSSEY = "HONDA ODYSSEY 2018" + ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" + ACURA_RDX = "ACURA RDX 2018" + ACURA_RDX_3G = "ACURA RDX 2020" + PILOT = "HONDA PILOT 2017" + PASSPORT = "HONDA PASSPORT 2021" + RIDGELINE = "HONDA RIDGELINE 2017" + INSIGHT = "HONDA INSIGHT 2019" + HONDA_E = "HONDA E 2020" + + +class Footnote(Enum): + CIVIC_DIESEL = CarFootnote( + "2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.", + Column.FSR_STEERING) + + +@dataclass +class HondaCarInfo(CarInfo): + package: str = "Honda Sensing" + min_steer_speed: float = 12. * CV.MPH_TO_MS + + +CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { + CAR.ACCORD: [ + HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + ], + CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"), + CAR.CIVIC_BOSCH: [ + HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS, harness=Harness.bosch_a), + HondaCarInfo("Honda Civic Hatchback 2017-21", harness=Harness.bosch_a), + ], + CAR.CIVIC_BOSCH_DIESEL: None, # same platform + CAR.CIVIC_2022: [ + HondaCarInfo("Honda Civic 2022", "All", min_steer_speed=0., harness=Harness.bosch_b), + HondaCarInfo("Honda Civic Hatchback 2022", "All", min_steer_speed=0., harness=Harness.bosch_b), + ], + CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec), + CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", harness=Harness.nidec), + CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", harness=Harness.bosch_a), + CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring + CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", harness=Harness.bosch_a), + CAR.FIT: HondaCarInfo("Honda Fit 2018-20", harness=Harness.nidec), + CAR.FREED: HondaCarInfo("Honda Freed 2020", harness=Harness.nidec), + CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", harness=Harness.nidec), + CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", min_steer_speed=0., harness=Harness.nidec), + CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey + CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", harness=Harness.nidec), + CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", harness=Harness.nidec), + CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", harness=Harness.nidec), + CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", harness=Harness.nidec), + CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), + CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a), +} + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.UDS_VERSION_REQUEST], + [StdQueries.UDS_VERSION_RESPONSE], + ), + ], +) + +FW_VERSIONS = { + CAR.ACCORD: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-6A0-8720\x00\x00', + b'37805-6A0-9520\x00\x00', + b'37805-6A0-9620\x00\x00', + b'37805-6A0-9720\x00\x00', + b'37805-6A0-A540\x00\x00', + b'37805-6A0-A550\x00\x00', + b'37805-6A0-A640\x00\x00', + b'37805-6A0-A650\x00\x00', + b'37805-6A0-A740\x00\x00', + b'37805-6A0-A750\x00\x00', + b'37805-6A0-A840\x00\x00', + b'37805-6A0-A850\x00\x00', + b'37805-6A0-A930\x00\x00', + b'37805-6A0-AF30\x00\x00', + b'37805-6A0-AG30\x00\x00', + b'37805-6B2-C520\x00\x00', + b'37805-6A0-C540\x00\x00', + b'37805-6A1-H650\x00\x00', + b'37805-6B2-A550\x00\x00', + b'37805-6B2-A560\x00\x00', + b'37805-6B2-A650\x00\x00', + b'37805-6B2-A660\x00\x00', + b'37805-6B2-A720\x00\x00', + b'37805-6B2-A810\x00\x00', + b'37805-6B2-A820\x00\x00', + b'37805-6B2-A920\x00\x00', + b'37805-6B2-M520\x00\x00', + b'37805-6B2-Y810\x00\x00', + b'37805-6M4-B730\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TVC-A910\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-6A7-A220\x00\x00', + b'28101-6A7-A230\x00\x00', + b'28101-6A7-A320\x00\x00', + b'28101-6A7-A330\x00\x00', + b'28101-6A7-A410\x00\x00', + b'28101-6A7-A510\x00\x00', + b'28101-6A7-A610\x00\x00', + b'28101-6A7-A710\x00\x00', + b'28101-6A9-H140\x00\x00', + b'28101-6A9-H420\x00\x00', + b'28102-6B8-A560\x00\x00', + b'28102-6B8-A570\x00\x00', + b'28102-6B8-A700\x00\x00', + b'28102-6B8-A800\x00\x00', + b'28102-6B8-C560\x00\x00', + b'28102-6B8-C570\x00\x00', + b'28102-6B8-M520\x00\x00', + b'28102-6B8-R700\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TVA-A060\x00\x00', + b'46114-TVA-A080\x00\x00', + b'46114-TVA-A120\x00\x00', + b'46114-TVA-A320\x00\x00', + b'46114-TVA-A050\x00\x00', + b'46114-TVE-H550\x00\x00', + b'46114-TVE-H560\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TVA-B040\x00\x00', + b'57114-TVA-B050\x00\x00', + b'57114-TVA-B060\x00\x00', + b'57114-TVA-B530\x00\x00', + b'57114-TVA-C040\x00\x00', + b'57114-TVA-C050\x00\x00', + b'57114-TVA-C060\x00\x00', + b'57114-TVA-C530\x00\x00', + b'57114-TVA-E520\x00\x00', + b'57114-TVE-H250\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TBX-H120\x00\x00', + b'39990-TVA-A140\x00\x00', + b'39990-TVA-A150\x00\x00', + b'39990-TVA-A160\x00\x00', + b'39990-TVA-A340\x00\x00', + b'39990-TVA-X030\x00\x00', + b'39990-TVA-X040\x00\x00', + b'39990-TVA,A150\x00\x00', + b'39990-TVE-H130\x00\x00', + ], + (Ecu.unknown, 0x18da3af1, None): [ + b'39390-TVA-A020\x00\x00', + b'39390-TVA-A120\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TBX-H230\x00\x00', + b'77959-TVA-A460\x00\x00', + b'77959-TVA-F330\x00\x00', + b'77959-TVA-H230\x00\x00', + b'77959-TVA-L420\x00\x00', + b'77959-TVA-X330\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TBX-H310\x00\x00', + b'78109-TVA-A010\x00\x00', + b'78109-TVA-A020\x00\x00', + b'78109-TVA-A030\x00\x00', + b'78109-TVA-A110\x00\x00', + b'78109-TVA-A120\x00\x00', + b'78109-TVA-A130\x00\x00', + b'78109-TVA-A210\x00\x00', + b'78109-TVA-A220\x00\x00', + b'78109-TVA-A230\x00\x00', + b'78109-TVA-A310\x00\x00', + b'78109-TVA-C010\x00\x00', + b'78109-TVA-L010\x00\x00', + b'78109-TVA-L210\x00\x00', + b'78109-TVA-R310\x00\x00', + b'78109-TVC-A010\x00\x00', + b'78109-TVC-A020\x00\x00', + b'78109-TVC-A030\x00\x00', + b'78109-TVC-A110\x00\x00', + b'78109-TVC-A130\x00\x00', + b'78109-TVC-A210\x00\x00', + b'78109-TVC-A220\x00\x00', + b'78109-TVC-A230\x00\x00', + b'78109-TVC-C010\x00\x00', + b'78109-TVC-C110\x00\x00', + b'78109-TVC-L010\x00\x00', + b'78109-TVC-L210\x00\x00', + b'78109-TVC-M510\x00\x00', + b'78109-TVC-YF10\x00\x00', + b'78109-TVE-H610\x00\x00', + b'78109-TWA-A210\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + b'78209-TVA-A110\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TBX-H140\x00\x00', + b'36802-TVA-A150\x00\x00', + b'36802-TVA-A160\x00\x00', + b'36802-TVA-A170\x00\x00', + b'36802-TVA-A330\x00\x00', + b'36802-TVC-A330\x00\x00', + b'36802-TVE-H070\x00\x00', + b'36802-TWA-A070\x00\x00', + b'36802-TWA-A080\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TBX-H130\x00\x00', + b'36161-TVA-A060\x00\x00', + b'36161-TVA-A330\x00\x00', + b'36161-TVC-A330\x00\x00', + b'36161-TVE-H050\x00\x00', + b'36161-TWA-A070\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TVA-A010\x00\x00', + b'38897-TVA-A020\x00\x00', + b'38897-TVA-A230\x00\x00', + b'38897-TVA-A240\x00\x00', + ], + }, + CAR.ACCORDH: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TWA-A120\x00\x00', + b'38897-TWD-J020\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TWA-A040\x00\x00', + b'57114-TWA-A050\x00\x00', + b'57114-TWA-A530\x00\x00', + b'57114-TWA-B520\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TWA-A440\x00\x00', + b'77959-TWA-L420\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TWA-A010\x00\x00', + b'78109-TWA-A020\x00\x00', + b'78109-TWA-A030\x00\x00', + b'78109-TWA-A110\x00\x00', + b'78109-TWA-A120\x00\x00', + b'78109-TWA-A130\x00\x00', + b'78109-TWA-A210\x00\x00', + b'78109-TWA-A220\x00\x00', + b'78109-TWA-A230\x00\x00', + b'78109-TWA-L010\x00\x00', + b'78109-TWA-L210\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TWA-A910\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + b'78209-TVA-A110\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TWA-A070\x00\x00', + b'36161-TWA-A330\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TWA-A070\x00\x00', + b'36802-TWA-A080\x00\x00', + b'36802-TWA-A330\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TVA-A160\x00\x00', + b'39990-TVA-A150\x00\x00', + b'39990-TVA-A340\x00\x00', + ], + }, + CAR.CIVIC: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5AA-A640\x00\x00', + b'37805-5AA-A650\x00\x00', + b'37805-5AA-A670\x00\x00', + b'37805-5AA-A680\x00\x00', + b'37805-5AA-A810\x00\x00', + b'37805-5AA-C640\x00\x00', + b'37805-5AA-C680\x00\x00', + b'37805-5AA-C820\x00\x00', + b'37805-5AA-L650\x00\x00', + b'37805-5AA-L660\x00\x00', + b'37805-5AA-L680\x00\x00', + b'37805-5AA-L690\x00\x00', + b'37805-5AA-L810\000\000', + b'37805-5AG-Q710\x00\x00', + b'37805-5AJ-A610\x00\x00', + b'37805-5AJ-A620\x00\x00', + b'37805-5AJ-L610\x00\x00', + b'37805-5BA-A310\x00\x00', + b'37805-5BA-A510\x00\x00', + b'37805-5BA-A740\x00\x00', + b'37805-5BA-A760\x00\x00', + b'37805-5BA-A930\x00\x00', + b'37805-5BA-A960\x00\x00', + b'37805-5BA-C860\x00\x00', + b'37805-5BA-L410\x00\x00', + b'37805-5BA-L760\x00\x00', + b'37805-5BA-L930\x00\x00', + b'37805-5BA-L940\x00\x00', + b'37805-5BA-L960\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5CG-A040\x00\x00', + b'28101-5CG-A050\x00\x00', + b'28101-5CG-A070\x00\x00', + b'28101-5CG-A080\x00\x00', + b'28101-5CG-A320\x00\x00', + b'28101-5CG-A810\x00\x00', + b'28101-5CG-A820\x00\x00', + b'28101-5DJ-A040\x00\x00', + b'28101-5DJ-A060\x00\x00', + b'28101-5DJ-A510\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TBA-A540\x00\x00', + b'57114-TBA-A550\x00\x00', + b'57114-TBA-A560\x00\x00', + b'57114-TBA-A570\x00\x00', + b'57114-TEA-Q220\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TBA,A030\x00\x00', # modified firmware + b'39990-TBA-A030\x00\x00', + b'39990-TBG-A030\x00\x00', + b'39990-TEA-T020\x00\x00', + b'39990-TEG-A010\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TBA-A030\x00\x00', + b'77959-TBA-A040\x00\x00', + b'77959-TBG-A030\x00\x00', + b'77959-TEA-Q820\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TBA-A510\x00\x00', + b'78109-TBA-A520\x00\x00', + b'78109-TBA-A530\x00\x00', + b'78109-TBA-C520\x00\x00', + b'78109-TBC-A310\x00\x00', + b'78109-TBC-A320\x00\x00', + b'78109-TBC-A510\x00\x00', + b'78109-TBC-A520\x00\x00', + b'78109-TBC-A530\x00\x00', + b'78109-TBC-C510\x00\x00', + b'78109-TBC-C520\x00\x00', + b'78109-TBC-C530\x00\x00', + b'78109-TBH-A510\x00\x00', + b'78109-TBH-A530\x00\x00', + b'78109-TED-Q510\x00\x00', + b'78109-TEG-A310\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-TBA-A020\x00\x00', + b'36161-TBA-A030\x00\x00', + b'36161-TBA-A040\x00\x00', + b'36161-TBC-A020\x00\x00', + b'36161-TBC-A030\x00\x00', + b'36161-TED-Q320\x00\x00', + b'36161-TEG-A010\x00\x00', + b'36161-TEG-A020\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A010\x00\x00', + b'38897-TBA-A020\x00\x00', + ], + }, + CAR.CIVIC_BOSCH: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5AA-A940\x00\x00', + b'37805-5AA-A950\x00\x00', + b'37805-5AA-C950\x00\x00', + b'37805-5AA-L940\x00\x00', + b'37805-5AA-L950\x00\x00', + b'37805-5AG-Z910\x00\x00', + b'37805-5AJ-A750\x00\x00', + b'37805-5AJ-L750\x00\x00', + b'37805-5AK-T530\x00\x00', + b'37805-5AN-A750\x00\x00', + b'37805-5AN-A830\x00\x00', + b'37805-5AN-A840\x00\x00', + b'37805-5AN-A930\x00\x00', + b'37805-5AN-A940\x00\x00', + b'37805-5AN-A950\x00\x00', + b'37805-5AN-AG20\x00\x00', + b'37805-5AN-AH20\x00\x00', + b'37805-5AN-AJ30\x00\x00', + b'37805-5AN-AK10\x00\x00', + b'37805-5AN-AK20\x00\x00', + b'37805-5AN-AR10\x00\x00', + b'37805-5AN-AR20\x00\x00', + b'37805-5AN-CH20\x00\x00', + b'37805-5AN-E630\x00\x00', + b'37805-5AN-E720\x00\x00', + b'37805-5AN-E820\x00\x00', + b'37805-5AN-J820\x00\x00', + b'37805-5AN-L840\x00\x00', + b'37805-5AN-L930\x00\x00', + b'37805-5AN-L940\x00\x00', + b'37805-5AN-LF20\x00\x00', + b'37805-5AN-LH20\x00\x00', + b'37805-5AN-LJ20\x00\x00', + b'37805-5AN-LR20\x00\x00', + b'37805-5AN-LS20\x00\x00', + b'37805-5AW-G720\x00\x00', + b'37805-5AZ-E850\x00\x00', + b'37805-5AZ-G540\x00\x00', + b'37805-5AZ-G740\x00\x00', + b'37805-5AZ-G840\x00\x00', + b'37805-5BB-A530\x00\x00', + b'37805-5BB-A540\x00\x00', + b'37805-5BB-A630\x00\x00', + b'37805-5BB-A640\x00\x00', + b'37805-5BB-C540\x00\x00', + b'37805-5BB-C630\x00\x00', + b'37805-5BB-C640\x00\x00', + b'37805-5BB-L540\x00\x00', + b'37805-5BB-L630\x00\x00', + b'37805-5BB-L640\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5CG-A920\x00\x00', + b'28101-5CG-AB10\x00\x00', + b'28101-5CG-C110\x00\x00', + b'28101-5CG-C220\x00\x00', + b'28101-5CG-C320\x00\x00', + b'28101-5CG-G020\x00\x00', + b'28101-5CG-L020\x00\x00', + b'28101-5CK-A130\x00\x00', + b'28101-5CK-A140\x00\x00', + b'28101-5CK-A150\x00\x00', + b'28101-5CK-C130\x00\x00', + b'28101-5CK-C140\x00\x00', + b'28101-5CK-C150\x00\x00', + b'28101-5CK-G210\x00\x00', + b'28101-5CK-J710\x00\x00', + b'28101-5CK-Q610\x00\x00', + b'28101-5DJ-A610\x00\x00', + b'28101-5DJ-A710\x00\x00', + b'28101-5DV-E330\x00\x00', + b'28101-5DV-E610\x00\x00', + b'28101-5DV-E820\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TBG-A330\x00\x00', + b'57114-TBG-A340\x00\x00', + b'57114-TBG-A350\x00\x00', + b'57114-TGG-A340\x00\x00', + b'57114-TGG-C320\x00\x00', + b'57114-TGG-G320\x00\x00', + b'57114-TGG-L320\x00\x00', + b'57114-TGG-L330\x00\x00', + b'57114-TGK-T320\x00\x00', + b'57114-TGL-G330\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TBA-C020\x00\x00', + b'39990-TBA-C120\x00\x00', + b'39990-TEA-T820\x00\x00', + b'39990-TEZ-T020\x00\x00', + b'39990-TGG-A020\x00\x00', + b'39990-TGG-A120\x00\x00', + b'39990-TGG-J510\x00\x00', + b'39990-TGL-E130\x00\x00', + b'39990-TGN-E120\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TBA-A060\x00\x00', + b'77959-TBG-A050\x00\x00', + b'77959-TEA-G020\x00\x00', + b'77959-TGG-A020\x00\x00', + b'77959-TGG-A030\x00\x00', + b'77959-TGG-E010\x00\x00', + b'77959-TGG-G010\x00\x00', + b'77959-TGG-G110\x00\x00', + b'77959-TGG-J320\x00\x00', + b'77959-TGG-Z820\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TBA-A110\x00\x00', + b'78109-TBA-A910\x00\x00', + b'78109-TBA-C340\x00\x00', + b'78109-TBA-C910\x00\x00', + b'78109-TBC-A740\x00\x00', + b'78109-TBC-C540\x00\x00', + b'78109-TBG-A110\x00\x00', + b'78109-TBH-A710\x00\x00', + b'78109-TEG-A720\x00\x00', + b'78109-TFJ-G020\x00\x00', + b'78109-TGG-9020\x00\x00', + b'78109-TGG-A210\x00\x00', + b'78109-TGG-A220\x00\x00', + b'78109-TGG-A310\x00\x00', + b'78109-TGG-A320\x00\x00', + b'78109-TGG-A330\x00\x00', + b'78109-TGG-A610\x00\x00', + b'78109-TGG-A620\x00\x00', + b'78109-TGG-A810\x00\x00', + b'78109-TGG-A820\x00\x00', + b'78109-TGG-C220\x00\x00', + b'78109-TGG-E110\x00\x00', + b'78109-TGG-G030\x00\x00', + b'78109-TGG-G230\x00\x00', + b'78109-TGG-G410\x00\x00', + b'78109-TGK-Z410\x00\x00', + b'78109-TGL-G120\x00\x00', + b'78109-TGL-G130\x00\x00', + b'78109-TGL-G210\x00\x00', + b'78109-TGL-G230\x00\x00', + b'78109-TGL-GM10\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TBA-A150\x00\x00', + b'36802-TBA-A160\x00\x00', + b'36802-TFJ-G060\x00\x00', + b'36802-TGG-A050\x00\x00', + b'36802-TGG-A060\x00\x00', + b'36802-TGG-A130\x00\x00', + b'36802-TGG-G040\x00\x00', + b'36802-TGG-G130\x00\x00', + b'36802-TGK-Q120\x00\x00', + b'36802-TGL-G040\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TBA-A130\x00\x00', + b'36161-TBA-A140\x00\x00', + b'36161-TFJ-G070\x00\x00', + b'36161-TGG-A060\x00\x00', + b'36161-TGG-A080\x00\x00', + b'36161-TGG-A120\x00\x00', + b'36161-TGG-G050\x00\x00', + b'36161-TGG-G130\x00\x00', + b'36161-TGG-G140\x00\x00', + b'36161-TGK-Q120\x00\x00', + b'36161-TGL-G050\x00\x00', + b'36161-TGL-G070\x00\x00', + b'36161-TGG-G070\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A110\x00\x00', + b'38897-TBA-A020\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'39494-TGL-G030\x00\x00', + ], + }, + CAR.CIVIC_BOSCH_DIESEL: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-59N-G630\x00\x00', + b'37805-59N-G830\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-59Y-G220\x00\x00', + b'28101-59Y-G620\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TGN-E320\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TFK-G020\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TFK-G210\x00\x00', + b'77959-TGN-G220\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TFK-G020\x00\x00', + b'78109-TGN-G120\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TFK-G130\x00\x00', + b'36802-TGN-G130\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TGN-E010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TFK-G130\x00\x00', + b'36161-TGN-G130\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A020\x00\x00', + ], + }, + CAR.CRV: { + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T1W-A230\x00\x00', + b'57114-T1W-A240\x00\x00', + b'57114-TFF-A940\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T0A-A230\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T1W-A210\x00\x00', + b'78109-T1W-C210\x00\x00', + b'78109-T1X-A210\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T1W-A830\x00\x00', + b'36161-T1W-C830\x00\x00', + b'36161-T1X-A830\x00\x00', + ], + }, + CAR.CRV_5G: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5PA-AH20\x00\x00', + b'37805-5PA-3060\x00\x00', + b'37805-5PA-3080\x00\x00', + b'37805-5PA-3180\x00\x00', + b'37805-5PA-4050\x00\x00', + b'37805-5PA-4150\x00\x00', + b'37805-5PA-6520\x00\x00', + b'37805-5PA-6530\x00\x00', + b'37805-5PA-6630\x00\x00', + b'37805-5PA-6640\x00\x00', + b'37805-5PA-7630\x00\x00', + b'37805-5PA-9630\x00\x00', + b'37805-5PA-9640\x00\x00', + b'37805-5PA-9730\x00\x00', + b'37805-5PA-9830\x00\x00', + b'37805-5PA-9840\x00\x00', + b'37805-5PA-A650\x00\x00', + b'37805-5PA-A670\x00\x00', + b'37805-5PA-A680\x00\x00', + b'37805-5PA-A850\x00\x00', + b'37805-5PA-A870\x00\x00', + b'37805-5PA-A880\x00\x00', + b'37805-5PA-A890\x00\x00', + b'37805-5PA-AB10\x00\x00', + b'37805-5PA-AD10\x00\x00', + b'37805-5PA-AF20\x00\x00', + b'37805-5PA-C680\x00\x00', + b'37805-5PD-Q630\x00\x00', + b'37805-5PF-F730\x00\x00', + b'37805-5PF-M630\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5RG-A020\x00\x00', + b'28101-5RG-A030\x00\x00', + b'28101-5RG-A040\x00\x00', + b'28101-5RG-A120\x00\x00', + b'28101-5RG-A220\x00\x00', + b'28101-5RH-A020\x00\x00', + b'28101-5RH-A030\x00\x00', + b'28101-5RH-A040\x00\x00', + b'28101-5RH-A120\x00\x00', + b'28101-5RH-A220\x00\x00', + b'28101-5RL-Q010\x00\x00', + b'28101-5RM-F010\x00\x00', + b'28101-5RM-K010\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TLA-A040\x00\x00', + b'57114-TLA-A050\x00\x00', + b'57114-TLA-A060\x00\x00', + b'57114-TLB-A830\x00\x00', + b'57114-TMC-Z040\x00\x00', + b'57114-TMC-Z050\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TLA-A040\x00\x00', + b'39990-TLA-A110\x00\x00', + b'39990-TLA-A220\x00\x00', + b'39990-TLA,A040\x00\x00', # modified firmware + b'39990-TME-T030\x00\x00', + b'39990-TME-T120\x00\x00', + b'39990-TMT-T010\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TLA-A040\x00\x00', + b'46114-TLA-A050\x00\x00', + b'46114-TLA-A930\x00\x00', + b'46114-TMC-U020\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TLA-A110\x00\x00', + b'78109-TLA-A120\x00\x00', + b'78109-TLA-A210\x00\x00', + b'78109-TLA-A220\x00\x00', + b'78109-TLA-C020\x00\x00', + b'78109-TLA-C110\x00\x00', + b'78109-TLA-C210\x00\x00', + b'78109-TLA-C310\x00\x00', + b'78109-TLB-A020\x00\x00', + b'78109-TLB-A110\x00\x00', + b'78109-TLB-A120\x00\x00', + b'78109-TLB-A210\x00\x00', + b'78109-TLB-A220\x00\x00', + b'78109-TMC-Q210\x00\x00', + b'78109-TMM-F210\x00\x00', + b'78109-TMM-M110\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TLA-A010\x00\x00', + b'38897-TLA-A110\x00\x00', + b'38897-TNY-G010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TLA-A040\x00\x00', + b'36802-TLA-A050\x00\x00', + b'36802-TLA-A060\x00\x00', + b'36802-TMC-Q040\x00\x00', + b'36802-TMC-Q070\x00\x00', + b'36802-TNY-A030\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TLA-A060\x00\x00', + b'36161-TLA-A070\x00\x00', + b'36161-TLA-A080\x00\x00', + b'36161-TMC-Q020\x00\x00', + b'36161-TMC-Q030\x00\x00', + b'36161-TMC-Q040\x00\x00', + b'36161-TNY-A020\x00\x00', + b'36161-TNY-A030\x00\x00', + b'36161-TNY-A040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TLA-A240\x00\x00', + b'77959-TLA-A250\x00\x00', + b'77959-TLA-A320\x00\x00', + b'77959-TLA-A410\x00\x00', + b'77959-TLA-A420\x00\x00', + b'77959-TLA-Q040\x00\x00', + b'77959-TLA-Z040\x00\x00', + b'77959-TMM-F040\x00\x00', + ], + }, + CAR.CRV_EU: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-R5Z-G740\x00\x00', + b'37805-R5Z-G780\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [b'57114-T1V-G920\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T1V-G520\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-T1V-G010\x00\x00'], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5LH-E120\x00\x00', + b'28103-5LH-E100\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T1V-G020\x00\x00', + b'78109-T1B-3050\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [b'77959-T1G-G940\x00\x00'], + }, + CAR.CRV_HYBRID: { + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TPA-G020\x00\x00', + b'57114-TPG-A020\x00\x00', + b'57114-TMB-H030\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TPA-G030\x00\x00', + b'39990-TPG-A020\x00\x00', + b'39990-TMA-H020\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TMA-H110\x00\x00', + b'38897-TPG-A110\x00\x00', + b'38897-TPG-A210\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TMB-H510\x00\x00', + b'54008-TMB-H610\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TMB-H040\x00\x00', + b'36161-TPA-E050\x00\x00', + b'36161-TPG-A030\x00\x00', + b'36161-TPG-A040\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TMB-H220\x00\x00', + b'78109-TPA-G520\x00\x00', + b'78109-TPG-A110\x00\x00', + b'78109-TPG-A210\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TLA-X010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TPA-E040\x00\x00', + b'36802-TPG-A020\x00\x00', + b'36802-TMB-H040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TLA-C320\x00\x00', + b'77959-TLA-C410\x00\x00', + b'77959-TLA-C420\x00\x00', + b'77959-TLA-G220\x00\x00', + b'77959-TLA-H240\x00\x00', + ], + }, + CAR.FIT: { + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T5R-L020\x00\x00', + b'57114-T5R-L220\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T5R-C020\x00\x00', + b'39990-T5R-C030\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T5A-J010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T5A-A210\x00\x00', + b'78109-T5A-A410\x00\x00', + b'78109-T5A-A420\x00\x00', + b'78109-T5A-A910\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T5R-A040\x00\x00', + b'36161-T5R-A240\x00\x00', + b'36161-T5R-A520\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T5R-A230\x00\x00', + ], + }, + CAR.FREED: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TDK-J010\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TDK-J050\x00\x00', + b'39990-TDK-N020\x00\x00', + ], + # TODO: vsa is "essential" for fpv2 but doesn't appear on some models + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TDK-J120\x00\x00', + b'57114-TDK-J330\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TDK-J310\x00\x00', + b'78109-TDK-J320\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-TDK-J070\x00\x00', + b'36161-TDK-J080\x00\x00', + b'36161-TDK-J530\x00\x00', + ], + }, + CAR.ODYSSEY: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-THR-A010\x00\x00', + b'38897-THR-A020\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5MR-4080\x00\x00', + b'37805-5MR-A240\x00\x00', + b'37805-5MR-A250\x00\x00', + b'37805-5MR-A310\x00\x00', + b'37805-5MR-A740\x00\x00', + b'37805-5MR-A750\x00\x00', + b'37805-5MR-A840\x00\x00', + b'37805-5MR-C620\x00\x00', + b'37805-5MR-D530\x00\x00', + b'37805-5MR-K730\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-THR-A020\x00\x00', + b'39990-THR-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-THR-A010\x00\x00', + b'77959-THR-A110\x00\x00', + b'77959-THR-X010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-THR-A020\x00\x00', + b'36161-THR-A030\x00\x00', + b'36161-THR-A110\x00\x00', + b'36161-THR-A720\x00\x00', + b'36161-THR-A730\x00\x00', + b'36161-THR-A810\x00\x00', + b'36161-THR-A910\x00\x00', + b'36161-THR-C010\x00\x00', + b'36161-THR-D110\x00\x00', + b'36161-THR-K020\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5NZ-A110\x00\x00', + b'28101-5NZ-A310\x00\x00', + b'28101-5NZ-C310\x00\x00', + b'28102-5MX-A001\x00\x00', + b'28102-5MX-A600\x00\x00', + b'28102-5MX-A610\x00\x00', + b'28102-5MX-A710\x00\x00', + b'28102-5MX-A900\x00\x00', + b'28102-5MX-A910\x00\x00', + b'28102-5MX-C001\x00\x00', + b'28102-5MX-D001\x00\x00', + b'28102-5MX-D710\x00\x00', + b'28102-5MX-K610\x00\x00', + b'28103-5NZ-A100\x00\x00', + b'28103-5NZ-A300\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-THR-A040\x00\x00', + b'57114-THR-A110\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-THR-A220\x00\x00', + b'78109-THR-A230\x00\x00', + b'78109-THR-A420\x00\x00', + b'78109-THR-A430\x00\x00', + b'78109-THR-A720\x00\x00', + b'78109-THR-A820\x00\x00', + b'78109-THR-A830\x00\x00', + b'78109-THR-AB20\x00\x00', + b'78109-THR-AB30\x00\x00', + b'78109-THR-AB40\x00\x00', + b'78109-THR-AC20\x00\x00', + b'78109-THR-AC30\x00\x00', + b'78109-THR-AC40\x00\x00', + b'78109-THR-AC50\x00\x00', + b'78109-THR-AD30\x00\x00', + b'78109-THR-AE20\x00\x00', + b'78109-THR-AE30\x00\x00', + b'78109-THR-AE40\x00\x00', + b'78109-THR-AK10\x00\x00', + b'78109-THR-AL10\x00\x00', + b'78109-THR-AN10\x00\x00', + b'78109-THR-C220\x00\x00', + b'78109-THR-C330\x00\x00', + b'78109-THR-CE20\x00\x00', + b'78109-THR-DA20\x00\x00', + b'78109-THR-DA30\x00\x00', + b'78109-THR-DA40\x00\x00', + b'78109-THR-K120\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-THR-A020\x00\x00', + ], + }, + CAR.PILOT: { + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TG7-A520\x00\x00', + b'54008-TG7-A530\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5EY-A050\x00\x00', + b'28101-5EY-A100\x00\x00', + b'28101-5EZ-A050\x00\x00', + b'28101-5EZ-A060\x00\x00', + b'28101-5EZ-A100\x00\x00', + b'28101-5EZ-A210\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-RLV-4060\x00\x00', + b'37805-RLV-4070\x00\x00', + b'37805-RLV-A830\x00\x00', + b'37805-RLV-A840\x00\x00', + b'37805-RLV-C430\x00\x00', + b'37805-RLV-C510\x00\x00', + b'37805-RLV-C520\x00\x00', + b'37805-RLV-C530\x00\x00', + b'37805-RLV-C910\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A030\x00\x00', + b'38897-TG7-A040\x00\x00', + b'38897-TG7-A110\x00\x00', + b'38897-TG7-A210\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TG7-A030\x00\x00', + b'39990-TG7-A040\x00\x00', + b'39990-TG7-A060\x00\x00', + b'39990-TG7-A070\x00\x00', + b'39990-TGS-A230\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-TG7-A310\x00\x00', + b'36161-TG7-A520\x00\x00', + b'36161-TG7-A630\x00\x00', + b'36161-TG7-A720\x00\x00', + b'36161-TG7-A820\x00\x00', + b'36161-TG7-A930\x00\x00', + b'36161-TG7-C520\x00\x00', + b'36161-TG7-D520\x00\x00', + b'36161-TG7-D630\x00\x00', + b'36161-TG7-Y630\x00\x00', + b'36161-TG8-A520\x00\x00', + b'36161-TG8-A630\x00\x00', + b'36161-TG8-A720\x00\x00', + b'36161-TG8-A830\x00\x00', + b'36161-TGS-A130\x00\x00', + b'36161-TGT-A030\x00\x00', + b'36161-TGT-A130\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TG7-A020\x00\x00', + b'77959-TG7-A110\x00\x00', + b'77959-TG7-A210\x00\x00', + b'77959-TG7-Y210\x00\x00', + b'77959-TGS-A010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TG7-A040\x00\x00', + b'78109-TG7-A050\x00\x00', + b'78109-TG7-A420\x00\x00', + b'78109-TG7-A520\x00\x00', + b'78109-TG7-A720\x00\x00', + b'78109-TG7-AJ10\x00\x00', + b'78109-TG7-AJ20\x00\x00', + b'78109-TG7-AK10\x00\x00', + b'78109-TG7-AK20\x00\x00', + b'78109-TG7-AM20\x00\x00', + b'78109-TG7-AP10\x00\x00', + b'78109-TG7-AP20\x00\x00', + b'78109-TG7-AS20\x00\x00', + b'78109-TG7-AT20\x00\x00', + b'78109-TG7-AU20\x00\x00', + b'78109-TG7-AX20\x00\x00', + b'78109-TG7-D020\x00\x00', + b'78109-TG7-DJ10\x00\x00', + b'78109-TG7-YK20\x00\x00', + b'78109-TG8-A420\x00\x00', + b'78109-TG8-A520\x00\x00', + b'78109-TG8-AJ10\x00\x00', + b'78109-TG8-AJ20\x00\x00', + b'78109-TG8-AK20\x00\x00', + b'78109-TGS-AK20\x00\x00', + b'78109-TGS-AP20\x00\x00', + b'78109-TGT-AJ20\x00\x00', + b'78109-TGT-AK30\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TG7-A130\x00\x00', + b'57114-TG7-A140\x00\x00', + b'57114-TG7-A230\x00\x00', + b'57114-TG7-A240\x00\x00', + b'57114-TG7-A630\x00\x00', + b'57114-TG7-A730\x00\x00', + b'57114-TG8-A140\x00\x00', + b'57114-TG8-A240\x00\x00', + b'57114-TG8-A630\x00\x00', + b'57114-TG8-A730\x00\x00', + b'57114-TGS-A530\x00\x00', + b'57114-TGT-A530\x00\x00', + ], + }, + CAR.PASSPORT: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-RLV-B220\x00\x00', + b'37805-RLV-B210\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TGS-A230\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-TGS-A030\x00\x00', + b'36161-TGS-A130\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TGS-A010\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TG7-A530\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5EZ-A600\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TGS-AT20\x00\x00', + b'78109-TGS-AX20\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TGS-A530\x00\x00', + ], + }, + CAR.ACURA_RDX: { + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TX5-A220\x00\x00', + b'57114-TX4-A220\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-TX5-A030\x00\x00', + b'36161-TX4-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TX4-C010\x00\x00', + b'77959-TX4-B010\x00\x00', + b'77959-TX4-C020\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TX5-A310\x00\x00', + b'78109-TX4-A210\x00\x00', + b'78109-TX4-A310\x00\x00', + ], + }, + CAR.ACURA_RDX_3G: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-5YF-A130\x00\x00', + b'37805-5YF-A230\x00\x00', + b'37805-5YF-A320\x00\x00', + b'37805-5YF-A330\x00\x00', + b'37805-5YF-A420\x00\x00', + b'37805-5YF-A430\x00\x00', + b'37805-5YF-A750\x00\x00', + b'37805-5YF-A850\x00\x00', + b'37805-5YF-A870\x00\x00', + b'37805-5YF-C210\x00\x00', + b'37805-5YF-C220\x00\x00', + b'37805-5YF-C410\000\000', + b'37805-5YF-C420\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TJB-A030\x00\x00', + b'57114-TJB-A040\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TJB-A040\x00\x00', + b'36802-TJB-A050\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TJB-A040\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TJB-A520\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28102-5YK-A610\x00\x00', + b'28102-5YK-A620\x00\x00', + b'28102-5YK-A630\x00\x00', + b'28102-5YK-A700\x00\x00', + b'28102-5YK-A711\x00\x00', + b'28102-5YL-A620\x00\x00', + b'28102-5YL-A700\x00\x00', + b'28102-5YL-A711\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TJB-A140\x00\x00', + b'78109-TJB-A240\x00\x00', + b'78109-TJB-A420\x00\x00', + b'78109-TJB-AB10\x00\x00', + b'78109-TJB-AD10\x00\x00', + b'78109-TJB-AF10\x00\x00', + b'78109-TJB-AR10\x00\x00', + b'78109-TJB-AS10\000\000', + b'78109-TJB-AU10\x00\x00', + b'78109-TJB-AW10\x00\x00', + b'78109-TJC-A420\x00\x00', + b'78109-TJC-AA10\x00\x00', + b'78109-TJC-AD10\x00\x00', + b'78109-TJC-AF10\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TJB-A040\x00\x00', + b'77959-TJB-A210\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TJB-A040\x00\x00', + b'46114-TJB-A050\x00\x00', + b'46114-TJB-A060\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TJB-A040\x00\x00', + b'38897-TJB-A110\x00\x00', + b'38897-TJB-A120\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TJB-A030\x00\x00', + b'39990-TJB-A040\x00\x00', + b'39990-TJB-A130\x00\x00' + ], + }, + CAR.RIDGELINE: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T6Z-A020\x00\x00', + b'39990-T6Z-A030\x00\x00', + b'39990-T6Z-A050\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-T6Z-A020\x00\x00', + b'36161-T6Z-A310\x00\x00', + b'36161-T6Z-A420\x00\x00', + b'36161-T6Z-A520\x00\x00', + b'36161-T6Z-A620\x00\x00', + b'36161-TJZ-A120\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T6Z-A010\x00\x00', + b'38897-T6Z-A110\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T6Z-A420\x00\x00', + b'78109-T6Z-A510\x00\x00', + b'78109-T6Z-A710\x00\x00', + b'78109-T6Z-A810\x00\x00', + b'78109-T6Z-A910\x00\x00', + b'78109-T6Z-AA10\x00\x00', + b'78109-T6Z-C620\x00\x00', + b'78109-TJZ-A510\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T6Z-A020\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T6Z-A120\x00\x00', + b'57114-T6Z-A130\x00\x00', + b'57114-T6Z-A520\x00\x00', + b'57114-TJZ-A520\x00\x00', + ], + }, + CAR.INSIGHT: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TXM-A040\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TXM-A070\x00\x00', + b'36802-TXM-A080\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TXM-A050\x00\x00', + b'36161-TXM-A060\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TXM-A230\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TXM-A030\x00\x00', + b'57114-TXM-A040\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TWA-A910\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TXM-A020\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TXM-A010\x00\x00', + b'78109-TXM-A020\x00\x00', + b'78109-TXM-A110\x00\x00', + b'78109-TXM-C010\x00\x00', + b'78109-TXM-A030\x00\x00', + ], + }, + CAR.HRV: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T7A-A010\x00\x00', + b'38897-T7A-A110\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-THX-A020\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-T7A-A140\x00\x00', + b'36161-T7A-A240\x00\x00', + b'36161-T7A-C440\x00\x00', + b'36161-T7A-A040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T7A-A230\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-THX-A110\x00\x00', + b'78109-THX-A120\x00\x00', + b'78109-THX-A210\x00\x00', + b'78109-THX-A220\x00\x00', + b'78109-THX-C220\x00\x00', + b'78109-THW-A110\x00\x00', + ], + }, + CAR.ACURA_ILX: { + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TX6-A010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36161-TV9-A140\x00\x00', + b'36161-TX6-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TX6-A230\x00\x00', + b'77959-TX6-C210\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T3R-A120\x00\x00', + b'78109-T3R-A410\x00\x00', + b'78109-TV9-A510\x00\x00', + ], + }, + CAR.HONDA_E:{ + (Ecu.eps, 0x18DA30F1, None):[ + b'39990-TYF-N030\x00\x00' + ], + (Ecu.gateway, 0x18DAEFF1, None):[ + b'38897-TYF-E140\x00\x00' + ], + (Ecu.shiftByWire, 0x18DA0BF1, None):[ + b'54008-TYF-E010\x00\x00' + ], + (Ecu.srs, 0x18DA53F1, None):[ + b'77959-TYF-G430\x00\x00' + ], + (Ecu.combinationMeter, 0x18DA60F1, None):[ + b'78108-TYF-G610\x00\x00' + ], + (Ecu.fwdRadar, 0x18DAB0F1, None):[ + b'36802-TYF-E030\x00\x00' + ], + (Ecu.fwdCamera, 0x18DAB5F1, None):[ + b'36161-TYF-E020\x00\x00' + ], + (Ecu.vsa, 0x18DA28F1, None):[ + b'57114-TYF-E030\x00\x00' + ], + }, + CAR.CIVIC_2022: { + (Ecu.eps, 0x18DA30F1, None): [ + b'39990-T39-A130\x00\x00', + b'39990-T43-J020\x00\x00', + ], + (Ecu.gateway, 0x18DAEFF1, None): [ + b'38897-T20-A020\x00\x00', + b'38897-T20-A510\x00\x00', + b'38897-T21-A010\x00\x00', + b'38897-T20-A210\x00\x00', + b'38897-T20-A310\x00\x00', + ], + (Ecu.srs, 0x18DA53F1, None): [ + b'77959-T20-A970\x00\x00', + b'77959-T47-A940\x00\x00', + b'77959-T47-A950\x00\x00', + ], + (Ecu.combinationMeter, 0x18DA60F1, None): [ + b'78108-T21-A220\x00\x00', + b'78108-T21-A620\x00\x00', + b'78108-T23-A110\x00\x00', + b'78108-T21-A230\x00\x00', + b'78108-T22-A020\x00\x00', + ], + (Ecu.vsa, 0x18DA28F1, None): [ + b'57114-T20-AB40\x00\x00', + b'57114-T43-JB30\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-65D-A020\x00\x00', + b'28101-65D-A120\x00\x00', + b'28101-65H-A020\x00\x00', + b'28101-65H-A120\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-64L-A540\x00\x00', + b'37805-64S-A540\x00\x00', + b'37805-64S-A720\x00\x00', + b'37805-64A-A540\x00\x00', + b'37805-64A-A620\x00\x00', + ], + }, +} + +DBC = { + CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None), + CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None), + CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None), + CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_accord_2018_can_generated', None), + CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), + CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None), + CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), + CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), + CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.PASSPORT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), + CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), + CAR.CIVIC_2022: dbc_dict('honda_civic_ex_2022_can_generated', None), +} + +STEER_THRESHOLD = { + # default is 1200, overrides go here + CAR.ACURA_RDX: 400, + CAR.CRV_EU: 400, +} + +HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} +HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, + CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE} +HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, + CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022} +HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} +HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022} diff --git a/system/loggerd/__init__.py b/selfdrive/car/hyundai/__init__.py similarity index 100% rename from system/loggerd/__init__.py rename to selfdrive/car/hyundai/__init__.py diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py new file mode 100644 index 00000000000000..db80bcdf4846cd --- /dev/null +++ b/selfdrive/car/hyundai/carcontroller.py @@ -0,0 +1,158 @@ +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import clip +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.hyundai import hyundaicanfd, hyundaican +from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR + +VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState + + +def process_hud_alert(enabled, fingerprint, hud_control): + sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) + + # initialize to no line visible + # TODO: this is not accurate for all cars + sys_state = 1 + if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active + sys_state = 3 if enabled or sys_warning else 4 + elif hud_control.leftLaneVisible: + sys_state = 5 + elif hud_control.rightLaneVisible: + sys_state = 6 + + # initialize to no warnings + left_lane_warning = 0 + right_lane_warning = 0 + if hud_control.leftLaneDepart: + left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2 + if hud_control.rightLaneDepart: + right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2 + + return sys_warning, sys_state, left_lane_warning, right_lane_warning + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.params = CarControllerParams(CP) + self.packer = CANPacker(dbc_name) + self.frame = 0 + + self.apply_steer_last = 0 + self.car_fingerprint = CP.carFingerprint + self.last_button_frame = 0 + self.accel = 0 + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + + # Steering Torque + + # These cars have significantly more torque than most HKG. Limit to 70% of max. + steer = actuators.steer + if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + steer = clip(steer, -0.7, 0.7) + new_steer = int(round(steer * self.params.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + + if not CC.latActive: + apply_steer = 0 + + self.apply_steer_last = apply_steer + + sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, + hud_control) + + can_sends = [] + + if self.CP.carFingerprint in CANFD_CAR: + # steering control + can_sends.append(hyundaicanfd.create_lkas(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer)) + + # block LFA on HDA2 + if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.CANFD_HDA2): + can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4)) + + # LFA and HDA icons + if self.frame % 2 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_HDA2): + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CC.enabled)) + + # button presses + if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: + # cruise cancel + if CC.cruiseControl.cancel: + if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: + can_sends.append(hyundaicanfd.create_cruise_info(self.packer, CS.cruise_info_copy, True)) + self.last_button_frame = self.frame + else: + for _ in range(20): + can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL)) + self.last_button_frame = self.frame + + # cruise standstill resume + elif CC.cruiseControl.resume: + if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): + can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL)) + self.last_button_frame = self.frame + else: + + # tester present - w/ no response (keeps radar disabled) + if self.CP.openpilotLongitudinalControl: + if self.frame % 100 == 0: + can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) + + can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive, + CS.lkas11, sys_warning, sys_state, CC.enabled, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + left_lane_warning, right_lane_warning)) + + if not self.CP.openpilotLongitudinalControl: + if CC.cruiseControl.cancel: + can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint)) + elif CC.cruiseControl.resume: + # send resume at a max freq of 10Hz + if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: + # send 25 messages at a time to increases the likelihood of resume being accepted + can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25) + self.last_button_frame = self.frame + + if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: + accel = actuators.accel + + #TODO unclear if this is needed + jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 + + accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + stopping = actuators.longControlState == LongCtrlState.stopping + set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), + hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed)) + self.accel = accel + + # 20 Hz LFA MFA message + if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, + CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022, + CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, + CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): + can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) + + # 5 Hz ACC options + if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: + can_sends.extend(hyundaican.create_acc_opt(self.packer)) + + # 2 Hz front radar options + if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: + can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) + + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / self.params.STEER_MAX + new_actuators.accel = self.accel + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py new file mode 100644 index 00000000000000..eab6e73f1f429b --- /dev/null +++ b/selfdrive/car/hyundai/carstate.py @@ -0,0 +1,473 @@ +from collections import deque +import copy + +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams +from selfdrive.car.interfaces import CarStateBase + +PREV_BUTTON_SAMPLES = 8 + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) + self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) + + if CP.carFingerprint in CANFD_CAR: + self.shifter_values = can_define.dv["GEAR_SHIFTER"]["GEAR"] + elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"] + elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + self.shifter_values = can_define.dv["TCU12"]["CUR_GR"] + else: # preferred and elect gear methods use same definition + self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] + + self.brake_error = False + self.park_brake = False + self.buttons_counter = 0 + + self.params = CarControllerParams(CP) + + def update(self, cp, cp_cam): + if self.CP.carFingerprint in CANFD_CAR: + return self.update_canfd(cp, cp_cam) + + ret = car.CarState.new_message() + + cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp + + ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"], + cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]]) + + ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0 + + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHL_SPD11"]["WHL_SPD_FL"], + cp.vl["WHL_SPD11"]["WHL_SPD_FR"], + cp.vl["WHL_SPD11"]["WHL_SPD_RL"], + cp.vl["WHL_SPD11"]["WHL_SPD_RR"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + + ret.standstill = ret.vEgoRaw < 0.1 + + ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] + ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"] + ret.yawRate = cp.vl["ESP12"]["YAW_RATE"] + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp( + 50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"]) + ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"] + ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"] + ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0 + + # cruise state + if self.CP.openpilotLongitudinalControl: + # These are not used for engage/disengage since openpilot keeps track of state using the buttons + ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 + ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 + ret.cruiseState.standstill = False + else: + ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1 + ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0 + ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4. + speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS + ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv + + # TODO: Find brake pressure + ret.brake = 0 + ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0 + ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY + ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 + + if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR): + if self.CP.carFingerprint in HYBRID_CAR: + ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254. + else: + ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254. + ret.gasPressed = ret.gas > 0 + else: + ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100. + ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"]) + + # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, + # as this seems to be standard over all cars, but is not the preferred method. + if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + gear = cp.vl["CLU15"]["CF_Clu_Gear"] + elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + gear = cp.vl["TCU12"]["CUR_GR"] + elif self.CP.carFingerprint in FEATURES["use_elect_gears"]: + gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"] + else: + gear = cp.vl["LVR12"]["CF_Lvr_Gear"] + + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) + + if not self.CP.openpilotLongitudinalControl: + if self.CP.carFingerprint in FEATURES["use_fca"]: + ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0 + ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2 + else: + ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0 + ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2 + + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 + ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0 + + # save the entire LKAS11 and CLU11 + self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) + self.clu11 = copy.copy(cp.vl["CLU11"]) + self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE + self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED + self.prev_cruise_buttons = self.cruise_buttons[-1] + self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) + self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + + return ret + + def update_canfd(self, cp, cp_cam): + ret = car.CarState.new_message() + + if self.CP.flags & HyundaiFlags.CANFD_HDA2: + ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255. + else: + ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023. + ret.gasPressed = ret.gas > 1e-5 + ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1 + + ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1 + ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0 + + gear = cp.vl["GEAR_SHIFTER"]["GEAR"] + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) + + # TODO: figure out positions + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.1 + + ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"] + ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1 + ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] + ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] + ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], + cp.vl["BLINKERS"]["RIGHT_LAMP"]) + + ret.cruiseState.available = True + ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1 + cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam + speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS + ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor + ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1 + + cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" + self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) + self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) + self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] + self.cruise_info_copy = copy.copy(cp_cruise_info.vl["CRUISE_INFO"]) + + if self.CP.flags & HyundaiFlags.CANFD_HDA2: + self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"]) + + return ret + + @staticmethod + def get_can_parser(CP): + if CP.carFingerprint in CANFD_CAR: + return CarState.get_can_parser_canfd(CP) + + signals = [ + # signal_name, signal_address + ("WHL_SPD_FL", "WHL_SPD11"), + ("WHL_SPD_FR", "WHL_SPD11"), + ("WHL_SPD_RL", "WHL_SPD11"), + ("WHL_SPD_RR", "WHL_SPD11"), + + ("YAW_RATE", "ESP12"), + + ("CF_Gway_DrvSeatBeltInd", "CGW4"), + + ("CF_Gway_DrvSeatBeltSw", "CGW1"), + ("CF_Gway_DrvDrSw", "CGW1"), # Driver Door + ("CF_Gway_AstDrSw", "CGW1"), # Passenger Door + ("CF_Gway_RLDrSw", "CGW2"), # Rear left Door + ("CF_Gway_RRDrSw", "CGW2"), # Rear right Door + ("CF_Gway_TurnSigLh", "CGW1"), + ("CF_Gway_TurnSigRh", "CGW1"), + ("CF_Gway_ParkBrakeSw", "CGW1"), + + ("CYL_PRES", "ESP12"), + + ("CF_Clu_CruiseSwState", "CLU11"), + ("CF_Clu_CruiseSwMain", "CLU11"), + ("CF_Clu_SldMainSW", "CLU11"), + ("CF_Clu_ParityBit1", "CLU11"), + ("CF_Clu_VanzDecimal" , "CLU11"), + ("CF_Clu_Vanz", "CLU11"), + ("CF_Clu_SPEED_UNIT", "CLU11"), + ("CF_Clu_DetentOut", "CLU11"), + ("CF_Clu_RheostatLevel", "CLU11"), + ("CF_Clu_CluInfo", "CLU11"), + ("CF_Clu_AmpInfo", "CLU11"), + ("CF_Clu_AliveCnt1", "CLU11"), + + ("ACCEnable", "TCS13"), + ("ACC_REQ", "TCS13"), + ("DriverBraking", "TCS13"), + ("StandStill", "TCS13"), + ("PBRAKE_ACT", "TCS13"), + + ("ESC_Off_Step", "TCS15"), + ("AVH_LAMP", "TCS15"), + + ("CR_Mdps_StrColTq", "MDPS12"), + ("CF_Mdps_ToiActive", "MDPS12"), + ("CF_Mdps_ToiUnavail", "MDPS12"), + ("CF_Mdps_ToiFlt", "MDPS12"), + ("CR_Mdps_OutTq", "MDPS12"), + + ("SAS_Angle", "SAS11"), + ("SAS_Speed", "SAS11"), + ] + checks = [ + # address, frequency + ("MDPS12", 50), + ("TCS13", 50), + ("TCS15", 10), + ("CLU11", 50), + ("ESP12", 100), + ("CGW1", 10), + ("CGW2", 5), + ("CGW4", 5), + ("WHL_SPD11", 50), + ("SAS11", 100), + ] + + if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR: + signals += [ + ("MainMode_ACC", "SCC11"), + ("VSetDis", "SCC11"), + ("SCCInfoDisplay", "SCC11"), + ("ACC_ObjDist", "SCC11"), + ("ACCMode", "SCC12"), + ] + checks += [ + ("SCC11", 50), + ("SCC12", 50), + ] + + if CP.carFingerprint in FEATURES["use_fca"]: + signals += [ + ("FCA_CmdAct", "FCA11"), + ("CF_VSM_Warn", "FCA11"), + ] + checks.append(("FCA11", 50)) + else: + signals += [ + ("AEB_CmdAct", "SCC12"), + ("CF_VSM_Warn", "SCC12"), + ] + + if CP.enableBsm: + signals += [ + ("CF_Lca_IndLeft", "LCA11"), + ("CF_Lca_IndRight", "LCA11"), + ] + checks.append(("LCA11", 50)) + + if CP.carFingerprint in (HYBRID_CAR | EV_CAR): + if CP.carFingerprint in HYBRID_CAR: + signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11")) + else: + signals.append(("Accel_Pedal_Pos", "E_EMS11")) + checks.append(("E_EMS11", 50)) + else: + signals += [ + ("PV_AV_CAN", "EMS12"), + ("CF_Ems_AclAct", "EMS16"), + ] + checks += [ + ("EMS12", 100), + ("EMS16", 100), + ] + + if CP.carFingerprint in FEATURES["use_cluster_gears"]: + signals.append(("CF_Clu_Gear", "CLU15")) + checks.append(("CLU15", 5)) + elif CP.carFingerprint in FEATURES["use_tcu_gears"]: + signals.append(("CUR_GR", "TCU12")) + checks.append(("TCU12", 100)) + elif CP.carFingerprint in FEATURES["use_elect_gears"]: + signals.append(("Elect_Gear_Shifter", "ELECT_GEAR")) + checks.append(("ELECT_GEAR", 20)) + else: + signals.append(("CF_Lvr_Gear", "LVR12")) + checks.append(("LVR12", 100)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + if CP.carFingerprint in CANFD_CAR: + return CarState.get_cam_can_parser_canfd(CP) + + signals = [ + # signal_name, signal_address + ("CF_Lkas_LdwsActivemode", "LKAS11"), + ("CF_Lkas_LdwsSysState", "LKAS11"), + ("CF_Lkas_SysWarning", "LKAS11"), + ("CF_Lkas_LdwsLHWarning", "LKAS11"), + ("CF_Lkas_LdwsRHWarning", "LKAS11"), + ("CF_Lkas_HbaLamp", "LKAS11"), + ("CF_Lkas_FcwBasReq", "LKAS11"), + ("CF_Lkas_HbaSysState", "LKAS11"), + ("CF_Lkas_FcwOpt", "LKAS11"), + ("CF_Lkas_HbaOpt", "LKAS11"), + ("CF_Lkas_FcwSysState", "LKAS11"), + ("CF_Lkas_FcwCollisionWarning", "LKAS11"), + ("CF_Lkas_FusionState", "LKAS11"), + ("CF_Lkas_FcwOpt_USM", "LKAS11"), + ("CF_Lkas_LdwsOpt_USM", "LKAS11"), + ] + checks = [ + ("LKAS11", 100) + ] + + if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR: + signals += [ + ("MainMode_ACC", "SCC11"), + ("VSetDis", "SCC11"), + ("SCCInfoDisplay", "SCC11"), + ("ACC_ObjDist", "SCC11"), + ("ACCMode", "SCC12"), + ] + checks += [ + ("SCC11", 50), + ("SCC12", 50), + ] + + if CP.carFingerprint in FEATURES["use_fca"]: + signals += [ + ("FCA_CmdAct", "FCA11"), + ("CF_VSM_Warn", "FCA11"), + ] + checks.append(("FCA11", 50)) + else: + signals += [ + ("AEB_CmdAct", "SCC12"), + ("CF_VSM_Warn", "SCC12"), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_can_parser_canfd(CP): + + cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" + signals = [ + ("WHEEL_SPEED_1", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_2", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_3", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_4", "WHEEL_SPEEDS"), + + ("GEAR", "GEAR_SHIFTER"), + ("BRAKE_PRESSED", "BRAKE"), + + ("STEERING_RATE", "STEERING_SENSORS"), + ("STEERING_ANGLE", "STEERING_SENSORS"), + ("STEERING_COL_TORQUE", "MDPS"), + ("STEERING_OUT_TORQUE", "MDPS"), + + ("CRUISE_ACTIVE", "SCC1"), + ("COUNTER", cruise_btn_msg), + ("CRUISE_BUTTONS", cruise_btn_msg), + ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), + + ("DISTANCE_UNIT", "CLUSTER_INFO"), + + ("LEFT_LAMP", "BLINKERS"), + ("RIGHT_LAMP", "BLINKERS"), + + ("DRIVER_DOOR_OPEN", "DOORS_SEATBELTS"), + ("DRIVER_SEATBELT_LATCHED", "DOORS_SEATBELTS"), + ] + + checks = [ + ("WHEEL_SPEEDS", 100), + ("GEAR_SHIFTER", 100), + ("BRAKE", 100), + ("STEERING_SENSORS", 100), + ("MDPS", 100), + ("SCC1", 50), + (cruise_btn_msg, 50), + ("CLUSTER_INFO", 4), + ("BLINKERS", 4), + ("DOORS_SEATBELTS", 4), + ] + + if CP.flags & HyundaiFlags.CANFD_HDA2: + signals += [ + ("ACCELERATOR_PEDAL", "ACCELERATOR"), + ("GEAR", "ACCELERATOR"), + ("SET_SPEED", "CRUISE_INFO"), + ("CRUISE_STANDSTILL", "CRUISE_INFO"), + ] + checks += [ + ("CRUISE_INFO", 50), + ("ACCELERATOR", 100), + ] + else: + signals += [ + ("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"), + ] + checks += [ + ("ACCELERATOR_ALT", 100), + ] + + bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus) + + @staticmethod + def get_cam_can_parser_canfd(CP): + if CP.flags & HyundaiFlags.CANFD_HDA2: + signals = [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)] + checks = [("CAM_0x2a4", 20)] + else: + signals = [ + ("COUNTER", "CRUISE_INFO"), + ("NEW_SIGNAL_1", "CRUISE_INFO"), + ("CRUISE_MAIN", "CRUISE_INFO"), + ("CRUISE_STATUS", "CRUISE_INFO"), + ("CRUISE_INACTIVE", "CRUISE_INFO"), + ("NEW_SIGNAL_2", "CRUISE_INFO"), + ("CRUISE_STANDSTILL", "CRUISE_INFO"), + ("NEW_SIGNAL_3", "CRUISE_INFO"), + ("BYTE11", "CRUISE_INFO"), + ("SET_SPEED", "CRUISE_INFO"), + ("NEW_SIGNAL_4", "CRUISE_INFO"), + ] + + signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(3, 7)] + signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(13, 31)] + + checks = [ + ("CRUISE_INFO", 50), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py new file mode 100644 index 00000000000000..df5cb6ae6edd24 --- /dev/null +++ b/selfdrive/car/hyundai/hyundaican.py @@ -0,0 +1,173 @@ +import crcmod +from selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR + +hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) + +def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, + lkas11, sys_warning, sys_state, enabled, + left_lane, right_lane, + left_lane_depart, right_lane_depart): + values = lkas11 + values["CF_Lkas_LdwsSysState"] = sys_state + values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0 + values["CF_Lkas_LdwsLHWarning"] = left_lane_depart + values["CF_Lkas_LdwsRHWarning"] = right_lane_depart + values["CR_Lkas_StrToqReq"] = apply_steer + values["CF_Lkas_ActToi"] = steer_req + values["CF_Lkas_MsgCount"] = frame % 0x10 + + if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, + CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, + CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, + CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): + values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) + values["CF_Lkas_LdwsOpt_USM"] = 2 + + # FcwOpt_USM 5 = Orange blinking car + lanes + # FcwOpt_USM 4 = Orange car + lanes + # FcwOpt_USM 3 = Green blinking car + lanes + # FcwOpt_USM 2 = Green car + lanes + # FcwOpt_USM 1 = White car + lanes + # FcwOpt_USM 0 = No car + lanes + values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1 + + # SysWarning 4 = keep hands on wheel + # SysWarning 5 = keep hands on wheel (red) + # SysWarning 6 = keep hands on wheel (red) + beep + # Note: the warning is hidden while the blinkers are on + values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 + + # Likely cars lacking the ability to show individual lane lines in the dash + elif car_fingerprint in (CAR.KIA_OPTIMA,): + # SysWarning 4 = keep hands on wheel + beep + values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 + + # SysState 0 = no icons + # SysState 1-2 = white car + lanes + # SysState 3 = green car + lanes, green steering wheel + # SysState 4 = green car + lanes + values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1 + values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition + + # these have no effect + values["CF_Lkas_LdwsActivemode"] = 0 + values["CF_Lkas_FcwOpt_USM"] = 0 + + elif car_fingerprint == CAR.HYUNDAI_GENESIS: + # This field is actually LdwsActivemode + # Genesis and Optima fault when forwarding while engaged + values["CF_Lkas_LdwsActivemode"] = 2 + + dat = packer.make_can_msg("LKAS11", 0, values)[2] + + if car_fingerprint in CHECKSUM["crc8"]: + # CRC Checksum as seen on 2019 Hyundai Santa Fe + dat = dat[:6] + dat[7:8] + checksum = hyundai_checksum(dat) + elif car_fingerprint in CHECKSUM["6B"]: + # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento + checksum = sum(dat[:6]) % 256 + else: + # Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger + checksum = (sum(dat[:6]) + dat[7]) % 256 + + values["CF_Lkas_Chksum"] = checksum + + return packer.make_can_msg("LKAS11", 0, values) + + +def create_clu11(packer, frame, clu11, button, car_fingerprint): + values = clu11 + values["CF_Clu_CruiseSwState"] = button + values["CF_Clu_AliveCnt1"] = frame % 0x10 + # send buttons to camera on camera-scc based cars + bus = 2 if car_fingerprint in CAMERA_SCC_CAR else 0 + return packer.make_can_msg("CLU11", bus, values) + + +def create_lfahda_mfc(packer, enabled, hda_set_speed=0): + values = { + "LFA_Icon_State": 2 if enabled else 0, + "HDA_Active": 1 if hda_set_speed else 0, + "HDA_Icon_State": 2 if hda_set_speed else 0, + "HDA_VSetReq": hda_set_speed, + } + return packer.make_can_msg("LFAHDA_MFC", 0, values) + +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, gas_pressed): + commands = [] + + scc11_values = { + "MainMode_ACC": 1, + "TauGapSet": 4, + "VSetDis": set_speed if enabled else 0, + "AliveCounterACC": idx % 0x10, + "ObjValid": 1, # close lead makes controls tighter + "ACC_ObjStatus": 1, # close lead makes controls tighter + "ACC_ObjLatPos": 0, + "ACC_ObjRelSpd": 0, + "ACC_ObjDist": 1, # close lead makes controls tighter + } + commands.append(packer.make_can_msg("SCC11", 0, scc11_values)) + + scc12_values = { + "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0, + "StopReq": 1 if stopping else 0, + "aReqRaw": accel, + "aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw + "CR_VSM_Alive": idx % 0xF, + } + scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2] + scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10 + + commands.append(packer.make_can_msg("SCC12", 0, scc12_values)) + + scc14_values = { + "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values + "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values + "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values + "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values + "ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage + "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead + } + commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) + + fca11_values = { + # seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat... + # (where first value is aligned to Supplemental_Counter == 0) + # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)] + "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1, + "Supplemental_Counter": idx % 0xF, + "PAINT1_Status": 1, + "FCA_DrvSetStatus": 1, + "FCA_Status": 1, # AEB disabled + } + fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] + fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10 + commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) + + return commands + +def create_acc_opt(packer): + commands = [] + + scc13_values = { + "SCCDrvModeRValue": 2, + "SCC_Equip": 1, + "Lead_Veh_Dep_Alert_USM": 2, + } + commands.append(packer.make_can_msg("SCC13", 0, scc13_values)) + + fca12_values = { + "FCA_DrvSetState": 2, + "FCA_USM": 1, # AEB disabled + } + commands.append(packer.make_can_msg("FCA12", 0, fca12_values)) + + return commands + +def create_frt_radar_opt(packer): + frt_radar11_values = { + "CF_FCA_Equip_Front_Radar": 1, + } + return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py new file mode 100644 index 00000000000000..a53be7627db2d8 --- /dev/null +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -0,0 +1,46 @@ +from selfdrive.car.hyundai.values import HyundaiFlags + + +def create_lkas(packer, CP, enabled, lat_active, apply_steer): + values = { + "LKA_MODE": 2, + "LKA_ICON": 2 if enabled else 1, + "TORQUE_REQUEST": apply_steer, + "LKA_ASSIST": 0, + "STEER_REQ": 1 if lat_active else 0, + "STEER_MODE": 0, + "SET_ME_1": 0, + "NEW_SIGNAL_1": 0, + "NEW_SIGNAL_2": 0, + } + + msg = "LKAS" if CP.flags & HyundaiFlags.CANFD_HDA2 else "LFA" + return packer.make_can_msg(msg, 4, values) + +def create_cam_0x2a4(packer, camera_values): + camera_values.update({ + "BYTE7": 0, + }) + return packer.make_can_msg("CAM_0x2a4", 4, camera_values) + +def create_buttons(packer, cnt, btn): + values = { + "COUNTER": cnt, + "SET_ME_1": 1, + "CRUISE_BUTTONS": btn, + } + return packer.make_can_msg("CRUISE_BUTTONS", 5, values) + +def create_cruise_info(packer, cruise_info_copy, cancel): + values = cruise_info_copy + if cancel: + values["CRUISE_STATUS"] = 0 + values["CRUISE_INACTIVE"] = 1 + return packer.make_can_msg("CRUISE_INFO", 4, values) + +def create_lfahda_cluster(packer, enabled): + values = { + "HDA_ICON": 1 if enabled else 0, + "LFA_ICON": 2 if enabled else 0, + } + return packer.make_can_msg("LFAHDA_CLUSTER", 4, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py new file mode 100644 index 00000000000000..c32cfbeec2d07a --- /dev/null +++ b/selfdrive/car/hyundai/interface.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from common.conversions import Conversions as CV +from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams +from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR +from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.disable_ecu import disable_ecu + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) +BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, + Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "hyundai" + ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + + # WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB) + ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR | CANFD_CAR) + ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable + + ret.pcmCruise = not ret.openpilotLongitudinalControl + + # These cars have been put into dashcam only due to both a lack of users and test coverage. + # These cars likely still work fine. Once a user confirms each car works and a test route is + # added to selfdrive/car/tests/routes.py, we can remove it from this list. + ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} + + ret.steerActuatorDelay = 0.1 # Default delay + ret.steerLimitTimer = 0.4 + tire_stiffness_factor = 1. + + ret.stoppingControl = True + ret.startingState = True + ret.vEgoStarting = 0.1 + ret.startAccel = 2.0 + + ret.longitudinalTuning.kpV = [0.5] + ret.longitudinalTuning.kiV = [0.0] + + ret.longitudinalActuatorDelayLowerBound = 0.5 # s + ret.longitudinalActuatorDelayUpperBound = 0.5 # s + if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.766 + # Values from optimizer + ret.steerRatio = 16.55 # 13.8 is spec end-to-end + tire_stiffness_factor = 0.82 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] + elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): + ret.mass = 1513. + STD_CARGO_KG + ret.wheelbase = 2.84 + ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable + tire_stiffness_factor = 0.65 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.SONATA_LF: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 4497. * CV.LB_TO_KG + ret.wheelbase = 2.804 + ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.PALISADE: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1999. + STD_CARGO_KG + ret.wheelbase = 2.90 + ret.steerRatio = 15.6 * 1.15 + tire_stiffness_factor = 0.63 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1275. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 + tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.ELANTRA_2021: + ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG + ret.wheelbase = 2.72 + ret.steerRatio = 12.9 + tire_stiffness_factor = 0.65 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.ELANTRA_HEV_2021: + ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG + ret.wheelbase = 2.72 + ret.steerRatio = 12.9 + tire_stiffness_factor = 0.65 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.HYUNDAI_GENESIS: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 2060. + STD_CARGO_KG + ret.wheelbase = 3.01 + ret.steerRatio = 16.5 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [3.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [2.0] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] + ret.minSteerSpeed = 60 * CV.KPH_TO_MS + elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743.}.get(candidate, 1275.) + STD_CARGO_KG + ret.wheelbase = 2.6 + ret.steerRatio = 13.42 # Spec + tire_stiffness_factor = 0.385 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 # Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.IONIQ_PHEV_2019: + ret.mass = 1550. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/us/en/vehicles/2019-ioniq-plug-in-hybrid/compare-specs + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [2.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.5] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.VELOSTER: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3558. * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 * 1.15 + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.TUCSON: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3520. * CV.LB_TO_KG + ret.wheelbase = 2.67 + ret.steerRatio = 14.00 * 1.15 + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.TUCSON_HYBRID_4TH_GEN: + ret.mass = 1680. + STD_CARGO_KG # average of all 3 trims + ret.wheelbase = 2.756 + ret.steerRatio = 16. + tire_stiffness_factor = 0.385 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + # Kia + elif candidate == CAR.KIA_SORENTO: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1985. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021): + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1737. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + if candidate == CAR.KIA_NIRO_PHEV: + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.KIA_SELTOS: + ret.mass = 1337. + STD_CARGO_KG + ret.wheelbase = 2.63 + ret.steerRatio = 14.56 + tire_stiffness_factor = 1 + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [4.] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] + elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H): + ret.mass = 3558. * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 + tire_stiffness_factor = 0.5 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.KIA_STINGER: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1825. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.KIA_FORTE: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3558. * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.KIA_CEED: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1450. + STD_CARGO_KG + ret.wheelbase = 2.65 + ret.steerRatio = 13.75 + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.KIA_K5_2021: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3228. * CV.LB_TO_KG + ret.wheelbase = 2.85 + ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.KIA_EV6: + ret.mass = 2055 + STD_CARGO_KG + ret.wheelbase = 2.9 + ret.steerRatio = 16. + tire_stiffness_factor = 0.65 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + elif candidate == CAR.IONIQ_5: + ret.mass = 2012 + STD_CARGO_KG + ret.wheelbase = 3.0 + ret.steerRatio = 16. + tire_stiffness_factor = 0.65 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + # Genesis + elif candidate == CAR.GENESIS_G70: + ret.lateralTuning.init('indi') + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [2.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.5] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] + ret.steerActuatorDelay = 0.1 + ret.mass = 1640.0 + STD_CARGO_KG + ret.wheelbase = 2.84 + ret.steerRatio = 13.56 + elif candidate == CAR.GENESIS_G70_2020: + ret.lateralTuning.pid.kf = 0. + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.112], [0.004]] + ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 12.9 + elif candidate == CAR.GENESIS_G80: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 2060. + STD_CARGO_KG + ret.wheelbase = 3.01 + ret.steerRatio = 16.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + elif candidate == CAR.GENESIS_G90: + ret.mass = 2200 + ret.wheelbase = 3.15 + ret.steerRatio = 12.069 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + + # panda safety config + if candidate in CANFD_CAR: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput), + get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)] + + # detect HDA2 with LKAS message + if 0x50 in fingerprint[6]: + ret.flags |= HyundaiFlags.CANFD_HDA2.value + ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 + else: + # non-HDA2 + if 0x1cf not in fingerprint[4]: + ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value + ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS + else: + ret.enableBsm = 0x58b in fingerprint[0] + + if candidate in LEGACY_SAFETY_MODE_CAR: + # these cars require a special panda safety mode due to missing counters and checksums in the messages + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy)] + else: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)] + + # set appropriate safety param for gas signal + if candidate in HYBRID_CAR: + ret.safetyConfigs[0].safetyParam = 2 + elif candidate in EV_CAR: + ret.safetyConfigs[0].safetyParam = 1 + + if ret.openpilotLongitudinalControl: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG + + if candidate in CAMERA_SCC_CAR: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + + ret.centerToFront = ret.wheelbase * 0.4 + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + return ret + + @staticmethod + def init(CP, logcan, sendcan): + if CP.openpilotLongitudinalControl: + disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01') + + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons: + buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)] + # Handle CF_Clu_CruiseSwState changing buttons mid-press + if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0: + buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT)) + + ret.buttonEvents = buttonEvents + + # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state + # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons + # Main button also can trigger an engagement on these cars + allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons) + events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) + + if self.CS.brake_error: + events.add(EventName.brakeUnavailable) + + # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) + if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: + self.low_speed_alert = True + if ret.vEgo > (self.CP.minSteerSpeed + 4.): + self.low_speed_alert = False + if self.low_speed_alert: + events.add(car.CarEvent.EventName.belowSteerSpeed) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py new file mode 100644 index 00000000000000..0d22611fb577ce --- /dev/null +++ b/selfdrive/car/hyundai/radar_interface.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +import math + +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import RadarInterfaceBase +from selfdrive.car.hyundai.values import DBC + +RADAR_START_ADDR = 0x500 +RADAR_MSG_COUNT = 32 + + +def get_radar_can_parser(CP): + if DBC[CP.carFingerprint]['radar'] is None: + return None + + signals = [] + checks = [] + + for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): + msg = f"RADAR_TRACK_{addr:x}" + signals += [ + ("STATE", msg), + ("AZIMUTH", msg), + ("LONG_DIST", msg), + ("REL_ACCEL", msg), + ("REL_SPEED", msg), + ] + checks += [(msg, 50)] + return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1) + + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.updated_messages = set() + self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 + self.track_id = 0 + + self.radar_off_can = CP.radarOffCan + self.rcp = get_radar_can_parser(CP) + + def update(self, can_strings): + if self.radar_off_can or (self.rcp is None): + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + rr = self._update(self.updated_messages) + self.updated_messages.clear() + + return rr + + def _update(self, updated_messages): + ret = car.RadarData.new_message() + if self.rcp is None: + return ret + + errors = [] + + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): + msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] + + if addr not in self.pts: + self.pts[addr] = car.RadarData.RadarPoint.new_message() + self.pts[addr].trackId = self.track_id + self.track_id += 1 + + valid = msg['STATE'] in (3, 4) + if valid: + azimuth = math.radians(msg['AZIMUTH']) + self.pts[addr].measured = True + self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] + self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] + self.pts[addr].vRel = msg['REL_SPEED'] + self.pts[addr].aRel = msg['REL_ACCEL'] + self.pts[addr].yvRel = float('nan') + + else: + del self.pts[addr] + + ret.points = list(self.pts.values()) + return ret diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py new file mode 100644 index 00000000000000..4225826c1d6414 --- /dev/null +++ b/selfdrive/car/hyundai/values.py @@ -0,0 +1,1421 @@ +from enum import IntFlag +from dataclasses import dataclass +from typing import Dict, List, Optional, Union + +from cereal import car +from panda.python import uds +from common.conversions import Conversions as CV +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 + +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + ACCEL_MIN = -3.5 # m/s + ACCEL_MAX = 2.0 # m/s + + def __init__(self, CP): + self.STEER_DELTA_UP = 3 + self.STEER_DELTA_DOWN = 7 + self.STEER_DRIVER_ALLOWANCE = 50 + self.STEER_DRIVER_MULTIPLIER = 2 + self.STEER_DRIVER_FACTOR = 1 + self.STEER_THRESHOLD = 150 + + if CP.carFingerprint in CANFD_CAR: + self.STEER_MAX = 270 + self.STEER_DRIVER_ALLOWANCE = 250 + self.STEER_DRIVER_MULTIPLIER = 2 + self.STEER_THRESHOLD = 250 + + # To determine the limit for your car, find the maximum value that the stock LKAS will request. + # If the max stock LKAS request is <384, add your car to this list. + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ, + CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, + CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO, CAR.KIA_STINGER): + self.STEER_MAX = 255 + + # Default for most HKG + else: + self.STEER_MAX = 384 + + +class HyundaiFlags(IntFlag): + CANFD_HDA2 = 1 + CANFD_ALT_BUTTONS = 2 + + +class CAR: + # Hyundai + ELANTRA = "HYUNDAI ELANTRA 2017" + ELANTRA_2021 = "HYUNDAI ELANTRA 2021" + ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021" + ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" + HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" + IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" + IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022" + IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" + IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020" + IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" + IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020" + KONA = "HYUNDAI KONA 2020" + KONA_EV = "HYUNDAI KONA ELECTRIC 2019" + KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022" + KONA_HEV = "HYUNDAI KONA HYBRID 2020" + SANTA_FE = "HYUNDAI SANTA FE 2019" + SANTA_FE_2022 = "HYUNDAI SANTA FE 2022" + SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022" + SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022" + SONATA = "HYUNDAI SONATA 2020" + SONATA_LF = "HYUNDAI SONATA 2019" + TUCSON = "HYUNDAI TUCSON 2019" + PALISADE = "HYUNDAI PALISADE 2020" + VELOSTER = "HYUNDAI VELOSTER 2019" + SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" + IONIQ_5 = "HYUNDAI IONIQ 5 2022" + TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN" + + # Kia + KIA_FORTE = "KIA FORTE E 2018 & GT 2021" + KIA_K5_2021 = "KIA K5 2021" + KIA_NIRO_EV = "KIA NIRO EV 2020" + KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" + KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" + KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" + KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" + KIA_SELTOS = "KIA SELTOS 2021" + KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_STINGER = "KIA STINGER GT2 2018" + KIA_CEED = "KIA CEED INTRO ED 2019" + KIA_EV6 = "KIA EV6 2022" + + # Genesis + GENESIS_G70 = "GENESIS G70 2018" + GENESIS_G70_2020 = "GENESIS G70 2020" + GENESIS_G80 = "GENESIS G80 2017" + GENESIS_G90 = "GENESIS G90 2017" + + +@dataclass +class HyundaiCarInfo(CarInfo): + # TODO: we can probably remove LKAS. LKAS is standard on many + # HKG and for others, it's likely packaged together with SCC + package: str = "Smart Cruise Control (SCC) & LKAS" + + +CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { + CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b), + CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), + CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", "Smart Cruise Control (SCC)", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), + CAR.ELANTRA_GT_I30: None, # dashcamOnly and same platform as CAR.ELANTRA + CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), + CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c), + CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "Smart Cruise Control (SCC) & LFA", harness=Harness.hyundai_h), + CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c), + CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", harness=Harness.hyundai_h), + CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c), + CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-21", "Smart Cruise Control (SCC)", harness=Harness.hyundai_h), + CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", "Smart Cruise Control (SCC)", harness=Harness.hyundai_b), + CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness=Harness.hyundai_g), + CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", "Smart Cruise Control (SCC)", harness=Harness.hyundai_o), + CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i), + CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d), + CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l), + CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l), + CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l), + CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a), + CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e), + CAR.TUCSON: [ + HyundaiCarInfo("Hyundai Tucson 2021", "Smart Cruise Control (SCC)", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l), + HyundaiCarInfo("Hyundai Tucson Diesel 2019", "Smart Cruise Control (SCC)", harness=Harness.hyundai_l), + ], + CAR.PALISADE: [ + HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h), + HyundaiCarInfo("Kia Telluride 2020", "All", harness=Harness.hyundai_h), + ], + CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", "Smart Cruise Control (SCC)", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e), + CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a), + CAR.IONIQ_5: HyundaiCarInfo("Hyundai Ioniq 5 2022", "Highway Driving Assist II", harness=Harness.hyundai_q), + CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022", "All", harness=Harness.hyundai_n), + + # Kia + CAR.KIA_FORTE: [ + HyundaiCarInfo("Kia Forte 2018", harness=Harness.hyundai_b), + HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), + ], + CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), + CAR.KIA_NIRO_EV: [ + HyundaiCarInfo("Kia Niro Electric 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), + HyundaiCarInfo("Kia Niro Electric 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f), + HyundaiCarInfo("Kia Niro Electric 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c), + HyundaiCarInfo("Kia Niro Electric 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h), + ], + CAR.KIA_NIRO_PHEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c), + CAR.KIA_NIRO_HEV_2021: [ + HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify + HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), + ], + CAR.KIA_OPTIMA: [ + HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS, harness=Harness.hyundai_b), + HyundaiCarInfo("Kia Optima 2019", harness=Harness.hyundai_g), + ], + CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017, 2019"), # TODO: info may be incorrect + CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), + CAR.KIA_SORENTO: [ + HyundaiCarInfo("Kia Sorento 2018", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c), + HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e), + ], + CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c), + CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e), + CAR.KIA_EV6: HyundaiCarInfo("Kia EV6 2022", "Highway Driving Assist II", harness=Harness.hyundai_p), + + # Genesis + CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f), + CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f), + CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2017-19", "All", harness=Harness.hyundai_h), + CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c), +} + +class Buttons: + NONE = 0 + RES_ACCEL = 1 + SET_DECEL = 2 + GAP_DIST = 3 + CANCEL = 4 # on newer models, this is a pause/resume button + +FINGERPRINTS = { + CAR.ELANTRA: [{ + 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], + CAR.ELANTRA_GT_I30: [{ + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1193: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1952: 8, 1960: 8, 1988: 8, 2000: 8, 2001: 8, 2005: 8, 2008: 8, 2009: 8, 2013: 8, 2017: 8, 2025: 8 + }, + { + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8 + }, + { + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1960: 8, 1990: 8, 1998: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], + CAR.HYUNDAI_GENESIS: [{ + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1268: 8, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1437: 8, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 + }], + CAR.SANTA_FE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 + }, + { + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 + }, + { + 67: 8, 68: 8, 80: 4, 160: 8, 161: 8, 272: 8, 288: 4, 339: 8, 356: 8, 357: 8, 399: 8, 544: 8, 608: 8, 672: 8, 688: 5, 704: 1, 790: 8, 809: 8, 848: 8, 880: 8, 898: 8, 900: 8, 901: 8, 904: 8, 1056: 8, 1064: 8, 1065: 8, 1072: 8, 1075: 8, 1087: 8, 1088: 8, 1151: 8, 1200: 8, 1201: 8, 1232: 4, 1264: 8, 1265: 8, 1266: 8, 1296: 8, 1306: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1348: 8, 1349: 8, 1369: 8, 1370: 8, 1371: 8, 1407: 8, 1415: 8, 1419: 8, 1440: 8, 1442: 4, 1461: 8, 1470: 8 + }], + CAR.SONATA: [ + {67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8}, + ], + CAR.SONATA_LF: [ + {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8}, + ], + CAR.KIA_OPTIMA: [{ + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8, 1371: 8, 1397: 8, 1961: 8 + }], + CAR.KIA_SORENTO: [{ + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 + }], + CAR.KIA_STINGER: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 + }], + CAR.GENESIS_G80: [{ + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8 + }, + { + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 546: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1437: 8, 1456: 4, 1470: 8 + }, + { + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1162: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1193: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4, 1470: 8 + }], + CAR.GENESIS_G90: [{ + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1162: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2003: 8, 2004: 8, 2005: 8, 2008: 8, 2011: 8, 2012: 8, 2013: 8 + }], + CAR.IONIQ_EV_2020: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 + }], + CAR.IONIQ: [{ + 68:8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576:8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 + }], + CAR.KONA_EV: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8 + }], + CAR.KONA_EV_2022: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1069: 8, 1078: 4, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1173: 8, 1183: 8, 1188: 8, 1191: 2, 1193: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1339: 8, 1342: 8, 1343: 8, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1446: 8, 1456: 4, 1470: 8, 1473: 8, 1485: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8 + }], + CAR.KIA_NIRO_EV: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 + }], + CAR.KIA_OPTIMA_H: [{ + 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1236: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 + }, + { + 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 + }], + CAR.PALISADE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 547: 8, 548: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8 + }], +} + +HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf100) # Long description +HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ + p16(0xf100) +HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [HYUNDAI_VERSION_REQUEST_LONG], + [HYUNDAI_VERSION_RESPONSE], + ), + Request( + [HYUNDAI_VERSION_REQUEST_MULTI], + [HYUNDAI_VERSION_RESPONSE], + ), + ], +) + +FW_VERSIONS = { + CAR.IONIQ: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2301 4AEHC107', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G2400 180222', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6F2051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3H1051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H1051\x00\x00HAE0G16US2\x00\x00\x00\x00', + ], + }, + CAR.IONIQ_PHEV_2019: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2100 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2501 4AEHC107', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2400 180222', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\xdbD\r\x81', + b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\x00\x00\x00\x00', + ], + }, + CAR.IONIQ_PHEV: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', + b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ', + b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\000AE MDPS C 1.00 1.01 56310/G2510 4APHC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2560 4APHC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310G2510\x00 4APHC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\000AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEP MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2700 201027', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2', + b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00', + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt', + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00', + ], + }, + CAR.IONIQ_EV_2020: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEev SCC F-CUP 1.00 1.01 99110-G7000 ', + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 99110-G7200 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7310 4APEC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7560 4APEC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.03 95740-G2500 190516', + b'\xf1\x00AEE MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', + ], + }, + CAR.IONIQ_EV_LTD: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7000 ', + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7100 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.02 56310G7300\x00 4AEEC102', + b'\xf1\x00AE MDPS C 1.00 1.04 56310/G7501 4AEEC104', + b'\xf1\x00AE MDPS C 1.00 1.03 56310/G7300 4AEEC103', + b'\xf1\x00AE MDPS C 1.00 1.03 56310G7300\x00 4AEEC103', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', + b'\xf1\x00AEE MFC AT USA LHD 1.00 1.00 95740-G2400 180222', + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G2300 170703', + ], + }, + CAR.IONIQ_HEV_2022: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.01 56310G2510\x00 4APHC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2700 201027', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HAE0G16NL2\x00\x00\x00\x00', + ], + }, + CAR.SONATA: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ', + b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC F-CUP 1.00 1.02 99110-L1000 ', + b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', + b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', + b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ', + b'\xf1\x8799110L0000\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', + b'\xf1\x8799110L0000\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', + b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300', + b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300', + b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', + b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', + b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', + b'\xf1\x8758910-L0300\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300', + b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81HM6M1_0a0_F00', + b'\xf1\x82DNBVN5GMCCXXXDCA', + b'\xf1\x82DNBVN5GMCCXXXG2F', + b'\xf1\x82DNBWN5TMDCXXXG2E', + b'\xf1\x82DNCVN5GMCCXXXF0A', + b'\xf1\x82DNCVN5GMCCXXXG2B', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A', + b'\xf1\x87391162M003', + b'\xf1\x87391162M013', + b'\xf1\x87391162M023', + b'HM6M1_0a0_F00', + b'HM6M1_0a0_G20', + b'HM6M2_0a0_BD0', + b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware + b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware + b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', + b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', + b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101', + b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', + b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101', + b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103', + b'\xf1\x8756310-L1030\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103', + b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', + b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101', + b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', + b'\xf1\x00DN8 MFC AT RUS LHD 1.00 1.03 99211-L1000 190705', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.03 99211-L0000 210603', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.05 99211-L1000 201109', + b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.06 99211-L1000 210325', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00HT6TA260BLHT6TA810A1TDN8M25GS0\x00\x00\x00\x00\x00\x00\xaa\x8c\xd9p', + b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', + b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:', + b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', + b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', + b'\xf1\x87SAKFBA2926554GJ2VefVww\x87xwwwww\x88\x87xww\x87wTo\xfb\xffvUo\xff\x8d\x16\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SAKFBA3030524GJ2UVugww\x97yx\x88\x87\x88vw\x87gww\x87wto\xf9\xfffUo\xff\xa2\x0c\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SAKFBA3356084GJ2\x86fvgUUuWgw\x86www\x87wffvf\xb6\xcf\xfc\xffeUO\xff\x12\x19\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SAKFBA3474944GJ2ffvgwwwwg\x88\x86x\x88\x88\x98\x88ffvfeo\xfa\xff\x86fo\xff\t\xae\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SAKFBA3475714GJ2Vfvgvg\x96yx\x88\x97\x88ww\x87ww\x88\x87xs_\xfb\xffvUO\xff\x0f\xff\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3753044GJ3UUeVff\x86hwwwwvwwgvfgfvo\xf9\xfffU_\xffC\xae\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3862294GJ3vfvgvefVxw\x87\x87w\x88\x87xwwwwc_\xf9\xff\x87w\x9f\xff\xd5\xdc\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA3873834GJ3fefVwuwWx\x88\x97\x88w\x88\x97xww\x87wU_\xfb\xff\x86f\x8f\xffN\x04\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4525334GJ3\x89\x99\x99\x99fevWh\x88\x86\x88fwvgw\x88\x87xfo\xfa\xffuDo\xff\xd1>\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4626804GJ3wwww\x88\x87\x88xx\x88\x87\x88wwgw\x88\x88\x98\x88\x95_\xf9\xffuDo\xff|\xe7\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA4803224GJ3wwwwwvwg\x88\x88\x98\x88wwww\x87\x88\x88xu\x9f\xfc\xff\x87f\x8f\xff\xea\xea\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA6212564GJ3\x87wwwUTuGg\x88\x86xx\x88\x87\x88\x87\x88\x98xu?\xf9\xff\x97f\x7f\xff\xb8\n\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA6347404GJ3wwwwff\x86hx\x88\x97\x88\x88\x88\x88\x88vfgf\x88?\xfc\xff\x86Uo\xff\xec/\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA6901634GJ3UUuWVeVUww\x87wwwwwvUge\x86/\xfb\xff\xbb\x99\x7f\xff]2\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALDBA7077724GJ3\x98\x88\x88\x88ww\x97ygwvwww\x87ww\x88\x87x\x87_\xfd\xff\xba\x99o\xff\x99\x01\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALFBA3525114GJ2wvwgvfvggw\x86wffvffw\x86g\x85_\xf9\xff\xa8wo\xffv\xcd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA3624024GJ2\x88\x88\x88\x88wv\x87hx\x88\x97\x88x\x88\x97\x88ww\x87w\x86o\xfa\xffvU\x7f\xff\xd1\xec\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA3960824GJ2wwwwff\x86hffvfffffvfwfg_\xf9\xff\xa9\x88\x8f\xffb\x99\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA4011074GJ2fgvwwv\x87hw\x88\x87xww\x87wwfgvu_\xfa\xffefo\xff\x87\xc0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA4121304GJ2x\x87xwff\x86hwwwwww\x87wwwww\x84_\xfc\xff\x98\x88\x9f\xffi\xa6\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA4195874GJ2EVugvf\x86hgwvwww\x87wgw\x86wc_\xfb\xff\x98\x88\x8f\xff\xe23\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA4625294GJ2eVefeUeVx\x88\x97\x88wwwwwwww\xa7o\xfb\xffvw\x9f\xff\xee.\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA4728774GJ2vfvg\x87vwgww\x87ww\x88\x97xww\x87w\x86_\xfb\xffeD?\xffk0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA5129064GJ2vfvgwv\x87hx\x88\x87\x88ww\x87www\x87wd_\xfa\xffvfo\xff\x1d\x00\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA5454914GJ2\x98\x88\x88\x88\x87vwgx\x88\x87\x88xww\x87ffvf\xa7\x7f\xf9\xff\xa8w\x7f\xff\x1b\x90\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA5987784GJ2UVugDDtGx\x88\x87\x88w\x88\x87xwwwwd/\xfb\xff\x97fO\xff\xb0h\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA5987864GJ2fgvwUUuWgwvw\x87wxwwwww\x84/\xfc\xff\x97w\x7f\xff\xdf\x1d\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA6337644GJ2vgvwwv\x87hgffvwwwwwwww\x85O\xfa\xff\xa7w\x7f\xff\xc5\xfc\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA6802004GJ2UUuWUUuWgw\x86www\x87www\x87w\x96?\xf9\xff\xa9\x88\x7f\xff\x9fK\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA6892284GJ233S5\x87w\x87xx\x88\x87\x88vwwgww\x87w\x84?\xfb\xff\x98\x88\x8f\xff*\x9e\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', + b'\xf1\x87SALFBA7005534GJ2eUuWfg\x86xxww\x87x\x88\x87\x88\x88w\x88\x87\x87O\xfc\xffuUO\xff\xa3k\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', + b'\xf1\x87SALFBA7152454GJ2gvwgFf\x86hx\x88\x87\x88vfWfffffd?\xfa\xff\xba\x88o\xff,\xcf\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', + b'\xf1\x87SALFBA7485034GJ2ww\x87xww\x87xfwvgwwwwvfgf\xa5/\xfc\xff\xa9w_\xff40\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMDBA7743924GJ3wwwwww\x87xgwvw\x88\x88\x88\x88wwww\x85_\xfa\xff\x86f\x7f\xff0\x9d\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SAMDBA7817334GJ3Vgvwvfvgww\x87wwwwwwfgv\x97O\xfd\xff\x88\x88o\xff\x8e\xeb\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SAMDBA8054504GJ3gw\x87xffvgffffwwwweUVUf?\xfc\xffvU_\xff\xddl\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SAMFB41553621GC7ww\x87xUU\x85Xvwwg\x88\x88\x88\x88wwgw\x86\xaf\xfb\xffuDo\xff\xaa\x8f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMFB42555421GC7\x88\x88\x88\x88wvwgx\x88\x87\x88wwgw\x87wxw3\x8f\xfc\xff\x98f\x8f\xffga\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMFBA7978674GJ2gw\x87xgw\x97ywwwwvUGeUUeU\x87O\xfb\xff\x98w\x8f\xfffF\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMFBA9283024GJ2wwwwEUuWwwgwwwwwwwww\x87/\xfb\xff\x98w\x8f\xff<\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMFBA9708354GJ2wwwwVf\x86h\x88wx\x87xww\x87\x88\x88\x88\x88w/\xfa\xff\x97w\x8f\xff\x86\xa0\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SANDB45316691GC6\x99\x99\x99\x99\x88\x88\xa8\x8avfwfwwww\x87wxwT\x9f\xfd\xff\x88wo\xff\x1c\xfa\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB3\x00\x00\x00\x00\x00\x00', + b'\xf1\x87SALFBA7460044GJ2gx\x87\x88Vf\x86hx\x88\x87\x88wwwwgw\x86wd?\xfa\xff\x86U_\xff\xaf\x1f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SAMFBA8105254GJ2wx\x87\x88Vf\x86hx\x88\x87\x88wwwwwwww\x86O\xfa\xff\x99\x88\x7f\xffZG\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + b'\xf1\x87SANFB45889451GC7wx\x87\x88gw\x87x\x88\x88x\x88\x87wxw\x87wxw\x87\x8f\xfc\xffeU\x8f\xff+Q\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', + ], + }, + CAR.SONATA_LF: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00LF__ SCC F-CUP 1.00 1.00 96401-C2200 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00LF ESC \f 11 \x17\x01\x13 58920-C2610', + b'\xf1\x00LF ESC \t 11 \x17\x01\x13 58920-C2610', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606D5051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606D5K51\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51', + b'\xf1\x00LFF LKAS AT USA LHD 1.01 1.02 95740-C1000 E52', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\x00\x00\x00\x00', + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24SL2n\x8d\xbe\xd8', + b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2\x00\x00\x00\x00', + b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', + b'\xf1\x87LAJSG49645724HF0\x87x\x87\x88\x87www\x88\x99\xa8\x89\x88\x99\xa8\x89\x88\x99\xa8\x89S_\xfb\xff\x87f\x7f\xff^2\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', + ], + }, + CAR.TUCSON: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TL__ FCA F-CUP 1.00 1.01 99110-D3500 ', + b'\xf1\x00TL__ FCA F-CUP 1.00 1.02 99110-D3510 ', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8971TLC2NAIDDIR002\xf1\x8271TLC2NAIDDIR002', + b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TL MFC AT KOR LHD 1.00 1.02 95895-D3800 180719', + b'\xf1\x00TL MFC AT USA LHD 1.00 1.06 95895-D3800 190107', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LBJXAN202299KF22\x87x\x87\x88ww\x87xx\x88\x97\x88\x87\x88\x98x\x88\x99\x98\x89\x87o\xf6\xff\x87w\x7f\xff\x12\x9a\xf1\x81U083\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U083\x00\x00\x00\x00\x00\x00TTL2V20KL1\x8fRn\x8a', + b'\xf1\x87KMLDCU585233TJ20wx\x87\x88x\x88\x98\x89vfwfwwww\x87f\x9f\xff\x98\xff\x7f\xf9\xf7s\xf1\x816T6G4051\x00\x00\xf1\x006T6J0_C2\x00\x006T6G4051\x00\x00TTL4G24NH2\x00\x00\x00\x00', + ], + }, + CAR.SANTA_FE: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TM__ SCC F-CUP 1.00 1.01 99110-S2000 ', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 ', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650', + b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650', + b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650', + b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600', + b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600', + b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600', + b'\xf1\x00TM ESC \x02 104\x19\x07\x07 58910-S2600', + b'\xf1\x00TM ESC \x03 103\x18\x11\x07 58910-S2600', + b'\xf1\x00TM ESC \x0c 103\x18\x11\x08 58910-S2650', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8A12', + b'\xf1\x00TM MDPS C 1.00 1.01 56340-S2000 9129', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LBJSGA7082574HG0\x87www\x98\x88\x88\x88\x99\xaa\xb9\x9afw\x86gx\x99\xa7\x89co\xf8\xffvU_\xffR\xaf\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\xa6\xe0\x91', + b'\xf1\x87LBKSGA0458404HG0vfvg\x87www\x89\x99\xa8\x99y\xaa\xa7\x9ax\x88\xa7\x88t_\xf9\xff\x86w\x8f\xff\x15x\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\x00\x00\x00', + b'\xf1\x87LDJUEA6010814HG1\x87w\x87x\x86gvw\x88\x88\x98\x88gw\x86wx\x88\x97\x88\x85o\xf8\xff\x86f_\xff\xd37\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', + b'\xf1\x87LDJUEA6458264HG1ww\x87x\x97x\x87\x88\x88\x99\x98\x89g\x88\x86xw\x88\x97x\x86o\xf7\xffvw\x8f\xff3\x9a\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', + b'\xf1\x87LDKUEA2045844HG1wwww\x98\x88x\x87\x88\x88\xa8\x88x\x99\x97\x89x\x88\xa7\x88U\x7f\xf8\xffvfO\xffC\x1e\xf1\x816W3E0051\x00\x00\xf1\x006W351_C2\x00\x006W3E0051\x00\x00TTM4T20NS3\x00\x00\x00\x00', + b'\xf1\x87LDKUEA9993304HG1\x87www\x97x\x87\x88\x99\x99\xa9\x99x\x99\xa7\x89w\x88\x97x\x86_\xf7\xffwwO\xffl#\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS1R\x7f\x90\n', + b'\xf1\x87LDLUEA6061564HG1\xa9\x99\x89\x98\x87wwwx\x88\x97\x88x\x99\xa7\x89x\x99\xa7\x89sO\xf9\xffvU_\xff<\xde\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', + b'\xf1\x87LDLUEA6159884HG1\x88\x87hv\x99\x99y\x97\x89\xaa\xb8\x9ax\x99\x87\x89y\x99\xb7\x99\xa7?\xf7\xff\x97wo\xff\xf3\x05\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', + b'\xf1\x87LDLUEA6852664HG1\x97wWu\x97www\x89\xaa\xc8\x9ax\x99\x97\x89x\x99\xa7\x89SO\xf7\xff\xa8\x88\x7f\xff\x03z\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', + b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', + b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', + b'\xf1\x87SBJWAA5842214GG0\x88\x87\x88xww\x87x\x89\x99\xa8\x99\x88\x99\x98\x89w\x88\x87xw_\xfa\xfffU_\xff\xd1\x8d\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', + b'\xf1\x87SBJWAA5890864GG0\xa9\x99\x89\x98\x98\x87\x98y\x89\x99\xa8\x99w\x88\x87xww\x87wvo\xfb\xffuD_\xff\x9f\xb5\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', + b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x00\x00\x00\x00', + b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', + b'\xf1\x87SBJWAA7780564GG0wvwgUUeVwwwwx\x88\x87\x88wwwwd_\xfc\xff\x86f\x7f\xff\xd7*\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', + b'\xf1\x87SBJWAA8278284GG0ffvgUU\x85Xx\x88\x87\x88x\x88w\x88ww\x87w\x96o\xfd\xff\xa7U_\xff\xf2\xa0\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', + b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6\x00\x00\x00\x00', + b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6x0\x17\xfe', + b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00', + b'\xf1\x87SBLWAA6622844GG0wwwwff\x86hwwwwx\x88\x87\x88\x88\x88\x88\x88\x98?\xfd\xff\xa9\x88\x7f\xffn\xe5\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7u\x1e{\x1c', + b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2\x00\x00\x00\x00', + b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2K\xdaV0', + b'\xf1\x87SDKXAA2443414GG1vfvgwv\x87h\x88\x88\x88\x88ww\x87wwwww\x99_\xfc\xffvD?\xffl\xd2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4G24NS6\x00\x00\x00\x00', + ], + }, + CAR.SANTA_FE_2022: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', + b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', + b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', + b'\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', + b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', + b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', + b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', + b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', + b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', + b'\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x82TACVN5GMI3XXXH0A', + b'\xf1\x82TMBZN5TMD3XXXG2E', + b'\xf1\x82TACVN5GSI3XXXH0A', + b'\xf1\x82TMCFD5MMCXXXXG0A', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A', + b'\xf1\x81HM6M2_0a0_G00', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.02 56370-S2AA0 0B19', + b'\xf1\x00TM MDPS C 1.00 1.01 56310-S1AB0 4TSDC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TMA MFC AT MEX LHD 1.00 1.01 99211-S2500 210205', + b'\xf1\x00TMA MFC AT USA LHD 1.00 1.00 99211-S2500 200720', + b'\xf1\x00TM MFC AT EUR LHD 1.00 1.03 99211-S1500 210224', + b'\xf1\x00TMA MFC AT USA LHD 1.00 1.01 99211-S2500 210205', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', + b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', + b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', + b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00', + b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6', + b'\xf1\x00HT6TA290BLHT6TAF00A1STM0M25GS1\x00\x00\x00\x00\x00\x006\xd8\x97\x15', + b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', + b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02900A1 \xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', + ], + }, + CAR.SANTA_FE_HEV_2022: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x87391312MTC1', + ], + }, + CAR.SANTA_FE_PHEV_2022: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', + b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLEC0 4TSHC102', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TMP MFC AT USA LHD 1.00 1.03 99211-S1500 210224', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe', + b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x87391312MTF0', + ], + }, + CAR.KIA_STINGER: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 ', + b'\xf1\x00CK__ SCC F_CUP 1.00 1.03 96400-J5100 ', + b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5000 ', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606DE051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x82CKJN3TMSDE0B\x00\x00\x00\x00', + b'\xf1\x82CKKN3TMD_H0A\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5200 4C2CL104', + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5220 4C2VL104', + b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104', + b'\xf1\x00CK MDPS R 1.00 1.06 57700-J5420 4C4VL106', + b'\xf1\x00CK MDPS R 1.00 1.07 57700-J5220 4C2VL107', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822', + b'\xf1\x00CK MFC AT USA LHD 1.00 1.04 95740-J5000 180504', + b'\xf1\x00CK MFC AT EUR LHD 1.00 1.03 95740-J5000 170822', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87VCJLE17622572DK0vd6D\x99\x98y\x97vwVffUfvfC%CuT&Dx\x87o\xff{\x1c\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x89E21\x00\x00\x00\x00\x00\x00\x00\xf1\x82SCK0T33NB0', + b'\xf1\x87VDHLG17034412DK2vD6DfVvVTD$D\x99w\x88\x98EDEDeT6DgfO\xff\xc3=\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', + b'\xf1\x87VDKLJ18675252DK6\x89vhgwwwwveVU\x88w\x87w\x99vgf\x97vXfgw_\xff\xc2\xfb\xf1\x89E25\x00\x00\x00\x00\x00\x00\x00\xf1\x82TCK0T33NB2', + b'\xf1\x87WAJTE17552812CH4vfFffvfVeT5DwvvVVdFeegeg\x88\x88o\xff\x1a]\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00TCK2T20NB1\x19\xd2\x00\x94', + b'\xf1\x87VDHLG17274082DK2wfFf\x89x\x98wUT5T\x88v\x97xgeGefTGTVvO\xff\x1c\x14\xf1\x81E19\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E19\x00\x00\x00\x00\x00\x00\x00SCK0T33UB2\xee[\x97S', + ], + }, + CAR.PALISADE: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00LX2_ SCC F-CUP 1.00 1.05 99110-S8100 ', + b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 ', + b'\xf1\x00LX2_ SCC FHCU- 1.00 1.05 99110-S8100 ', + b'\xf1\x00LX2_ SCC FHCUP 1.00 1.00 99110-S8110 ', + b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ', + b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ', + b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x01 1031\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330', + b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', + b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330', + b'\xf1\x00LX ESC \x0b 103\x19\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x0b 104 \x10\x16 58910-S8360', + b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360', + b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360', + b'\xf1\x00ON ESC \x0b 101\x19\t\x05 58910-S9320', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640K0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640S1051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', + b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', + b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8020 4LXDC104', + b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13', + b'\xf1\x00ON MDPS C 1.00 1.01 56340-S9000 9201', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125', + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909', + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 200422', + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 200903', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 181105', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720', + b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', + b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', + b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', + b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', + b'\xf1\x87LBLUFN731381KF36\xb9\x99\x89\x98\x98\x88\x88\x88\x89\x99\xa8\x99\x88\x99\xa8\x89\x88\x88\x98\x88V\x7f\xf6\xff\x99w\x8f\xff\xad\xd8\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', + b'\xf1\x87LDKVAA0028604HH1\xa8\x88x\x87vgvw\x88\x99\xa8\x89gw\x86ww\x88\x97x\x97o\xf9\xff\x97w\x7f\xffo\x02\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', + b'\xf1\x87LDKVAA3068374HH1wwww\x87xw\x87y\x99\xa7\x99w\x88\x87xw\x88\x97x\x85\xaf\xfa\xffvU/\xffU\xdc\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', + b'\xf1\x87LDKVBN382172KF26\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\xa7\x89\x87\x88\x98x\x98\x99\xa9\x89\xa5_\xf6\xffDDO\xff\xcd\x16\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', + b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', + b'\xf1\x87LDKVBN540766KF37\x87wgv\x87w\x87xx\x99\x97\x89v\x88\x97h\x88\x88\x88\x88x\x7f\xf6\xffvUo\xff\xd3\x01\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', + b'\xf1\x87LDLVAA4225634HH1\x98\x88\x88\x88eUeVx\x88\x87\x88g\x88\x86xx\x88\x87\x88\x86o\xf9\xff\x87w\x7f\xff\xf2\xf7\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LDLVAA4777834HH1\x98\x88x\x87\x87wwwx\x88\x87\x88x\x99\x97\x89x\x88\x97\x88\x86o\xfa\xff\x86fO\xff\x1d9\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LDLVAA5194534HH1ffvguUUUx\x88\xa7\x88h\x99\x96\x89x\x88\x97\x88ro\xf9\xff\x98wo\xff\xaaM\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LDLVAA5949924HH1\xa9\x99y\x97\x87wwwx\x99\x97\x89x\x99\xa7\x89x\x99\xa7\x89\x87_\xfa\xffeD?\xff\xf1\xfd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', + b'\xf1\x87LDLVBN602045KF26\xb9\x99\x89\x98\x97vwgy\xaa\xb7\x9af\x88\x96hw\x99\xa7y\xa9\x7f\xf5\xff\x99w\x7f\xff,\xd3\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN628911KF26\xa9\x99\x89\x98\x98\x88\x88\x88y\x99\xa7\x99fw\x86gw\x88\x87x\x83\x7f\xf6\xff\x98wo\xff2\xda\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN645817KF37\x87www\x98\x87xwx\x99\x97\x89\x99\x99\x99\x99g\x88\x96x\xb6_\xf7\xff\x98fo\xff\xe2\x86\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN662115KF37\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\x97\x89x\x99\xa7\x89\x88\x99\xa8\x89\x88\x7f\xf7\xfffD_\xff\xdc\x84\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN667933KF37\xb9\x99\x89\x98\xb9\x99\x99\x99x\x88\x87\x88w\x88\x87x\x88\x88\x98\x88\xcbo\xf7\xffe3/\xffQ!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN673087KF37\x97www\x86fvgx\x99\x97\x89\x99\xaa\xa9\x9ag\x88\x86x\xe9_\xf8\xff\x98w\x7f\xff"\xad\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN673841KF37\x98\x88x\x87\x86g\x86xy\x99\xa7\x99\x88\x99\xa8\x89w\x88\x97xdo\xf5\xff\x98\x88\x8f\xffT\xec\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN681363KF37\x98\x88\x88\x88\x97x\x87\x88y\xaa\xa7\x9a\x88\x88\x98\x88\x88\x88\x88\x88vo\xf6\xffvD\x7f\xff%v\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN713782KF37\x99\x99y\x97\x98\x88\x88\x88x\x88\x97\x88\x88\x99\x98\x89\x88\x99\xa8\x89\x87o\xf7\xffeU?\xff7,\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN713890KF26\xb9\x99\x89\x98\xa9\x99\x99\x99x\x99\x97\x89\x88\x99\xa8\x89\x88\x99\xb8\x89Do\xf7\xff\xa9\x88o\xffs\r\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN733215KF37\x99\x98y\x87\x97wwwi\x99\xa6\x99x\x99\xa7\x89V\x88\x95h\x86o\xf7\xffeDO\xff\x12\xe7\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN750044KF37\xca\xa9\x8a\x98\xa7wwwy\xaa\xb7\x9ag\x88\x96x\x88\x99\xa8\x89\xb9\x7f\xf6\xff\xa8w\x7f\xff\xbe\xde\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN752612KF37\xba\xaa\x8a\xa8\x87w\x87xy\xaa\xa7\x9a\x88\x99\x98\x89x\x88\x97\x88\x96o\xf6\xffvU_\xffh\x1b\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN755553KF37\x87xw\x87\x97w\x87xy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95gwo\xf6\xffwUO\xff\xb5T\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', + b'\xf1\x87LDLVBN757883KF37\x98\x87xw\x98\x87\x88xy\xaa\xb7\x9ag\x88\x96x\x89\x99\xa8\x99e\x7f\xf6\xff\xa9\x88o\xff5\x15\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN778156KF37\x87vWe\xa9\x99\x99\x99y\x99\xb7\x99\x99\x99\x99\x99x\x99\x97\x89\xa8\x7f\xf8\xffwf\x7f\xff\x82_\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN780576KF37\x98\x87hv\x97x\x97\x89x\x99\xa7\x89\x88\x99\x98\x89w\x88\x97x\x98\x7f\xf7\xff\xba\x88\x8f\xff\x1e0\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN783485KF37\x87www\x87vwgy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95g\x89_\xf6\xff\xa9w_\xff\xc5\xd6\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN811844KF37\x87vwgvfffx\x99\xa7\x89Vw\x95gg\x88\xa6xe\x8f\xf6\xff\x97wO\xff\t\x80\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN830601KF37\xa7www\xa8\x87xwx\x99\xa7\x89Uw\x85Ww\x88\x97x\x88o\xf6\xff\x8a\xaa\x7f\xff\xe2:\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', + b'\xf1\x87LDMVBN848789KF37\x87w\x87x\x87w\x87xy\x99\xb7\x99\x87\x88\x98x\x88\x99\xa8\x89\x87\x7f\xf6\xfffUo\xff\xe3!\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN851595KF37\x97wgvvfffx\x99\xb7\x89\x88\x99\x98\x89\x87\x88\x98x\x99\x7f\xf7\xff\x97w\x7f\xff@\xf3\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN873175KF26\xa8\x88\x88\x88vfVex\x99\xb7\x89\x88\x99\x98\x89x\x88\x97\x88f\x7f\xf7\xff\xbb\xaa\x8f\xff,\x04\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN879401KF26veVU\xa8\x88\x88\x88g\x88\xa6xVw\x95gx\x88\xa7\x88v\x8f\xf9\xff\xdd\xbb\xbf\xff\xb3\x99\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN881314KF37\xa8\x88h\x86\x97www\x89\x99\xa8\x99w\x88\x97xx\x99\xa7\x89\xca\x7f\xf8\xff\xba\x99\x8f\xff\xd8v\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN888651KF37\xa9\x99\x89\x98vfff\x88\x99\x98\x89w\x99\xa7y\x88\x88\x98\x88D\x8f\xf9\xff\xcb\x99\x8f\xff\xa5\x1e\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN889419KF37\xa9\x99y\x97\x87w\x87xx\x88\x97\x88w\x88\x97x\x88\x99\x98\x89e\x9f\xf9\xffeUo\xff\x901\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN895969KF37vefV\x87vgfx\x99\xa7\x89\x99\x99\xb9\x99f\x88\x96he_\xf7\xffxwo\xff\x14\xf9\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDMVBN899222KF37\xa8\x88x\x87\x97www\x98\x99\x99\x89\x88\x99\x98\x89f\x88\x96hdo\xf7\xff\xbb\xaa\x9f\xff\xe2U\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b"\xf1\x87LBLUFN622950KF36\xa8\x88\x88\x88\x87w\x87xh\x99\x96\x89\x88\x99\x98\x89\x88\x99\x98\x89\x87o\xf6\xff\x98\x88o\xffx'\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8", + b'\xf1\x87LDMVBN950669KF37\x97www\x96fffy\x99\xa7\x99\xa9\x99\xaa\x99g\x88\x96x\xb8\x8f\xf9\xffTD/\xff\xa7\xcb\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + b'\xf1\x87LDLVAA4478824HH1\x87wwwvfvg\x89\x99\xa8\x99w\x88\x87x\x89\x99\xa8\x99\xa6o\xfa\xfffU/\xffu\x92\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x87LDMVBN871852KF37\xb9\x99\x99\x99\xa8\x88\x88\x88y\x99\xa7\x99x\x99\xa7\x89\x88\x88\x98\x88\x89o\xf7\xff\xaa\x88o\xff\x0e\xed\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', + ], + }, + CAR.VELOSTER: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ', + b'\xf1\x00JS__ SCC HNCUP 1.00 1.02 95650-J3100 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816V8RAC00121.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x01TJS-JNU06F200H0A', + b'\x01TJS-JDK06F200H0A', + b'391282BJF5 ', + ], + (Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', + b'\xf1\x00JS LKAS AT KOR LHD 1.00 1.03 95740-J3000 K33', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00', + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16KS2\016\xba\036\xa2', + ], + }, + CAR.GENESIS_G70: { + (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', ], + (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ], + (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ], + (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ], + }, + CAR.GENESIS_G70_2020: { + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9220 4I2VL107', + b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9420 4I4VL107', + b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9420 4I4VL108', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00', + b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', + b'\xf1\x87VDKLT18912362DN4wfVfwefeveVUwfvw\x88vWfvUFU\x89\xa9\x8f\xff\x87w\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', + b'\xf1\x87VDJLC18480772DK9\x88eHfwfff\x87eFUeDEU\x98eFe\x86T5DVyo\xff\x87s\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33KB5\x9f\xa5&\x81', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', + b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', + b'\xf1\x00IK__ SCC FHCUP 1.00 1.02 96400-G9000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', + b'\xf1\x00IK MFC AT KOR LHD 1.00 1.01 95740-G9000 170920', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.GENESIS_G90: { + (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7'], + (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 '], + (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2020 160302'], + (Ecu.engine, 0x7e0, None): [b'\xf1\x810000000000\x00'], + }, + CAR.KONA: { + (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 ', ], + (Ecu.abs, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.engine, 0x7e0, None): [b'"\x01TOS-0NU06F301J02', ], + (Ecu.eps, 0x7d4, None): [b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ], + (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ], + (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ], + }, + CAR.KIA_CEED: { + (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\000CD__ SCC F-CUP 1.00 1.02 99110-J7000 ', ], + (Ecu.eps, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ], + (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\000CD LKAS AT EUR LHD 1.00 1.01 99211-J7000 B40', ], + (Ecu.engine, 0x7E0, None): [b'\001TCD-JECU4F202H0K', ], + (Ecu.transmission, 0x7E1, None): [ + b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000', + b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1U\x867Z', + ], + (Ecu.abs, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ], + }, + CAR.KIA_FORTE: { + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00BD MDPS C 1.00 1.02 56310-XX000 4BD2C102', + b'\xf1\x00BD MDPS C 1.00 1.08 56310/M6300 4BDDC108', + b'\xf1\x00BD MDPS C 1.00 1.08 56310M6300\x00 4BDDC108', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00BD__ SCC H-CUP 1.00 1.02 99110-M6000 ', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x01TBDM1NU06F200H01', + b'391182B945\x00', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\x00\x00\x00\x00', + b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3", + ], + }, + CAR.KIA_K5_2021: { + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', + b'\xf1\x8799110L2000\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', + b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', + b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101', + b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', + b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915', + b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.04 99210-L3000 210208', + ], + (Ecu.abs, 0x7D1, None): [ + b'\xf1\000DL ESC \006 101 \004\002 58910-L3200', + b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200', + b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', + b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600', + ], + (Ecu.engine, 0x7E0, None): [ + b'\xf1\x87391212MKT0', + b'\xf1\x87391212MKV0', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DLDWN5TMDCXXXJ1B', + ], + (Ecu.transmission, 0x7E1, None): [ + b'\xf1\000bcsh8p54 U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8', + b'\xf1\x87SALFEA5652514GK2UUeV\x88\x87\x88xxwg\x87ww\x87wwfwvd/\xfb\xffvU_\xff\x93\xd3\xf1\x81U913\000\000\000\000\000\000\xf1\000bcsh8p54 U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8', + b'\xf1\x87SALFEA6046104GK2wvwgeTeFg\x88\x96xwwwwffvfe?\xfd\xff\x86fo\xff\x97A\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', + b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', + b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 WDL3T25XXX730NS2b\x1f\xb8%', + ], + }, + CAR.KONA_EV: { + (Ecu.abs, 0x7D1, None): [ + b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000', + b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000', + b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000', + b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000', + b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00OE2 LKAS AT EUR LHD 1.00 1.00 95740-K4200 200', + b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT EUR RHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT KOR LHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4300 W50', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00OS MDPS C 1.00 1.03 56310/K4550 4OEDC103', + b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104', + b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00OSev SCC F-CUP 1.00 1.00 99110-K4000 ', + b'\xf1\x00OSev SCC F-CUP 1.00 1.00 99110-K4100 ', + b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 ', + b'\xf1\x00OSev SCC FNCUP 1.00 1.01 99110-K4000 ', + ], + }, + CAR.KONA_EV_2022: { + (Ecu.abs, 0x7D1, None): [ + b'\xf1\x8758520-K4010\xf1\x00OS IEB \x02 101 \x11\x13 58520-K4010', + b'\xf1\x8758520-K4010\xf1\x00OS IEB \x04 101 \x11\x13 58520-K4010', + b'\xf1\x8758520-K4010\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010', + # TODO: these return from the MULTI request, above return from LONG + b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xee\xff\xe0\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xffP\xf5\xff\xfd\x00\x00\x00\x00\xfc\x00\x01', + b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xdb\x00\x00\x00\x00\xff\xb1\xff\xd9\xff\xd2\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xff\xd6\xf5\x00\x06\x00\x00\x00\x14\xfd\x00\x04', + b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xf4\xff\xd9\x00\xc0', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00OSP LKA AT CND LHD 1.00 1.02 99211-J9110 802', + b'\xf1\x00OSP LKA AT EUR RHD 1.00 1.02 99211-J9110 802', + b'\xf1\x00OSP LKA AT AUS RHD 1.00 1.04 99211-J9200 904', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4260\x00 4OEPC102', + b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4970 4OEPC102', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-K4500 \x00\x00\x00', + ], + }, + CAR.KIA_NIRO_EV: { + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4000 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4100 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ', + b'\xf1\x00DEev SCC FHCUP 1.00 1.03 96400-Q4000 ', + b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', + b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', + b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', + b'\xf1\x8799110Q4600\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ', + b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', + b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105', + b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105', + b'\xf1\x00DE MDPS C 1.00 1.04 56310Q4100\x00 4DEEC104', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706', + b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211', + b'\xf1\x00DEE MFC AT USA LHD 1.00 1.00 99211-Q4000 191211', + b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821', + b'\xf1\x00DEE MFC AT USA LHD 1.00 1.01 99211-Q4500 210428', + b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.03 95740-Q4000 180821', + b'\xf1\x00DEE MFC AT KOR LHD 1.00 1.03 95740-Q4000 180821', + ], + }, + CAR.KIA_NIRO_PHEV: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91", + b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\x00\x00\x00\x00', + b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', + b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', + ], + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', + b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ', + ], + }, + CAR.KIA_NIRO_HEV_2021: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\x00\x00\x00\x00', + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\xb9\xd3\xfaW', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DEH MFC AT USA LHD 1.00 1.07 99211-G5000 201221', + b'\xf1\x00DEH MFC AT USA LHD 1.00 1.00 99211-G5500 210428', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00DEhe SCC FHCUP 1.00 1.00 99110-G5600 ', + ], + }, + CAR.KIA_SELTOS: { + (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 ',], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450', + b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81616D2051\000\000\000\000\000\000\000\000', + b'\xf1\x81616D5051\000\000\000\000\000\000\000\000', + b'\001TSP2KNL06F100J0K', + b'\001TSP2KNL06F200J0K', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\000SP2 MDPS C 1.00 1.04 56300Q5200 ', + b'\xf1\000SP2 MDPS C 1.01 1.05 56300Q5200 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\000SP2 MFC AT USA LHD 1.00 1.04 99210-Q5000 191114', + b'\xf1\000SP2 MFC AT USA LHD 1.00 1.05 99210-Q5000 201012', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87CZLUB49370612JF7h\xa8y\x87\x99\xa7hv\x99\x97fv\x88\x87x\x89x\x96O\xff\x88\xff\xff\xff.@\xf1\x816V2C2051\000\000\xf1\0006V2B0_C2\000\0006V2C2051\000\000CSP4N20NS3\000\000\000\000', + b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS6\xd30\xa5\xb9', + b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b', + ], + }, + CAR.KIA_OPTIMA: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00JF ESC \x0b 11 \x18\x030 58920-D5180', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x89F1JF600AISEIU702\xf1\x82F1JF600AISEIU702', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5001 h32', + b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW', + b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\xca3\xeb.', + b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\x00\x00\x00\x00', + ], + }, + CAR.ELANTRA_2021: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', + b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', + b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', + b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106', + b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', + b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA000 200819', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', + b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', + b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', + b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\xe8\xba\xce\xfa', + b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\177\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', + b'\xf1\x87CXMQFM1916035JB2\x88vvgg\x87Wuwgev\xa9\x98\x88\x98h\x99\x9f\xffh\xff\xff\xff\xa5\xee\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', + b'\xf1\x87CXLQF40189012JL2f\x88\x86\x88\x88vUex\xb8\x88\x88\x88\x87\x88\x89fh?\xffz\xff\xff\xff\x08z\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', + b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', + b'\xf1\x87CXMQFM3806705JB2\x89\x87wwx\x88g\x86\x99\x87\x86xwwv\x88yv\x7f\xffz\xff\xff\xffV\x15\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x82CNCWD0AMFCXCSFFA', + b'\xf1\x82CNCWD0AMFCXCSFFB', + b'\xf1\x82CNCVD0AMFCXCSFFB', + b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_G80', + ], + }, + CAR.ELANTRA_HEV_2021: { + (Ecu.fwdCamera, 0x7c4, None) : [ + b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA000 210930', + b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819', + ], + (Ecu.fwdRadar, 0x7d0, None) : [ + b'\xf1\000CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', + b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', + ], + (Ecu.eps, 0x7d4, None) :[ + b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103', + b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102', + ], + (Ecu.transmission, 0x7e1, None) :[ + b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa', + b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000', + b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa', + b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None) : [ + b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', + ] + }, + CAR.KONA_HEV: { + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00OS IEB \x01 104 \x11 58520-CM000', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00OShe SCC FNCUP 1.00 1.01 99110-CM000 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00OS MDPS C 1.00 1.00 56310CM030\x00 4OHDC100', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00OSH LKAS AT KOR LHD 1.00 1.01 95740-CM000 l31', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HOS0G16DS1\x16\xc7\xb0\xd9', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', + ] + }, + CAR.SONATA_HYBRID: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\000DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', + b'\xf1\x8799110L5000\xf1\000DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', + b'\xf1\000DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', + b'\xf1\x8799110L5000\xf1\000DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x8756310-L5500\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102', + b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102', + b'\xf1\x8756310-L5450\xf1\000DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.04 99211-L1000 191016', + b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.05 99211-L1000 201109', + b'\xf1\000DN8HMFC AT USA LHD 1.00 1.06 99211-L1000 210325', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\000PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW', + b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E09\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2323 E09\x00\x00\x00\x00\x00\x00\x00TDN2H20SA5\x97R\x88\x9e', + b'\xf1\000PSBG2323 E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e', + b'\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab', + b'\xf1\x87PCU\000\000\000\000\000\000\000\000\000\xf1\x81E16\000\000\000\000\000\000\000\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab', + b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x87391162J012', + b'\xf1\x87391162J013', + b'\xf1\x87391062J002', + ], + }, + CAR.KIA_SORENTO: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01' + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00UM ESC \x0c 12 \x18\x05\x06 58910-C6330' + ], + (Ecu.fwdRadar, 0x7D0, None): [ + b'\xf1\x00UM__ SCC F-CUP 1.00 1.00 96400-C6500 ' + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LDKUAA0348164HE3\x87www\x87www\x88\x88\xa8\x88w\x88\x97xw\x88\x97x\x86o\xf8\xff\x87f\x7f\xff\x15\xe0\xf1\x81U811\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U811\x00\x00\x00\x00\x00\x00TUM4G33NL3V|DG' + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00' + ], + }, + CAR.KIA_EV6: { + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x8758520CV100\xf1\x00CV IEB \x02 101!\x10\x18 58520-CV100', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CV1 MDPS R 1.00 1.04 57700-CV000 1B30', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', + b'\xf1\x8799110CV000\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.05 99210-CV000 211027', + ], + }, + CAR.IONIQ_5: { + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', + b'\xf1\x8758520GI010\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', + b'\xf1\x00NE1 IEB \x08 104!\x04\x05 58520-GI000', + b'\xf1\x8758520GI000\xf1\x00NE1 IEB \x08 104!\x04\x05 58520-GI000', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00NE MDPS R 1.00 1.06 57700GI000 4NEDR106', + b'\xf1\x8757700GI000 \xf1\x00NE MDPS R 1.00 1.06 57700GI000 4NEDR106', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', + b'\xf1\x8799110GI000\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.02 99211-GI010 211206', + b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', + ], + }, + CAR.TUCSON_HYBRID_4TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9240 14Q', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00NX4 MDPS C 1.00 1.01 56300-P0100 2228', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x87391312MND0', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00PSBG2441 G19_Rev\x00\x00\x00SNX4T16XXHS01NS2lS\xdfa', + b'\xf1\x8795441-3D220\x00\xf1\x81G19_Rev\x00\x00\x00\xf1\x00PSBG2441 G19_Rev\x00\x00\x00SNX4T16XXHS01NS2lS\xdfa', + ], + }, +} + +CHECKSUM = { + "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022], + "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], +} + +FEATURES = { + # which message has the gear + "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, + "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, + "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022}, + + # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 + "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022}, +} + +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN} + +# The camera does SCC on these cars, rather than the radar +CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } + +HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019} # these cars use a different gas signal +EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022} + +# these cars require a special panda safety mode due to missing counters and checksums in the messages +LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} + +# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. +# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py +DBC = { + CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), + CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None), + CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None), + CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), + CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None), + CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None), + CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None), + CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format + CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), + CAR.KONA: dbc_dict('hyundai_kia_generic', None), + CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), + CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None), + CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None), + CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), + CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), + CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), + CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format + CAR.TUCSON: dbc_dict('hyundai_kia_generic', None), + CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), + CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'), + CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.IONIQ_5: dbc_dict('hyundai_canfd', None), +} diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py new file mode 100644 index 00000000000000..a33560cd0e618c --- /dev/null +++ b/selfdrive/car/interfaces.py @@ -0,0 +1,389 @@ +import yaml +import os +import time +from abc import abstractmethod, ABC +from typing import Any, Dict, Optional, Tuple, List + +from cereal import car +from common.basedir import BASEDIR +from common.conversions import Conversions as CV +from common.kalman.simple_kalman import KF1D +from common.realtime import DT_CTRL +from selfdrive.car import create_button_enable_events, gen_empty_fingerprint +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.controls.lib.events import Events +from selfdrive.controls.lib.vehicle_model import VehicleModel + +GearShifter = car.CarState.GearShifter +EventName = car.CarEvent.EventName + +MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS +ACCEL_MAX = 2.0 +ACCEL_MIN = -3.5 + +TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml') +TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml') +TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml') + + +def get_torque_params(candidate): + with open(TORQUE_SUBSTITUTE_PATH) as f: + sub = yaml.load(f, Loader=yaml.CSafeLoader) + if candidate in sub: + candidate = sub[candidate] + + with open(TORQUE_PARAMS_PATH) as f: + params = yaml.load(f, Loader=yaml.CSafeLoader) + with open(TORQUE_OVERRIDE_PATH) as f: + override = yaml.load(f, Loader=yaml.CSafeLoader) + + # Ensure no overlap + if sum([candidate in x for x in [sub, params, override]]) > 1: + raise RuntimeError(f'{candidate} is defined twice in torque config') + + if candidate in override: + out = override[candidate] + elif candidate in params: + out = params[candidate] + else: + raise NotImplementedError(f"Did not find torque params for {candidate}") + return {key: out[i] for i, key in enumerate(params['legend'])} + + +# generic car and radar interfaces + +class CarInterfaceBase(ABC): + def __init__(self, CP, CarController, CarState): + self.CP = CP + self.VM = VehicleModel(CP) + + self.frame = 0 + self.steering_unpressed = 0 + self.low_speed_alert = False + self.silent_steer_warning = True + self.v_ego_cluster_seen = False + + self.CS = None + self.can_parsers = [] + if CarState is not None: + self.CS = CarState(CP) + + self.cp = self.CS.get_can_parser(CP) + self.cp_cam = self.CS.get_cam_can_parser(CP) + self.cp_adas = self.CS.get_adas_can_parser(CP) + self.cp_body = self.CS.get_body_can_parser(CP) + self.cp_loopback = self.CS.get_loopback_can_parser(CP) + self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback] + + self.CC = None + if CarController is not None: + self.CC = CarController(self.cp.dbc_name, CP, self.VM) + + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + return ACCEL_MIN, ACCEL_MAX + + @staticmethod + @abstractmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + pass + + @staticmethod + def init(CP, logcan, sendcan): + pass + + @staticmethod + def get_steer_feedforward_default(desired_angle, v_ego): + # Proportional to realigning tire momentum: lateral acceleration. + # TODO: something with lateralPlan.curvatureRates + return desired_angle * (v_ego**2) + + def get_steer_feedforward_function(self): + return self.get_steer_feedforward_default + + # returns a set of default params to avoid repetition in car specific params + @staticmethod + def get_std_params(candidate, fingerprint): + ret = car.CarParams.new_message() + ret.carFingerprint = candidate + + # Car docs fields + ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] + ret.autoResumeSng = True # describes whether car can resume from a stop automatically + + # standard ALC params + ret.steerControlType = car.CarParams.SteerControlType.torque + ret.minSteerSpeed = 0. + ret.wheelSpeedFactor = 1.0 + + ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars + ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this + ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA + ret.openpilotLongitudinalControl = False + ret.stopAccel = -2.0 + ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop + ret.vEgoStopping = 0.5 + ret.vEgoStarting = 0.5 + ret.stoppingControl = True + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kf = 1. + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [1.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [1.] + # TODO estimate car specific lag, use .15s for now + ret.longitudinalActuatorDelayLowerBound = 0.15 + ret.longitudinalActuatorDelayUpperBound = 0.15 + ret.steerLimitTimer = 1.0 + return ret + + @staticmethod + def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): + params = get_torque_params(candidate) + + tune.init('torque') + tune.torque.useSteeringAngle = use_steering_angle + tune.torque.kp = 1.0 / params['LAT_ACCEL_FACTOR'] + tune.torque.kf = 1.0 / params['LAT_ACCEL_FACTOR'] + tune.torque.ki = 0.1 / params['LAT_ACCEL_FACTOR'] + tune.torque.friction = params['FRICTION'] + tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg + + @abstractmethod + def _update(self, c: car.CarControl) -> car.CarState: + pass + + def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState: + # parse can + for cp in self.can_parsers: + if cp is not None: + cp.update_strings(can_strings) + + # get CarState + ret = self._update(c) + + ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None) + ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None) + + if ret.vEgoCluster == 0.0 and not self.v_ego_cluster_seen: + ret.vEgoCluster = ret.vEgo + else: + self.v_ego_cluster_seen = True + + if ret.cruiseState.speedCluster == 0: + ret.cruiseState.speedCluster = ret.cruiseState.speed + + # copy back for next iteration + reader = ret.as_reader() + if self.CS is not None: + self.CS.out = reader + + return reader + + @abstractmethod + def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]: + pass + + def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True): + events = Events() + + if cs_out.doorOpen: + events.add(EventName.doorOpen) + if cs_out.seatbeltUnlatched: + events.add(EventName.seatbeltNotLatched) + if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or + cs_out.gearShifter not in extra_gears): + events.add(EventName.wrongGear) + if cs_out.gearShifter == GearShifter.reverse: + events.add(EventName.reverseGear) + if not cs_out.cruiseState.available: + events.add(EventName.wrongCarMode) + if cs_out.espDisabled: + events.add(EventName.espDisabled) + if cs_out.stockFcw: + events.add(EventName.stockFcw) + if cs_out.stockAeb: + events.add(EventName.stockAeb) + if cs_out.vEgo > MAX_CTRL_SPEED: + events.add(EventName.speedTooHigh) + if cs_out.cruiseState.nonAdaptive: + events.add(EventName.wrongCruiseMode) + if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl: + events.add(EventName.brakeHold) + if cs_out.parkingBrake: + events.add(EventName.parkBrake) + if cs_out.accFaulted: + events.add(EventName.accFaulted) + + # Handle button presses + events.events.extend(create_button_enable_events(cs_out.buttonEvents, pcm_cruise=self.CP.pcmCruise)) + + # Handle permanent and temporary steering faults + self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1 + if cs_out.steerFaultTemporary: + # if the user overrode recently, show a less harsh alert + if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL): + self.silent_steer_warning = True + events.add(EventName.steerTempUnavailableSilent) + else: + events.add(EventName.steerTempUnavailable) + else: + self.silent_steer_warning = False + if cs_out.steerFaultPermanent: + events.add(EventName.steerUnavailable) + + # we engage when pcm is active (rising edge) + # enabling can optionally be blocked by the car interface + if pcm_enable: + if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable: + events.add(EventName.pcmEnable) + elif not cs_out.cruiseState.enabled: + events.add(EventName.pcmDisable) + + return events + + +class RadarInterfaceBase(ABC): + def __init__(self, CP): + self.rcp = None + self.pts = {} + self.delay = 0 + self.radar_ts = CP.radarTimeStep + self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ + + def update(self, can_strings): + ret = car.RadarData.new_message() + if not self.no_radar_sleep: + time.sleep(self.radar_ts) # radard runs on RI updates + return ret + + +class CarStateBase(ABC): + def __init__(self, CP): + self.CP = CP + self.car_fingerprint = CP.carFingerprint + self.out = car.CarState.new_message() + + self.cruise_buttons = 0 + self.left_blinker_cnt = 0 + self.right_blinker_cnt = 0 + self.left_blinker_prev = False + self.right_blinker_prev = False + + # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) + # R = 0.3 + self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], + A=[[1.0, DT_CTRL], [0.0, 1.0]], + C=[1.0, 0.0], + K=[[0.17406039], [1.65925647]]) + + def update_speed_kf(self, v_ego_raw): + if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_kf.x = [[v_ego_raw], [0.0]] + + v_ego_x = self.v_ego_kf.update(v_ego_raw) + return float(v_ego_x[0]), float(v_ego_x[1]) + + def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS): + factor = unit * self.CP.wheelSpeedFactor + + wheelSpeeds = car.CarState.WheelSpeeds.new_message() + wheelSpeeds.fl = fl * factor + wheelSpeeds.fr = fr * factor + wheelSpeeds.rl = rl * factor + wheelSpeeds.rr = rr * factor + return wheelSpeeds + + def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool): + """Update blinkers from lights. Enable output when light was seen within the last `blinker_time` + iterations""" + # TODO: Handle case when switching direction. Now both blinkers can be on at the same time + self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0) + self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0) + return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0 + + def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool): + """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time, + or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker + is forced to the other side. On a rising edge of the stalk the timeout is reset.""" + + if left_blinker_stalk: + self.right_blinker_cnt = 0 + if not self.left_blinker_prev: + self.left_blinker_cnt = blinker_time + + if right_blinker_stalk: + self.left_blinker_cnt = 0 + if not self.right_blinker_prev: + self.right_blinker_cnt = blinker_time + + self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0) + self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0) + + self.left_blinker_prev = left_blinker_stalk + self.right_blinker_prev = right_blinker_stalk + + return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0) + + @staticmethod + def parse_gear_shifter(gear: Optional[str]) -> car.CarState.GearShifter: + if gear is None: + return GearShifter.unknown + + d: Dict[str, car.CarState.GearShifter] = { + 'P': GearShifter.park, 'PARK': GearShifter.park, + 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, + 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, + 'E': GearShifter.eco, 'ECO': GearShifter.eco, + 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, + 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, + 'S': GearShifter.sport, 'SPORT': GearShifter.sport, + 'L': GearShifter.low, 'LOW': GearShifter.low, + 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, + } + return d.get(gear.upper(), GearShifter.unknown) + + @staticmethod + def get_cam_can_parser(CP): + return None + + @staticmethod + def get_adas_can_parser(CP): + return None + + @staticmethod + def get_body_can_parser(CP): + return None + + @staticmethod + def get_loopback_can_parser(CP): + return None + + +# interface-specific helpers + +def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str, Any]: + # read all the folders in selfdrive/car and return a dict where: + # - keys are all the car models or brand names + # - values are attr values from all car folders + result = {} + for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]): + try: + brand_name = car_folder.split('/')[-1] + brand_values = __import__(f'selfdrive.car.{brand_name}.values', fromlist=[attr]) + if hasattr(brand_values, attr) or not ignore_none: + attr_data = getattr(brand_values, attr, None) + else: + continue + + if combine_brands: + if isinstance(attr_data, dict): + for f, v in attr_data.items(): + result[f] = v + else: + result[brand_name] = attr_data + except (ImportError, OSError): + pass + + return result diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py new file mode 100644 index 00000000000000..65122ab8971b9b --- /dev/null +++ b/selfdrive/car/isotp_parallel_query.py @@ -0,0 +1,153 @@ +import time +from collections import defaultdict +from functools import partial +from typing import Optional + +import cereal.messaging as messaging +from system.swaglog import cloudlog +from selfdrive.boardd.boardd import can_list_to_can_capnp +from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr + + +class IsoTpParallelQuery: + def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False, response_pending_timeout=10): + self.sendcan = sendcan + self.logcan = logcan + self.bus = bus + self.request = request + self.response = response + self.debug = debug + self.functional_addr = functional_addr + self.response_pending_timeout = response_pending_timeout + + self.real_addrs = [] + for a in addrs: + if isinstance(a, tuple): + self.real_addrs.append(a) + else: + self.real_addrs.append((a, None)) + + self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in self.real_addrs} + self.msg_buffer = defaultdict(list) + + def rx(self): + """Drain can socket and sort messages into buffers based on address""" + can_packets = messaging.drain_sock(self.logcan, wait_for_one=True) + + for packet in can_packets: + for msg in packet.can: + if msg.src == self.bus: + if self.functional_addr: + if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF): + fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32) + self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src)) + elif msg.address in self.msg_addrs.values(): + self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src)) + + def _can_tx(self, tx_addr, dat, bus): + """Helper function to send single message""" + msg = [tx_addr, 0, dat, bus] + self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan')) + + def _can_rx(self, addr, sub_addr=None): + """Helper function to retrieve message with specified address and subadress from buffer""" + keep_msgs = [] + + if sub_addr is None: + msgs = self.msg_buffer[addr] + else: + # Filter based on subadress + msgs = [] + for m in self.msg_buffer[addr]: + first_byte = m[2][0] + if first_byte == sub_addr: + msgs.append(m) + else: + keep_msgs.append(m) + + self.msg_buffer[addr] = keep_msgs + return msgs + + def _drain_rx(self): + messaging.drain_sock(self.logcan) + self.msg_buffer = defaultdict(list) + + def get_data(self, timeout, total_timeout=60.): + self._drain_rx() + + # Create message objects + msgs = {} + request_counter = {} + request_done = {} + for tx_addr, rx_addr in self.msg_addrs.items(): + # rx_addr not set when using functional tx addr + id_addr = rx_addr or tx_addr[0] + sub_addr = tx_addr[1] + + can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr, + self.bus, sub_addr=sub_addr, debug=self.debug) + + max_len = 8 if sub_addr is None else 7 + + msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug) + msg.send(self.request[0]) + + msgs[tx_addr] = msg + request_counter[tx_addr] = 0 + request_done[tx_addr] = False + + results = {} + start_time = time.monotonic() + response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs} + while True: + self.rx() + + if all(request_done.values()): + break + + for tx_addr, msg in msgs.items(): + try: + dat: Optional[bytes] = msg.recv() + except Exception: + cloudlog.exception("Error processing UDS response") + request_done[tx_addr] = True + continue + + if not dat: + continue + + counter = request_counter[tx_addr] + expected_response = self.response[counter] + response_valid = dat[:len(expected_response)] == expected_response + + if response_valid: + response_timeouts[tx_addr] = time.monotonic() + timeout + if counter + 1 < len(self.request): + msg.send(self.request[counter + 1]) + request_counter[tx_addr] += 1 + else: + results[(tx_addr, msg._can_client.rx_addr)] = dat[len(expected_response):] + request_done[tx_addr] = True + else: + error_code = dat[2] if len(dat) > 2 else -1 + if error_code == 0x78: + response_timeouts[tx_addr] = time.monotonic() + self.response_pending_timeout + if self.debug: + cloudlog.warning(f"iso-tp query response pending: {tx_addr}") + else: + response_timeouts[tx_addr] = 0 + request_done[tx_addr] = True + cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") + + cur_time = time.monotonic() + if cur_time - max(response_timeouts.values()) > 0: + for tx_addr in msgs: + if request_counter[tx_addr] > 0 and not request_done[tx_addr]: + cloudlog.warning(f"iso-tp query timeout after receiving response: {tx_addr}") + break + + if cur_time - start_time > total_timeout: + cloudlog.warning("iso-tp query timeout while receiving data") + break + + return results diff --git a/system/loggerd/tests/__init__.py b/selfdrive/car/mazda/__init__.py similarity index 100% rename from system/loggerd/tests/__init__.py rename to selfdrive/car/mazda/__init__.py diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py new file mode 100644 index 00000000000000..2add59ccb05884 --- /dev/null +++ b/selfdrive/car/mazda/carcontroller.py @@ -0,0 +1,64 @@ +from cereal import car +from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.mazda import mazdacan +from selfdrive.car.mazda.values import CarControllerParams, Buttons + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.apply_steer_last = 0 + self.packer = CANPacker(dbc_name) + self.brake_counter = 0 + self.frame = 0 + + def update(self, CC, CS): + can_sends = [] + + apply_steer = 0 + + if CC.latActive: + # calculate steer and also set limits due to driver torque + new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, + CS.out.steeringTorque, CarControllerParams) + + if CC.cruiseControl.cancel: + # If brake is pressed, let us wait >70ms before trying to disable crz to avoid + # a race condition with the stock system, where the second cancel from openpilot + # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to + # read 3 messages and most likely sync state before we attempt cancel. + self.brake_counter = self.brake_counter + 1 + if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): + # Cancel Stock ACC if it's enabled while OP is disengaged + # Send at a rate of 10hz until we sync with stock ACC state + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) + else: + self.brake_counter = 0 + if CC.cruiseControl.resume and self.frame % 5 == 0: + # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds + # Send Resume button when planner wants car to move + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) + + self.apply_steer_last = apply_steer + + # send HUD alerts + if self.frame % 50 == 0: + ldw = CC.hudControl.visualAlert == VisualAlert.ldw + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + # TODO: find a way to silence audible warnings so we can add more hud alerts + steer_required = steer_required and CS.lkas_allowed_speed + can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) + + # send steering command + can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, + self.frame, apply_steer, CS.cam_lkas)) + + new_actuators = CC.actuators.copy() + new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py new file mode 100644 index 00000000000000..944d79809b1606 --- /dev/null +++ b/selfdrive/car/mazda/carstate.py @@ -0,0 +1,208 @@ +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.shifter_values = can_define.dv["GEAR"]["GEAR"] + + self.crz_btns_counter = 0 + self.acc_active_last = False + self.low_speed_alert = False + self.lkas_allowed_speed = False + self.lkas_disabled = False + + def update(self, cp, cp_cam): + + ret = car.CarState.new_message() + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS"]["FL"], + cp.vl["WHEEL_SPEEDS"]["FR"], + cp.vl["WHEEL_SPEEDS"]["RL"], + cp.vl["WHEEL_SPEEDS"]["RR"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + + # Match panda speed reading + speed_kph = cp.vl["ENGINE_DATA"]["SPEED"] + ret.standstill = speed_kph < .1 + + can_gear = int(cp.vl["GEAR"]["GEAR"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + + ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"]) + ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1 + ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1 + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, + cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] + ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + + ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"] + ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"] + + # TODO: this should be from 0 - 1. + ret.brakePressed = cp.vl["PEDALS"]["BRAKE_ON"] == 1 + ret.brake = cp.vl["BRAKE"]["BRAKE_PRESSURE"] + + ret.seatbeltUnlatched = cp.vl["SEATBELT"]["DRIVER_SEATBELT"] == 0 + ret.doorOpen = any([cp.vl["DOORS"]["FL"], cp.vl["DOORS"]["FR"], + cp.vl["DOORS"]["BL"], cp.vl["DOORS"]["BR"]]) + + # TODO: this should be from 0 - 1. + ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"] + ret.gasPressed = ret.gas > 0 + + # Either due to low speed or hands off + lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1 + + if self.CP.minSteerSpeed > 0: + # LKAS is enabled at 52kph going up and disabled at 45kph going down + # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes + if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: + self.lkas_allowed_speed = True + elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: + self.lkas_allowed_speed = False + else: + self.lkas_allowed_speed = True + + # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on + # it should be used for carState.cruiseState.nonAdaptive instead + ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1 + ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1 + ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS + + if ret.cruiseState.enabled: + if not self.lkas_allowed_speed and self.acc_active_last: + self.low_speed_alert = True + else: + self.low_speed_alert = False + + # Check if LKAS is disabled due to lack of driver torque when all other states indicate + # it should be enabled (steer lockout). Don't warn until we actually get lkas active + # and lose it again, i.e, after initial lkas activation + ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked + + self.acc_active_last = ret.cruiseState.enabled + + self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] + + # camera signals + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 + self.cam_lkas = cp_cam.vl["CAM_LKAS"] + self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] + ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("LEFT_BLINK", "BLINK_INFO"), + ("RIGHT_BLINK", "BLINK_INFO"), + ("HIGH_BEAMS", "BLINK_INFO"), + ("STEER_ANGLE", "STEER"), + ("STEER_ANGLE_RATE", "STEER_RATE"), + ("STEER_TORQUE_SENSOR", "STEER_TORQUE"), + ("STEER_TORQUE_MOTOR", "STEER_TORQUE"), + ("FL", "WHEEL_SPEEDS"), + ("FR", "WHEEL_SPEEDS"), + ("RL", "WHEEL_SPEEDS"), + ("RR", "WHEEL_SPEEDS"), + ] + + checks = [ + # sig_address, frequency + ("BLINK_INFO", 10), + ("STEER", 67), + ("STEER_RATE", 83), + ("STEER_TORQUE", 83), + ("WHEEL_SPEEDS", 100), + ] + + if CP.carFingerprint in GEN1: + signals += [ + ("LKAS_BLOCK", "STEER_RATE"), + ("LKAS_TRACK_STATE", "STEER_RATE"), + ("HANDS_OFF_5_SECONDS", "STEER_RATE"), + ("CRZ_ACTIVE", "CRZ_CTRL"), + ("CRZ_AVAILABLE", "CRZ_CTRL"), + ("CRZ_SPEED", "CRZ_EVENTS"), + ("STANDSTILL", "PEDALS"), + ("BRAKE_ON", "PEDALS"), + ("BRAKE_PRESSURE", "BRAKE"), + ("GEAR", "GEAR"), + ("DRIVER_SEATBELT", "SEATBELT"), + ("FL", "DOORS"), + ("FR", "DOORS"), + ("BL", "DOORS"), + ("BR", "DOORS"), + ("PEDAL_GAS", "ENGINE_DATA"), + ("SPEED", "ENGINE_DATA"), + ("CTR", "CRZ_BTNS"), + ("LEFT_BS1", "BSM"), + ("RIGHT_BS1", "BSM"), + ] + + checks += [ + ("ENGINE_DATA", 100), + ("CRZ_CTRL", 50), + ("CRZ_EVENTS", 50), + ("CRZ_BTNS", 10), + ("PEDALS", 50), + ("BRAKE", 50), + ("SEATBELT", 10), + ("DOORS", 10), + ("GEAR", 20), + ("BSM", 10), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [] + + if CP.carFingerprint in GEN1: + signals += [ + # sig_name, sig_address + ("LKAS_REQUEST", "CAM_LKAS"), + ("CTR", "CAM_LKAS"), + ("ERR_BIT_1", "CAM_LKAS"), + ("LINE_NOT_VISIBLE", "CAM_LKAS"), + ("BIT_1", "CAM_LKAS"), + ("ERR_BIT_2", "CAM_LKAS"), + ("STEERING_ANGLE", "CAM_LKAS"), + ("ANGLE_ENABLED", "CAM_LKAS"), + ("CHKSUM", "CAM_LKAS"), + + ("LINE_VISIBLE", "CAM_LANEINFO"), + ("LINE_NOT_VISIBLE", "CAM_LANEINFO"), + ("LANE_LINES", "CAM_LANEINFO"), + ("BIT1", "CAM_LANEINFO"), + ("BIT2", "CAM_LANEINFO"), + ("BIT3", "CAM_LANEINFO"), + ("NO_ERR_BIT", "CAM_LANEINFO"), + ("S1", "CAM_LANEINFO"), + ("S1_HBEAM", "CAM_LANEINFO"), + ] + + checks += [ + # sig_address, frequency + ("CAM_LANEINFO", 2), + ("CAM_LKAS", 16), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py new file mode 100755 index 00000000000000..89ed5638cbd508 --- /dev/null +++ b/selfdrive/car/mazda/interface.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car.mazda.values import CAR, LKAS_LIMITS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName + +class CarInterface(CarInterfaceBase): + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 4.0 + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "mazda" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] + ret.radarOffCan = True + + ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) + + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.8 + tire_stiffness_factor = 0.70 # not optimized yet + + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + if candidate in (CAR.CX5, CAR.CX5_2022): + ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 15.5 + elif candidate in (CAR.CX9, CAR.CX9_2021): + ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.1 + ret.steerRatio = 17.6 + elif candidate == CAR.MAZDA3: + ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 14.0 + elif candidate == CAR.MAZDA6: + ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 15.5 + + if candidate not in (CAR.CX5_2022, ): + ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS + + ret.centerToFront = ret.wheelbase * 0.41 + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + # events + events = self.create_common_events(ret) + + if self.CS.lkas_disabled: + events.add(EventName.lkasDisabled) + elif self.CS.low_speed_alert: + events.add(EventName.belowSteerSpeed) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py new file mode 100644 index 00000000000000..e2ee93e022de90 --- /dev/null +++ b/selfdrive/car/mazda/mazdacan.py @@ -0,0 +1,119 @@ +import copy + +from selfdrive.car.mazda.values import GEN1, Buttons + + +def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): + + tmp = apply_steer + 2048 + + lo = tmp & 0xFF + hi = tmp >> 8 + + # copy values from camera + b1 = int(lkas["BIT_1"]) + er1 = int(lkas["ERR_BIT_1"]) + lnv = 0 + ldw = 0 + er2 = int(lkas["ERR_BIT_2"]) + + # Some older models do have these, newer models don't. + # Either way, they all work just fine if set to zero. + steering_angle = 0 + b2 = 0 + + tmp = steering_angle + 2048 + ahi = tmp >> 10 + amd = (tmp & 0x3FF) >> 2 + amd = (amd >> 4) | (( amd & 0xF) << 4) + alo = (tmp & 0x3) << 2 + + ctr = frame % 16 + # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] + csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5) + + # bytes [ 5 ] [ 6 ] [ 7 ] + csum = csum - ahi - amd - alo - b2 + + if ahi == 1: + csum = csum + 15 + + if csum < 0: + if csum < -256: + csum = csum + 512 + else: + csum = csum + 256 + + csum = csum % 256 + + if car_fingerprint in GEN1: + values = { + "LKAS_REQUEST": apply_steer, + "CTR": ctr, + "ERR_BIT_1": er1, + "LINE_NOT_VISIBLE" : lnv, + "LDW": ldw, + "BIT_1": b1, + "ERR_BIT_2": er2, + "STEERING_ANGLE": steering_angle, + "ANGLE_ENABLED": b2, + "CHKSUM": csum + } + + return packer.make_can_msg("CAM_LKAS", 0, values) + + +def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): + values = copy.copy(cam_msg) + values.update({ + # TODO: what's the difference between all these? do we need to send all? + "HANDS_WARN_3_BITS": 0b111 if steer_required else 0, + "HANDS_ON_STEER_WARN": steer_required, + "HANDS_ON_STEER_WARN_2": steer_required, + + # TODO: right lane works, left doesn't + # TODO: need to do something about L/R + "LDW_WARN_LL": 0, + "LDW_WARN_RL": 0, + }) + return packer.make_can_msg("CAM_LANEINFO", 0, values) + + +def create_button_cmd(packer, car_fingerprint, counter, button): + + can = int(button == Buttons.CANCEL) + res = int(button == Buttons.RESUME) + + if car_fingerprint in GEN1: + values = { + "CAN_OFF": can, + "CAN_OFF_INV": (can + 1) % 2, + + "SET_P": 0, + "SET_P_INV": 1, + + "RES": res, + "RES_INV": (res + 1) % 2, + + "SET_M": 0, + "SET_M_INV": 1, + + "DISTANCE_LESS": 0, + "DISTANCE_LESS_INV": 1, + + "DISTANCE_MORE": 0, + "DISTANCE_MORE_INV": 1, + + "MODE_X": 0, + "MODE_X_INV": 1, + + "MODE_Y": 0, + "MODE_Y_INV": 1, + + "BIT1": 1, + "BIT2": 1, + "BIT3": 1, + "CTR": (counter + 1) % 16, + } + + return packer.make_can_msg("CRZ_BTNS", 0, values) diff --git a/selfdrive/car/mazda/radar_interface.py b/selfdrive/car/mazda/radar_interface.py new file mode 100755 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/mazda/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py new file mode 100644 index 00000000000000..9befad4d0b2c6c --- /dev/null +++ b/selfdrive/car/mazda/values.py @@ -0,0 +1,317 @@ +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu + + +# Steer torque limits + +class CarControllerParams: + STEER_MAX = 800 # theoretical max_steer 2047 + STEER_DELTA_UP = 10 # torque increase per refresh + STEER_DELTA_DOWN = 25 # torque decrease per refresh + STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting + STEER_DRIVER_MULTIPLIER = 1 # weight driver torque + STEER_DRIVER_FACTOR = 1 # from dbc + STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + + +class CAR: + CX5 = "MAZDA CX-5" + CX9 = "MAZDA CX-9" + MAZDA3 = "MAZDA 3" + MAZDA6 = "MAZDA 6" + CX9_2021 = "MAZDA CX-9 2021" + CX5_2022 = "MAZDA CX-5 2022" + + +@dataclass +class MazdaCarInfo(CarInfo): + package: str = "All" + harness: Enum = Harness.mazda + + +CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { + CAR.CX5: MazdaCarInfo("Mazda CX-5 2017-21"), + CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"), + CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), + CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), + CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-22", video_link="https://youtu.be/dA3duO4a0O4"), + CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022"), +} + + +class LKAS_LIMITS: + STEER_THRESHOLD = 15 + DISABLE_SPEED = 45 # kph + ENABLE_SPEED = 52 # kph + + +class Buttons: + NONE = 0 + SET_PLUS = 1 + SET_MINUS = 2 + RESUME = 3 + CANCEL = 4 + + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + ), + ], +) + +FW_VERSIONS = { + CAR.CX5_2022: { + (Ecu.eps, 0x730, None): [ + b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'SH54-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXFG-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.CX5: { + (Ecu.eps, 0x730, None): [ + b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KJ01-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KJ01-3210X-M-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PA53-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2F-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2H-188K2-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX68-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'SHKT-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KN0W-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'B61L-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX39-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB1-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.CX9 : { + (Ecu.eps, 0x730, None): [ + b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KJ01-3210X-L-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXN8-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYD6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.MAZDA3: { + (Ecu.eps, 0x730, None): [ + b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + + ], + (Ecu.engine, 0x7e0, None): [ + b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYJW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'B63C-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'B61L-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-Q\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PY2S-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'P52G-21PS1-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYKA-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYKE-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.MAZDA6: { + (Ecu.eps, 0x730, None): [ + b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GFBC-3210X-A-00\000\000\000\000\000\000\000\000\000', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX4F-188K2-D\000\000\000\000\000\000\000\000\000\000\000\000', + b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-E\000\000\000\000\000\000\000\000\000\000\000\000', + ], + (Ecu.abs, 0x760, None): [ + b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GDDM-437K2-A\000\000\000\000\000\000\000\000\000\000\000\000', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-P\000\000\000\000\000\000\000\000\000\000\000\000', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PYH3-21PS1-D\000\000\000\000\000\000\000\000\000\000\000\000', + b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.CX9_2021 : { + (Ecu.eps, 0x730, None): [ + b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + } +} + + +DBC = { + CAR.CX5: dbc_dict('mazda_2017', None), + CAR.CX9: dbc_dict('mazda_2017', None), + CAR.MAZDA3: dbc_dict('mazda_2017', None), + CAR.MAZDA6: dbc_dict('mazda_2017', None), + CAR.CX9_2021: dbc_dict('mazda_2017', None), + CAR.CX5_2022: dbc_dict('mazda_2017', None), +} + +# Gen 1 hardware: same CAN messages and same camera +GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} diff --git a/system/manager/__init__.py b/selfdrive/car/mock/__init__.py similarity index 100% rename from system/manager/__init__.py rename to selfdrive/car/mock/__init__.py diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py new file mode 100755 index 00000000000000..2c7f4611ee599a --- /dev/null +++ b/selfdrive/car/mock/interface.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 +import math +from cereal import car +from common.conversions import Conversions as CV +from system.swaglog import cloudlog +import cereal.messaging as messaging +from selfdrive.car import gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase + +# mocked car interface to work with chffrplus +TS = 0.01 # 100Hz +YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter +# low pass gain +LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS) + + +class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) + + cloudlog.debug("Using Mock Car Interface") + + self.sensor = messaging.sub_sock('sensorEvents') + self.gps = messaging.sub_sock('gpsLocationExternal') + + self.speed = 0. + self.prev_speed = 0. + self.yaw_rate = 0. + self.yaw_rate_meas = 0. + + @staticmethod + def compute_gb(accel, speed): + return accel + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "mock" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] + ret.mass = 1700. + ret.rotationalInertia = 2500. + ret.wheelbase = 2.70 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 13. # reasonable + ret.tireStiffnessFront = 1e6 # very stiff to neglect slip + ret.tireStiffnessRear = 1e6 # very stiff to neglect slip + + return ret + + # returns a car.CarState + def _update(self, c): + # get basic data from phone and gps since CAN isn't connected + sensors = messaging.recv_sock(self.sensor) + if sensors is not None: + for sensor in sensors.sensorEvents: + if sensor.type == 4: # gyro + self.yaw_rate_meas = -sensor.gyro.v[0] + + gps = messaging.recv_sock(self.gps) + if gps is not None: + self.prev_speed = self.speed + self.speed = gps.gpsLocationExternal.speed + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.speed + ret.vEgoRaw = self.speed + a = self.speed - self.prev_speed + + ret.aEgo = a + ret.brakePressed = a < -0.5 + + ret.standstill = self.speed < 0.01 + ret.wheelSpeeds.fl = self.speed + ret.wheelSpeeds.fr = self.speed + ret.wheelSpeeds.rl = self.speed + ret.wheelSpeeds.rr = self.speed + + self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate + curvature = self.yaw_rate / max(self.speed, 1.) + ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG + + return ret + + def apply(self, c): + # in mock no carcontrols + actuators = car.CarControl.Actuators.new_message() + return actuators, [] diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py new file mode 100755 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/mock/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py new file mode 100644 index 00000000000000..dfc7902e412ea5 --- /dev/null +++ b/selfdrive/car/mock/values.py @@ -0,0 +1,12 @@ +from typing import Dict, List, Optional, Union + +from selfdrive.car.docs_definitions import CarInfo + + +class CAR: + MOCK = 'mock' + + +CAR_INFO: Dict[str, Optional[Union[CarInfo, List[CarInfo]]]] = { + CAR.MOCK: None, +} diff --git a/system/manager/test/__init__.py b/selfdrive/car/nissan/__init__.py similarity index 100% rename from system/manager/test/__init__.py rename to selfdrive/car/nissan/__init__.py diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py new file mode 100644 index 00000000000000..dbc2b33c6ba6a8 --- /dev/null +++ b/selfdrive/car/nissan/carcontroller.py @@ -0,0 +1,89 @@ +from cereal import car +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.nissan import nissancan +from selfdrive.car.nissan.values import CAR, CarControllerParams + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.car_fingerprint = CP.carFingerprint + self.frame = 0 + + self.lkas_max_torque = 0 + self.last_angle = 0 + + self.packer = CANPacker(dbc_name) + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel + + can_sends = [] + + ### STEER ### + lkas_hud_msg = CS.lkas_hud_msg + lkas_hud_info_msg = CS.lkas_hud_info_msg + apply_angle = actuators.steeringAngleDeg + + steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 + + if CC.latActive: + # windup slower + if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) + else: + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) + + apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + + # Max torque from driver before EPS will give up and not apply torque + if not bool(CS.out.steeringPressed): + self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE + else: + # Scale max torque based on how much torque the driver is applying to the wheel + self.lkas_max_torque = max( + # Scale max torque down to half LKAX_MAX_TORQUE as a minimum + CarControllerParams.LKAS_MAX_TORQUE * 0.5, + # Start scaling torque at STEER_THRESHOLD + CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD) + ) + + else: + apply_angle = CS.out.steeringAngleDeg + self.lkas_max_torque = 0 + + self.last_angle = apply_angle + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: + can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) + + # TODO: Find better way to cancel! + # For some reason spamming the cancel button is unreliable on the Leaf + # We now cancel by making propilot think the seatbelt is unlatched, + # this generates a beep and a warning message every time you disengage + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0: + can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) + + can_sends.append(nissancan.create_steering_control( + self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) + + if lkas_hud_msg and lkas_hud_info_msg: + if self.frame % 2 == 0: + can_sends.append(nissancan.create_lkas_hud_msg( + self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + + if self.frame % 50 == 0: + can_sends.append(nissancan.create_lkas_hud_info_msg( + self.packer, lkas_hud_info_msg, steer_hud_alert + )) + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py new file mode 100644 index 00000000000000..a5ca2371103935 --- /dev/null +++ b/selfdrive/car/nissan/carstate.py @@ -0,0 +1,350 @@ +import copy +from collections import deque +from cereal import car +from opendbc.can.can_define import CANDefine +from selfdrive.car.interfaces import CarStateBase +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams + +TORQUE_SAMPLES = 12 + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + self.lkas_hud_msg = None + self.lkas_hud_info_msg = None + + self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) + self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] + + def update(self, cp, cp_adas, cp_cam): + ret = car.CarState.new_message() + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] + + ret.gasPressed = bool(ret.gas > 3) + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) + + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"], + cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"], + cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"], + cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 + + if self.CP.carFingerprint == CAR.ALTIMA: + ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + else: + ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 + ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + if self.CP.carFingerprint == CAR.LEAF: + ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 + elif self.CP.carFingerprint == CAR.LEAF_IC: + ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1 + ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) + elif self.CP.carFingerprint == CAR.ALTIMA: + ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 + ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"]) + + if self.CP.carFingerprint == CAR.ALTIMA: + speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"] + else: + speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] + + if speed != 255: + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS + else: + conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS + ret.cruiseState.speed = speed * conversion + ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus + + if self.CP.carFingerprint == CAR.ALTIMA: + ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + else: + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + + self.steeringTorqueSamples.append(ret.steeringTorque) + # Filtering driver torque to prevent steeringPressed false positives + ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD) + + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + + ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) + ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) + + ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]]) + + ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"]) + + can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + + if self.CP.carFingerprint == CAR.ALTIMA: + self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + else: + self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + + self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) + + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) + + if self.CP.carFingerprint != CAR.ALTIMA: + self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) + self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"), + + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), + + ("DOOR_OPEN_FR", "DOORS_LIGHTS"), + ("DOOR_OPEN_FL", "DOORS_LIGHTS"), + ("DOOR_OPEN_RR", "DOORS_LIGHTS"), + ("DOOR_OPEN_RL", "DOORS_LIGHTS"), + + ("RIGHT_BLINKER", "LIGHTS"), + ("LEFT_BLINKER", "LIGHTS"), + + ("ESP_DISABLED", "ESP"), + + ("GEAR_SHIFTER", "GEARBOX"), + ] + + checks = [ + # sig_address, frequency + ("STEER_ANGLE_SENSOR", 100), + ("WHEEL_SPEEDS_REAR", 50), + ("WHEEL_SPEEDS_FRONT", 50), + ("ESP", 25), + ("GEARBOX", 25), + ("DOORS_LIGHTS", 10), + ("LIGHTS", 10), + ] + + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + signals += [ + ("USER_BRAKE_PRESSED", "DOORS_LIGHTS"), + + ("GAS_PEDAL", "GAS_PEDAL"), + ("SEATBELT_DRIVER_LATCHED", "HUD"), + ("SPEED_MPH", "HUD"), + + ("PROPILOT_BUTTON", "CRUISE_THROTTLE"), + ("CANCEL_BUTTON", "CRUISE_THROTTLE"), + ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"), + ("SET_BUTTON", "CRUISE_THROTTLE"), + ("RES_BUTTON", "CRUISE_THROTTLE"), + ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"), + ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("NEW_SIGNAL_2", "CRUISE_THROTTLE"), + ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"), + ("unsure1", "CRUISE_THROTTLE"), + ("unsure2", "CRUISE_THROTTLE"), + ("unsure3", "CRUISE_THROTTLE"), + ] + + checks += [ + ("GAS_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ("HUD", 25), + ] + + elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + signals += [ + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("CRUISE_AVAILABLE", "CRUISE_THROTTLE"), + ("SPEED_MPH", "HUD_SETTINGS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT"), + + # Copy other values, we use this to cancel + ("CANCEL_SEATBELT", "CANCEL_MSG"), + ("NEW_SIGNAL_1", "CANCEL_MSG"), + ("NEW_SIGNAL_2", "CANCEL_MSG"), + ("NEW_SIGNAL_3", "CANCEL_MSG"), + ] + checks += [ + ("BRAKE_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ("CANCEL_MSG", 50), + ("HUD_SETTINGS", 25), + ("SEATBELT", 10), + ] + + if CP.carFingerprint == CAR.ALTIMA: + signals += [ + ("LKAS_ENABLED", "LKAS_SETTINGS"), + ("CRUISE_ENABLED", "CRUISE_STATE"), + ("SET_SPEED", "PROPILOT_HUD"), + ] + checks += [ + ("CRUISE_STATE", 10), + ("LKAS_SETTINGS", 10), + ("PROPILOT_HUD", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) + + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_adas_can_parser(CP): + # this function generates lists for signal, messages and initial values + + if CP.carFingerprint == CAR.ALTIMA: + signals = [ + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + ("CRUISE_ON", "PRO_PILOT"), + ] + checks = [ + ("LKAS", 100), + ("PRO_PILOT", 100), + ] + else: + signals = [ + # sig_name, sig_address + ("LKAS_ENABLED", "LKAS_SETTINGS"), + + ("CRUISE_ENABLED", "CRUISE_STATE"), + + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + # Below are the HUD messages. We copy the stock message and modify + ("LARGE_WARNING_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"), + ("LEAD_CAR", "PROPILOT_HUD"), + ("LEAD_CAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"), + ("LKAS_ERROR_FLASHING", "PROPILOT_HUD"), + ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"), + ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("FOLLOW_DISTANCE", "PROPILOT_HUD"), + ("AUDIBLE_TONE", "PROPILOT_HUD"), + ("SPEED_SET_ICON", "PROPILOT_HUD"), + ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"), + ("unknown59", "PROPILOT_HUD"), + ("unknown55", "PROPILOT_HUD"), + ("unknown26", "PROPILOT_HUD"), + ("unknown28", "PROPILOT_HUD"), + ("unknown31", "PROPILOT_HUD"), + ("SET_SPEED", "PROPILOT_HUD"), + ("unknown43", "PROPILOT_HUD"), + ("unknown08", "PROPILOT_HUD"), + ("unknown05", "PROPILOT_HUD"), + ("unknown02", "PROPILOT_HUD"), + + ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"), + ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"), + ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"), + ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"), + ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"), + ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"), + ("unknown07", "PROPILOT_HUD_INFO_MSG"), + ("unknown10", "PROPILOT_HUD_INFO_MSG"), + ("unknown15", "PROPILOT_HUD_INFO_MSG"), + ("unknown23", "PROPILOT_HUD_INFO_MSG"), + ("unknown19", "PROPILOT_HUD_INFO_MSG"), + ("unknown31", "PROPILOT_HUD_INFO_MSG"), + ("unknown32", "PROPILOT_HUD_INFO_MSG"), + ("unknown46", "PROPILOT_HUD_INFO_MSG"), + ("unknown61", "PROPILOT_HUD_INFO_MSG"), + ("unknown55", "PROPILOT_HUD_INFO_MSG"), + ("unknown50", "PROPILOT_HUD_INFO_MSG"), + ] + + checks = [ + ("PROPILOT_HUD_INFO_MSG", 2), + ("LKAS_SETTINGS", 10), + ("CRUISE_STATE", 50), + ("PROPILOT_HUD", 50), + ("LKAS", 100), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [] + + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + signals.append(("CRUISE_ON", "PRO_PILOT")) + checks.append(("PRO_PILOT", 100)) + elif CP.carFingerprint == CAR.ALTIMA: + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py new file mode 100644 index 00000000000000..e095ceb461d49e --- /dev/null +++ b/selfdrive/car/nissan/interface.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +from cereal import car +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.nissan.values import CAR + + +class CarInterface(CarInterfaceBase): + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "nissan" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] + ret.autoResumeSng = False + + ret.steerLimitTimer = 1.0 + + ret.steerActuatorDelay = 0.1 + ret.steerRatio = 17 + + if candidate in (CAR.ROGUE, CAR.XTRAIL): + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.705 + ret.centerToFront = ret.wheelbase * 0.44 + elif candidate in (CAR.LEAF, CAR.LEAF_IC): + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.705 + ret.centerToFront = ret.wheelbase * 0.44 + elif candidate == CAR.ALTIMA: + # Altima has EPS on C-CAN unlike the others that have it on V-CAN + ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus + ret.mass = 1492 + STD_CARGO_KG + ret.wheelbase = 2.824 + ret.centerToFront = ret.wheelbase * 0.44 + + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.radarOffCan = True + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) + + buttonEvents = [] + be = car.CarState.ButtonEvent.new_message() + be.type = car.CarState.ButtonEvent.Type.accelCruise + buttonEvents.append(be) + + events = self.create_common_events(ret) + + if self.CS.lkas_enabled: + events.add(car.CarEvent.EventName.invalidLkasSetting) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py new file mode 100644 index 00000000000000..01fb3463a98c67 --- /dev/null +++ b/selfdrive/car/nissan/nissancan.py @@ -0,0 +1,67 @@ +import copy +import crcmod +from selfdrive.car.nissan.values import CAR + +# TODO: add this checksum to the CANPacker +nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) + + +def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque): + values = { + "COUNTER": frame % 0x10, + "DESIRED_ANGLE": apply_steer, + "SET_0x80_2": 0x80, + "SET_0x80": 0x80, + "MAX_TORQUE": lkas_max_torque if steer_on else 0, + "LKA_ACTIVE": steer_on, + } + + dat = packer.make_can_msg("LKAS", 0, values)[2] + + values["CHECKSUM"] = nissan_checksum(dat[:7]) + return packer.make_can_msg("LKAS", 0, values) + + +def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg): + values = copy.copy(cruise_throttle_msg) + can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2 + + values["CANCEL_BUTTON"] = 1 + values["NO_BUTTON_PRESSED"] = 0 + values["PROPILOT_BUTTON"] = 0 + values["SET_BUTTON"] = 0 + values["RES_BUTTON"] = 0 + values["FOLLOW_DISTANCE_BUTTON"] = 0 + + return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values) + + +def create_cancel_msg(packer, cancel_msg, cruise_cancel): + values = copy.copy(cancel_msg) + + if cruise_cancel: + values["CANCEL_SEATBELT"] = 1 + + return packer.make_can_msg("CANCEL_MSG", 2, values) + + +def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): + values = lkas_hud_msg + + values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0 + values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0 + + values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0 + values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0 + values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0 + + return packer.make_can_msg("PROPILOT_HUD", 0, values) + + +def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert): + values = lkas_hud_info_msg + + if steer_hud_alert: + values["HANDS_ON_WHEEL_WARNING"] = 1 + + return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values) diff --git a/selfdrive/car/nissan/radar_interface.py b/selfdrive/car/nissan/radar_interface.py new file mode 100644 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/nissan/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py new file mode 100644 index 00000000000000..09bd7ca838e54c --- /dev/null +++ b/selfdrive/car/nissan/values.py @@ -0,0 +1,171 @@ +from dataclasses import dataclass +from typing import Dict, List, Optional, Union +from enum import Enum + +from cereal import car +from panda.python import uds +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower + STEER_THRESHOLD = 1.0 + + +class CAR: + XTRAIL = "NISSAN X-TRAIL 2017" + LEAF = "NISSAN LEAF 2018" + # Leaf with ADAS ECU found behind instrument cluster instead of glovebox + # Currently the only known difference between them is the inverted seatbelt signal. + LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" + ROGUE = "NISSAN ROGUE 2019" + ALTIMA = "NISSAN ALTIMA 2020" + + +@dataclass +class NissanCarInfo(CarInfo): + package: str = "ProPILOT Assist" + harness: Enum = Harness.nissan_a + + +CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { + CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), + CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"), + CAR.LEAF_IC: None, # same platforms + CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), + CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), +} + +FINGERPRINTS = { + CAR.XTRAIL: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8 + }, + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8 + }, + ], + CAR.LEAF: [ + { + 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8 + }, + # 2020 Leaf SV Plus + { + 2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 + }, + ], + CAR.LEAF_IC: [ + { + 2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8 + }, + ], + CAR.ROGUE: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + ], + CAR.ALTIMA: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + ] +} + +NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xc0]) +NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xc0]) + +NISSAN_VERSION_REQUEST_KWP = b'\x21\x83' +NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83' + +NISSAN_RX_OFFSET = 0x20 + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + ), + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + rx_offset=NISSAN_RX_OFFSET, + ), + Request( + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + rx_offset=NISSAN_RX_OFFSET, + ), + ], +) + +FW_VERSIONS = { + CAR.ALTIMA: { + (Ecu.fwdCamera, 0x707, None): [ + b'284N86CA1D', + ], + (Ecu.eps, 0x742, None): [ + b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80', + ], + (Ecu.engine, 0x7e0, None): [ + b'237109HE2B', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U29HE0A', + ], + }, + CAR.LEAF_IC: { + (Ecu.fwdCamera, 0x707, None): [ + b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', + b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80', + ], + (Ecu.abs, 0x740, None): [ + b'476605SH1D', + b'476605SK2A', + ], + (Ecu.eps, 0x742, None): [ + b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80', + b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U25SH3A', + b'284U25SK2D', + ], + }, + CAR.XTRAIL: { + (Ecu.fwdCamera, 0x707, None): [ + b'284N86FR2A', + ], + (Ecu.abs, 0x740, None): [ + b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80', + b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80', + ], + (Ecu.eps, 0x742, None): [ + b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.combinationMeter, 0x743, None): [ + b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.engine, 0x7e0, None): [ + b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U26FR0E', + ], + }, +} + +DBC = { + CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None), + CAR.LEAF: dbc_dict('nissan_leaf_2018', None), + CAR.LEAF_IC: dbc_dict('nissan_leaf_2018', None), + CAR.ROGUE: dbc_dict('nissan_x_trail_2017', None), + CAR.ALTIMA: dbc_dict('nissan_x_trail_2017', None), +} diff --git a/system/sensord/__init__.py b/selfdrive/car/subaru/__init__.py similarity index 100% rename from system/sensord/__init__.py rename to selfdrive/car/subaru/__init__.py diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py new file mode 100644 index 00000000000000..b5429daef221d3 --- /dev/null +++ b/selfdrive/car/subaru/carcontroller.py @@ -0,0 +1,92 @@ +from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.subaru import subarucan +from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.apply_steer_last = 0 + self.frame = 0 + + self.es_lkas_cnt = -1 + self.es_distance_cnt = -1 + self.es_dashstatus_cnt = -1 + self.cruise_button_prev = 0 + self.last_cancel_frame = 0 + + self.p = CarControllerParams(CP) + self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel + + can_sends = [] + + # *** steering *** + if (self.frame % self.p.STEER_STEP) == 0: + + apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) + + # limits due to driver torque + + new_steer = int(round(apply_steer)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) + + if not CC.latActive: + apply_steer = 0 + + if self.CP.carFingerprint in PREGLOBAL_CARS: + can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer)) + else: + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer)) + + self.apply_steer_last = apply_steer + + + # *** alerts and pcm cancel *** + + if self.CP.carFingerprint in PREGLOBAL_CARS: + if self.es_distance_cnt != CS.es_distance_msg["COUNTER"]: + # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep + # disengage ACC when OP is disengaged + if pcm_cancel_cmd: + cruise_button = 1 + # turn main on if off and past start-up state + elif not CS.out.cruiseState.available and CS.ready: + cruise_button = 1 + else: + cruise_button = CS.cruise_button + + # unstick previous mocked button press + if cruise_button == 1 and self.cruise_button_prev == 1: + cruise_button = 0 + self.cruise_button_prev = cruise_button + + can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) + self.es_distance_cnt = CS.es_distance_msg["COUNTER"] + + else: + if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2: + bus = 1 if self.CP.carFingerprint in GLOBAL_GEN2 else 0 + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd)) + self.last_cancel_frame = self.frame + + if self.es_dashstatus_cnt != CS.es_dashstatus_msg["COUNTER"]: + can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg)) + self.es_dashstatus_cnt = CS.es_dashstatus_msg["COUNTER"] + + if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]: + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] + + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py new file mode 100644 index 00000000000000..7fc5456d98f92f --- /dev/null +++ b/selfdrive/car/subaru/carstate.py @@ -0,0 +1,316 @@ +import copy +from cereal import car +from opendbc.can.can_define import CANDefine +from common.conversions import Conversions as CV +from selfdrive.car.interfaces import CarStateBase +from opendbc.can.parser import CANParser +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.shifter_values = can_define.dv["Transmission"]["Gear"] + + def update(self, cp, cp_cam, cp_body): + ret = car.CarState.new_message() + + ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255. + ret.gasPressed = ret.gas > 1e-5 + if self.car_fingerprint in PREGLOBAL_CARS: + ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2 + else: + cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 + + cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + ret.wheelSpeeds = self.get_wheel_speeds( + cp_wheels.vl["Wheel_Speeds"]["FL"], + cp_wheels.vl["Wheel_Speeds"]["FR"], + cp_wheels.vl["Wheel_Speeds"]["RL"], + cp_wheels.vl["Wheel_Speeds"]["RR"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 + + # continuous blinker signals for assisted lane change + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"], + cp.vl["Dashlights"]["RIGHT_BLINKER"]) + + if self.CP.enableBsm: + ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) + ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1) + + can_gear = int(cp.vl["Transmission"]["Gear"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + + ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] + ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"] + ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"] + + steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 + ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold + + cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 + ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 + ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS + + if self.car_fingerprint not in PREGLOBAL_CARS: + ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 + + if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ + (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): + ret.cruiseState.speed *= CV.MPH_TO_KPH + + ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1 + ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"], + cp.vl["BodyInfo"]["DOOR_OPEN_RL"], + cp.vl["BodyInfo"]["DOOR_OPEN_FR"], + cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) + ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 + + if self.car_fingerprint in PREGLOBAL_CARS: + self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] + self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] + else: + ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 + ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 + self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + + cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) + self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) + + return ret + + @staticmethod + def get_common_global_signals(): + signals = [ + ("Cruise_On", "CruiseControl"), + ("Cruise_Activated", "CruiseControl"), + ("FL", "Wheel_Speeds"), + ("FR", "Wheel_Speeds"), + ("RL", "Wheel_Speeds"), + ("RR", "Wheel_Speeds"), + ("Brake", "Brake_Status"), + ] + checks = [ + ("CruiseControl", 20), + ("Wheel_Speeds", 50), + ("Brake_Status", 50), + ] + + return signals, checks + + @staticmethod + def get_global_es_distance_signals(): + signals = [ + ("COUNTER", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Cruise_Throttle", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Cruise_Soft_Disable", "ES_Distance"), + ("Signal7", "ES_Distance"), + ("Cruise_Brake_Active", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Cruise_EPB", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("Cruise_Cancel", "ES_Distance"), + ("Cruise_Set", "ES_Distance"), + ("Cruise_Resume", "ES_Distance"), + ("Signal6", "ES_Distance"), + ] + checks = [ + ("ES_Distance", 20), + ] + + return signals, checks + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("Steer_Torque_Sensor", "Steering_Torque"), + ("Steer_Torque_Output", "Steering_Torque"), + ("Steering_Angle", "Steering_Torque"), + ("Steer_Error_1", "Steering_Torque"), + ("Brake_Pedal", "Brake_Pedal"), + ("Throttle_Pedal", "Throttle"), + ("LEFT_BLINKER", "Dashlights"), + ("RIGHT_BLINKER", "Dashlights"), + ("SEATBELT_FL", "Dashlights"), + ("DOOR_OPEN_FR", "BodyInfo"), + ("DOOR_OPEN_FL", "BodyInfo"), + ("DOOR_OPEN_RR", "BodyInfo"), + ("DOOR_OPEN_RL", "BodyInfo"), + ("Gear", "Transmission"), + ] + + checks = [ + # sig_address, frequency + ("Throttle", 100), + ("Dashlights", 10), + ("Brake_Pedal", 50), + ("Transmission", 100), + ("Steering_Torque", 50), + ("BodyInfo", 1), + ] + + if CP.enableBsm: + signals += [ + ("L_ADJACENT", "BSD_RCTA"), + ("R_ADJACENT", "BSD_RCTA"), + ("L_APPROACHING", "BSD_RCTA"), + ("R_APPROACHING", "BSD_RCTA"), + ] + checks.append(("BSD_RCTA", 17)) + + if CP.carFingerprint not in PREGLOBAL_CARS: + if CP.carFingerprint not in GLOBAL_GEN2: + signals += CarState.get_common_global_signals()[0] + checks += CarState.get_common_global_signals()[1] + + signals += [ + ("Steer_Warning", "Steering_Torque"), + ("UNITS", "Dashlights"), + ] + + checks += [ + ("Dashlights", 10), + ("BodyInfo", 10), + ] + else: + signals += [ + ("FL", "Wheel_Speeds"), + ("FR", "Wheel_Speeds"), + ("RL", "Wheel_Speeds"), + ("RR", "Wheel_Speeds"), + ("UNITS", "Dash_State2"), + ("Cruise_On", "CruiseControl"), + ("Cruise_Activated", "CruiseControl"), + ] + checks += [ + ("Wheel_Speeds", 50), + ("Dash_State2", 1), + ] + + if CP.carFingerprint == CAR.FORESTER_PREGLOBAL: + checks += [ + ("Dashlights", 20), + ("BodyInfo", 1), + ("CruiseControl", 50), + ] + + if CP.carFingerprint in (CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): + checks += [ + ("Dashlights", 10), + ("CruiseControl", 50), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + if CP.carFingerprint in PREGLOBAL_CARS: + signals = [ + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Not_Ready_Startup", "ES_DashStatus"), + + ("Cruise_Throttle", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Brake_On", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Standstill", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Standstill_2", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("COUNTER", "ES_Distance"), + ("Signal6", "ES_Distance"), + ("Cruise_Button", "ES_Distance"), + ("Signal7", "ES_Distance"), + ] + + checks = [ + ("ES_DashStatus", 20), + ("ES_Distance", 20), + ] + else: + signals = [ + ("Counter", "ES_DashStatus"), + ("PCB_Off", "ES_DashStatus"), + ("LDW_Off", "ES_DashStatus"), + ("Signal1", "ES_DashStatus"), + ("Cruise_State_Msg", "ES_DashStatus"), + ("LKAS_State_Msg", "ES_DashStatus"), + ("Signal2", "ES_DashStatus"), + ("Cruise_Soft_Disable", "ES_DashStatus"), + ("EyeSight_Status_Msg", "ES_DashStatus"), + ("Signal3", "ES_DashStatus"), + ("Cruise_Distance", "ES_DashStatus"), + ("Signal4", "ES_DashStatus"), + ("Conventional_Cruise", "ES_DashStatus"), + ("Signal5", "ES_DashStatus"), + ("Cruise_Disengaged", "ES_DashStatus"), + ("Cruise_Activated", "ES_DashStatus"), + ("Signal6", "ES_DashStatus"), + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Cruise_Fault", "ES_DashStatus"), + ("Cruise_On", "ES_DashStatus"), + ("Display_Own_Car", "ES_DashStatus"), + ("Brake_Lights", "ES_DashStatus"), + ("Car_Follow", "ES_DashStatus"), + ("Signal7", "ES_DashStatus"), + ("Far_Distance", "ES_DashStatus"), + ("Cruise_State", "ES_DashStatus"), + + ("COUNTER", "ES_LKAS_State"), + ("LKAS_Alert_Msg", "ES_LKAS_State"), + ("Signal1", "ES_LKAS_State"), + ("LKAS_ACTIVE", "ES_LKAS_State"), + ("LKAS_Dash_State", "ES_LKAS_State"), + ("Signal2", "ES_LKAS_State"), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State"), + ("LKAS_Left_Line_Enable", "ES_LKAS_State"), + ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Right_Line_Enable", "ES_LKAS_State"), + ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Left_Line_Visible", "ES_LKAS_State"), + ("LKAS_Right_Line_Visible", "ES_LKAS_State"), + ("LKAS_Alert", "ES_LKAS_State"), + ("Signal3", "ES_LKAS_State"), + ] + + checks = [ + ("ES_DashStatus", 10), + ("ES_LKAS_State", 10), + ] + + if CP.carFingerprint not in GLOBAL_GEN2: + signals += CarState.get_global_es_distance_signals()[0] + checks += CarState.get_global_es_distance_signals()[1] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_body_can_parser(CP): + if CP.carFingerprint in GLOBAL_GEN2: + signals, checks = CarState.get_common_global_signals() + signals += CarState.get_global_es_distance_signals()[0] + checks += CarState.get_global_es_distance_signals()[1] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) + + return None \ No newline at end of file diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py new file mode 100644 index 00000000000000..a920c0253456f4 --- /dev/null +++ b/selfdrive/car/subaru/interface.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS + + +class CarInterface(CarInterfaceBase): + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "subaru" + ret.radarOffCan = True + ret.dashcamOnly = candidate in PREGLOBAL_CARS + ret.autoResumeSng = False + + if candidate in PREGLOBAL_CARS: + ret.enableBsm = 0x25c in fingerprint[0] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)] + else: + ret.enableBsm = 0x228 in fingerprint[0] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] + if candidate in GLOBAL_GEN2: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 + + ret.steerLimitTimer = 0.4 + ret.steerActuatorDelay = 0.1 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + if candidate == CAR.ASCENT: + ret.mass = 2031. + STD_CARGO_KG + ret.wheelbase = 2.89 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 13.5 + ret.steerActuatorDelay = 0.3 # end-to-end angle controller + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kf = 0.00003 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]] + + elif candidate == CAR.IMPREZA: + ret.mass = 1568. + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 15 + ret.steerActuatorDelay = 0.4 # end-to-end angle controller + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] + + elif candidate == CAR.IMPREZA_2020: + ret.mass = 1480. + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 17 # learned, 14 stock + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] + + elif candidate == CAR.FORESTER: + ret.mass = 1568. + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 17 # learned, 14 stock + ret.lateralTuning.init('pid') + ret.lateralTuning.pid.kf = 0.000038 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] + + elif candidate in (CAR.OUTBACK, CAR.LEGACY): + ret.mass = 1568. + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 17 + ret.steerActuatorDelay = 0.1 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): + ret.safetyConfigs[0].safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal + ret.mass = 1568 + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 20 # learned, 14 stock + + elif candidate == CAR.LEGACY_PREGLOBAL: + ret.mass = 1568 + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 12.5 # 14.5 stock + ret.steerActuatorDelay = 0.15 + + elif candidate == CAR.OUTBACK_PREGLOBAL: + ret.mass = 1568 + STD_CARGO_KG + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 20 # learned, 14 stock + + else: + raise ValueError(f"unknown car: {candidate}") + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + # returns a car.CarState + def _update(self, c): + + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + + ret.events = self.create_common_events(ret).to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py new file mode 100644 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/subaru/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py new file mode 100644 index 00000000000000..d83b639a41fb0a --- /dev/null +++ b/selfdrive/car/subaru/subarucan.py @@ -0,0 +1,101 @@ +import copy +from cereal import car + +VisualAlert = car.CarControl.HUDControl.VisualAlert + +def create_steering_control(packer, apply_steer): + values = { + "LKAS_Output": apply_steer, + "LKAS_Request": 1 if apply_steer != 0 else 0, + "SET_1": 1 + } + return packer.make_can_msg("ES_LKAS", 0, values) + +def create_steering_status(packer): + return packer.make_can_msg("ES_LKAS_State", 0, {}) + +def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): + values = copy.copy(es_distance_msg) + values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 + if pcm_cancel_cmd: + values["Cruise_Cancel"] = 1 + return packer.make_can_msg("ES_Distance", bus, values) + +def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + + values = copy.copy(es_lkas_msg) + + # Filter the stock LKAS "Keep hands on wheel" alert + if values["LKAS_Alert_Msg"] == 1: + values["LKAS_Alert_Msg"] = 0 + + # Filter the stock LKAS sending an audible alert when it turns off LKAS + if values["LKAS_Alert"] == 27: + values["LKAS_Alert"] = 0 + + # Filter the stock LKAS sending an audible alert when "Keep hands on wheel" alert is active (2020+ models) + if values["LKAS_Alert"] == 28 and values["LKAS_Alert_Msg"] == 7: + values["LKAS_Alert"] = 0 + + # Filter the stock LKAS sending an audible alert when "Keep hands on wheel OFF" alert is active (2020+ models) + if values["LKAS_Alert"] == 30: + values["LKAS_Alert"] = 0 + + # Filter the stock LKAS sending "Keep hands on wheel OFF" alert (2020+ models) + if values["LKAS_Alert_Msg"] == 7: + values["LKAS_Alert_Msg"] = 0 + + # Show Keep hands on wheel alert for openpilot steerRequired alert + if visual_alert == VisualAlert.steerRequired: + values["LKAS_Alert_Msg"] = 1 + + # Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW) + if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0: + if left_lane_depart: + values["LKAS_Alert"] = 12 # Left lane departure dash alert + elif right_lane_depart: + values["LKAS_Alert"] = 11 # Right lane departure dash alert + + if enabled: + values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines + values["LKAS_Dash_State"] = 2 # Green enabled indicator + else: + values["LKAS_Dash_State"] = 0 # LKAS Not enabled + + values["LKAS_Left_Line_Visible"] = int(left_line) + values["LKAS_Right_Line_Visible"] = int(right_line) + + return packer.make_can_msg("ES_LKAS_State", 0, values) + +def create_es_dashstatus(packer, dashstatus_msg): + values = copy.copy(dashstatus_msg) + + # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts + if values["LKAS_State_Msg"] in [2, 3]: + values["LKAS_State_Msg"] = 0 + + return packer.make_can_msg("ES_DashStatus", 0, values) + +# *** Subaru Pre-global *** + +def subaru_preglobal_checksum(packer, values, addr): + dat = packer.make_can_msg(addr, 0, values)[2] + return (sum(dat[:7])) % 256 + +def create_preglobal_steering_control(packer, apply_steer): + values = { + "LKAS_Command": apply_steer, + "LKAS_Active": 1 if apply_steer != 0 else 0 + } + values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS") + + return packer.make_can_msg("ES_LKAS", 0, values) + +def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): + + values = copy.copy(es_distance_msg) + values["Cruise_Button"] = cruise_button + + values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance") + + return packer.make_can_msg("ES_Distance", 0, values) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py new file mode 100644 index 00000000000000..58fe111fbdab88 --- /dev/null +++ b/selfdrive/car/subaru/values.py @@ -0,0 +1,504 @@ +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from panda.python import uds +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 + +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + def __init__(self, CP): + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max + self.STEER_DELTA_DOWN = 70 # torque decrease per refresh + self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily + self.STEER_DRIVER_FACTOR = 1 # from dbc + + if CP.carFingerprint in GLOBAL_GEN2: + self.STEER_MAX = 1000 + self.STEER_DELTA_UP = 40 + self.STEER_DELTA_DOWN = 40 + elif CP.carFingerprint == CAR.IMPREZA_2020: + self.STEER_MAX = 1439 + else: + self.STEER_MAX = 2047 + + +class CAR: + # Global platform + ASCENT = "SUBARU ASCENT LIMITED 2019" + IMPREZA = "SUBARU IMPREZA LIMITED 2019" + IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020" + FORESTER = "SUBARU FORESTER 2019" + OUTBACK = "SUBARU OUTBACK 6TH GEN" + LEGACY = "SUBARU LEGACY 7TH GEN" + + # Pre-global + FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018" + LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018" + OUTBACK_PREGLOBAL = "SUBARU OUTBACK 2015 - 2017" + OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019" + + +@dataclass +class SubaruCarInfo(CarInfo): + package: str = "EyeSight Driver Assistance" + harness: Enum = Harness.subaru_a + + +CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { + CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"), + CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness=Harness.subaru_b), + CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness=Harness.subaru_b), + CAR.IMPREZA: [ + SubaruCarInfo("Subaru Impreza 2017-19"), + SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + SubaruCarInfo("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + ], + CAR.IMPREZA_2020: [ + SubaruCarInfo("Subaru Impreza 2020-22"), + SubaruCarInfo("Subaru Crosstrek 2020-21"), + SubaruCarInfo("Subaru XV 2020-21"), + ], + CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), + CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), + CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), + CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), + CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"), +} + +SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + ), + ], +) + +FW_VERSIONS = { + CAR.ASCENT: { + (Ecu.abs, 0x7b0, None): [ + b'\xa5 \x19\x02\x00', + b'\xa5 !\002\000', + b'\xf1\x82\xa5 \x19\x02\x00', + ], + (Ecu.eps, 0x746, None): [ + b'\x85\xc0\xd0\x00', + b'\005\xc0\xd0\000', + b'\x95\xc0\xd0\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00d\xb9\x1f@ \x10', + b'\000\000e~\037@ \'', + b'\x00\x00e@\x1f@ $', + b'\x00\x00d\xb9\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xbb,\xa0t\a', + b'\xf1\x82\xbb,\xa0t\x87', + b'\xf1\x82\xbb,\xa0t\a', + b'\xf1\x82\xd9,\xa0@\a', + b'\xf1\x82\xd1,\xa0q\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\x00\xfe\xf7\x00\x00', + b'\001\xfe\xf9\000\000', + b'\x01\xfe\xf7\x00\x00', + ], + }, + CAR.LEGACY: { + (Ecu.abs, 0x7b0, None): [ + b'\xa1\\ x04\x01', + b'\xa1 \x03\x03' + ], + (Ecu.eps, 0x746, None): [ + b'\x9b\xc0\x11\x00', + b'\x9b\xc0\x11\x02' + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00e\x80\x00\x1f@ \x19\x00', + b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00' + ], + (Ecu.engine, 0x7e0, None): [ + b'\xde\"a0\x07', + b'\xe2"aq\x07' + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xa5\xf6\x05@\x00', + b'\xa7\xf6\x04@\x00' + ], + }, + CAR.IMPREZA: { + (Ecu.abs, 0x7b0, None): [ + b'\x7a\x94\x3f\x90\x00', + b'\xa2 \x185\x00', + b'\xa2 \x193\x00', + b'\xa2 \x194\x00', + b'z\x94.\x90\x00', + b'z\x94\b\x90\x01', + b'\xa2 \x19`\x00', + b'z\x94\f\x90\001', + b'z\x9c\x19\x80\x01', + b'z\x94\x08\x90\x00', + b'z\x84\x19\x90\x00', + b'\xf1\x00\xb2\x06\x04', + b'z\x94\x0c\x90\x00', + ], + (Ecu.eps, 0x746, None): [ + b'\x7a\xc0\x0c\x00', + b'z\xc0\x08\x00', + b'\x8a\xc0\x00\x00', + b'z\xc0\x04\x00', + b'z\xc0\x00\x00', + b'\x8a\xc0\x10\x00', + b'z\xc0\n\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00\x64\xb5\x1f\x40\x20\x0e', + b'\x00\x00d\xdc\x1f@ \x0e', + b'\x00\x00e\x1c\x1f@ \x14', + b'\x00\x00d)\x1f@ \a', + b'\x00\x00e+\x1f@ \x14', + b'\000\000e+\000\000\000\000', + b'\000\000dd\037@ \016', + b'\000\000e\002\037@ \024', + b'\x00\x00d)\x00\x00\x00\x00', + b'\x00\x00c\xf4\x00\x00\x00\x00', + b'\x00\x00d\xdc\x00\x00\x00\x00', + b'\x00\x00dd\x00\x00\x00\x00', + b'\x00\x00c\xf4\x1f@ \x07', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xaa\x61\x66\x73\x07', + b'\xbeacr\a', + b'\xc5!`r\a', + b'\xaa!ds\a', + b'\xaa!`u\a', + b'\xaa!dq\a', + b'\xaa!dt\a', + b'\xc5!ar\a', + b'\xbe!as\a', + b'\xc5!ds\a', + b'\xc5!`s\a', + b'\xaa!au\a', + b'\xbe!at\a', + b'\xaa\x00Bu\x07', + b'\xc5!dr\x07', + b'\xaa!aw\x07', + b'\xaa!av\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xe3\xe5\x46\x31\x00', + b'\xe4\xe5\x061\x00', + b'\xe5\xf5\x04\x00\x00', + b'\xe3\xf5G\x00\x00', + b'\xe3\xf5\a\x00\x00', + b'\xe3\xf5C\x00\x00', + b'\xe5\xf5B\x00\x00', + b'\xe5\xf5$\000\000', + b'\xe4\xf5\a\000\000', + b'\xe3\xf5F\000\000', + b'\xe4\xf5\002\000\000', + b'\xe3\xd0\x081\x00', + b'\xe3\xf5\x06\x00\x00', + ], + }, + CAR.IMPREZA_2020: { + (Ecu.abs, 0x7b0, None): [ + b'\xa2 \0314\000', + b'\xa2 \0313\000', + b'\xa2 !i\000', + b'\xa2 !`\000', + b'\xf1\x00\xb2\x06\x04', + b'\xa2 `\x00', + ], + (Ecu.eps, 0x746, None): [ + b'\x9a\xc0\000\000', + b'\n\xc0\004\000', + b'\x9a\xc0\x04\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\000\000eb\037@ \"', + b'\000\000e\x8f\037@ )', + b'\x00\x00eq\x1f@ "', + b'\x00\x00eq\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xca!ap\a', + b'\xca!`p\a', + b'\xca!`0\a', + b'\xcc\"f0\a', + b'\xcc!fp\a', + b'\xca!f@\x07', + b'\xca!fp\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xe6\xf5\004\000\000', + b'\xe6\xf5$\000\000', + b'\xe7\xf6B0\000', + b'\xe7\xf5D0\000', + b'\xf1\x00\xd7\x10@', + b'\xe6\xf5D0\x00', + ], + }, + CAR.FORESTER: { + (Ecu.abs, 0x7b0, None): [ + b'\xa3 \x18\x14\x00', + b'\xa3 \024\000', + b'\xa3 \031\024\000', + b'\xa3 \x14\x01', + b'\xf1\x00\xbb\r\x05', + ], + (Ecu.eps, 0x746, None): [ + b'\x8d\xc0\x04\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00e!\x1f@ \x11', + b'\x00\x00e\x97\x1f@ 0', + b'\000\000e`\037@ ', + b'\xf1\x00\xac\x02\x00', + b'\x00\x00e!\x00\x00\x00\x00', + b'\x00\x00e\x97\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xb6"`A\x07', + b'\xcf"`0\x07', + b'\xcb\"`@\a', + b'\xcb\"`p\a', + b'\xf1\x00\xa2\x10\n', + b'\xcf"`p\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\032\xf6B0\000', + b'\x1a\xf6F`\x00', + b'\032\xf6b`\000', + b'\x1a\xf6B`\x00', + b'\x1a\xf6b0\x00', + ], + }, + CAR.FORESTER_PREGLOBAL: { + (Ecu.abs, 0x7b0, None): [ + b'\x7d\x97\x14\x40', + b'\xf1\x00\xbb\x0c\x04', + ], + (Ecu.eps, 0x746, None): [ + b'}\xc0\x10\x00', + b'm\xc0\x10\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00\x64\x35\x1f\x40\x20\x09', + b'\x00\x00c\xe9\x1f@ \x03', + b'\x00\x00d\xd3\x1f@ \t' + ], + (Ecu.engine, 0x7e0, None): [ + b'\xba"@p\a', + b'\xa7)\xa0q\a', + b'\xf1\x82\xa7)\xa0q\a', + b'\xba"@@\a', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xdc\xf2\x60\x60\x00', + b'\xdc\xf2@`\x00', + b'\xda\xfd\xe0\x80\x00', + b'\xdc\xf2`\x81\000', + b'\xdc\xf2`\x80\x00', + b'\x1a\xf6F`\x00', + ], + }, + CAR.LEGACY_PREGLOBAL: { + (Ecu.abs, 0x7b0, None): [ + b'k\x97D\x00', + b'[\xba\xc4\x03', + b'{\x97D\x00', + b'[\x97D\000', + ], + (Ecu.eps, 0x746, None): [ + b'[\xb0\x00\x01', + b'K\xb0\x00\x01', + b'k\xb0\x00\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00c\xb7\x1f@\x10\x16', + b'\x00\x00c\x94\x1f@\x10\x08', + b'\x00\x00c\xec\x1f@ \x04', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xab*@r\a', + b'\xa0+@p\x07', + b'\xb4"@0\x07', + b'\xa0"@q\a', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xbe\xf2\x00p\x00', + b'\xbf\xfb\xc0\x80\x00', + b'\xbd\xf2\x00`\x00', + b'\xbf\xf2\000\x80\000', + ], + }, + CAR.OUTBACK_PREGLOBAL: { + (Ecu.abs, 0x7b0, None): [ + b'{\x9a\xac\x00', + b'k\x97\xac\x00', + b'\x5b\xf7\xbc\x03', + b'[\xf7\xac\x03', + b'{\x97\xac\x00', + b'k\x9a\xac\000', + b'[\xba\xac\x03', + b'[\xf7\xac\000', + ], + (Ecu.eps, 0x746, None): [ + b'k\xb0\x00\x00', + b'[\xb0\x00\x00', + b'\x4b\xb0\x00\x02', + b'K\xb0\x00\x00', + b'{\xb0\x00\x01', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00c\xec\x1f@ \x04', + b'\x00\x00c\xd1\x1f@\x10\x17', + b'\xf1\x00\xf0\xe0\x0e', + b'\x00\x00c\x94\x00\x00\x00\x00', + b'\x00\x00c\x94\x1f@\x10\b', + b'\x00\x00c\xb7\x1f@\x10\x16', + b'\000\000c\x90\037@\020\016', + b'\x00\x00c\xec\x37@\x04', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xb4+@p\a', + b'\xab\"@@\a', + b'\xa0\x62\x41\x71\x07', + b'\xa0*@q\a', + b'\xab*@@\a', + b'\xb4"@0\a', + b'\xb4"@p\a', + b'\xab"@s\a', + b'\xab+@@\a', + b'\xb4"@r\a', + b'\xa0+@@\x07', + b'\xa0\"@\x80\a', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xbd\xfb\xe0\x80\x00', + b'\xbe\xf2@\x80\x00', + b'\xbf\xe2\x40\x80\x00', + b'\xbf\xf2@\x80\x00', + b'\xbe\xf2@p\x00', + b'\xbd\xf2@`\x00', + b'\xbd\xf2@\x81\000', + b'\xbe\xfb\xe0p\000', + b'\xbf\xfb\xe0b\x00', + ], + }, + CAR.OUTBACK_PREGLOBAL_2018: { + (Ecu.abs, 0x7b0, None): [ + b'\x8b\x97\xac\x00', + b'\x8b\x9a\xac\x00', + b'\x9b\x97\xac\x00', + b'\x8b\x97\xbc\x00', + b'\x8b\x99\xac\x00', + b'\x9b\x9a\xac\000', + b'\x9b\x97\xbe\x10', + ], + (Ecu.eps, 0x746, None): [ + b'{\xb0\x00\x00', + b'{\xb0\x00\x01', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00df\x1f@ \n', + b'\x00\x00d\xfe\x1f@ \x15', + b'\x00\x00d\x95\x00\x00\x00\x00', + b'\x00\x00d\x95\x1f@ \x0f', + b'\x00\x00d\xfe\x00\x00\x00\x00', + b'\x00\x00e\x19\x1f@ \x15', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xb5"@p\a', + b'\xb5+@@\a', + b'\xb5"@P\a', + b'\xc4"@0\a', + b'\xb5b@1\x07', + b'\xb5q\xe0@\a', + b'\xc4+@0\a', + b'\xc4b@p\a', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xbc\xf2@\x81\x00', + b'\xbc\xfb\xe0\x80\x00', + b'\xbc\xf2@\x80\x00', + b'\xbb\xf2@`\x00', + b'\xbc\xe2@\x80\x00', + b'\xbc\xfb\xe0`\x00', + b'\xbc\xaf\xe0`\x00', + b'\xbb\xfb\xe0`\000', + ], + }, + CAR.OUTBACK: { + (Ecu.abs, 0x7b0, None): [ + b'\xa1 \x06\x01', + b'\xa1 \a\x00', + b'\xa1 \b\001', + b'\xa1 \x06\x00', + b'\xa1 "\t\x01', + b'\xa1 \x08\x02', + b'\xa1 \x06\x02', + b'\xa1 \x08\x00', + ], + (Ecu.eps, 0x746, None): [ + b'\x9b\xc0\x10\x00', + b'\x9b\xc0\x20\x00', + b'\x1b\xc0\x10\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00eJ\x00\x1f@ \x19\x00', + b'\000\000e\x80\000\037@ \031\000', + b'\x00\x00e\x9a\x00\x1f@ 1\x00', + b'\x00\x00eJ\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xbc,\xa0q\x07', + b'\xbc\"`@\a', + b'\xde"`0\a', + b'\xf1\x82\xbc,\xa0q\a', + b'\xf1\x82\xe3,\xa0@\x07', + b'\xe2"`p\x07', + b'\xf1\x82\xe2,\xa0@\x07', + b'\xbc"`q\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xa5\xfe\xf7@\x00', + b'\xa5\xf6D@\x00', + b'\xa5\xfe\xf6@\x00', + b'\xa7\x8e\xf40\x00', + b'\xf1\x82\xa7\xf6D@\x00', + b'\xa7\xfe\xf4@\x00', + ], + }, +} + +DBC = { + CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None), + CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), + CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None), + CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), + CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), + CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), + CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), + CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), + CAR.OUTBACK_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), + CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), +} + +GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY) +PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018) diff --git a/system/sensord/sensors/__init__.py b/selfdrive/car/tesla/__init__.py similarity index 100% rename from system/sensord/sensors/__init__.py rename to selfdrive/car/tesla/__init__.py diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py new file mode 100644 index 00000000000000..cf43b8ef00bfe3 --- /dev/null +++ b/selfdrive/car/tesla/carcontroller.py @@ -0,0 +1,69 @@ +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.tesla.teslacan import TeslaCAN +from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.frame = 0 + self.last_angle = 0 + self.packer = CANPacker(dbc_name) + self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) + self.tesla_can = TeslaCAN(self.packer, self.pt_packer) + + def update(self, CC, CS): + actuators = CC.actuators + pcm_cancel_cmd = CC.cruiseControl.cancel + + can_sends = [] + + # Temp disable steering on a hands_on_fault, and allow for user override + hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 + lkas_enabled = CC.latActive and not hands_on_fault + + if lkas_enabled: + apply_angle = actuators.steeringAngleDeg + + # Angular rate limit based on speed + steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) + rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN + max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) + apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) + + # To not fault the EPS + apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) + else: + apply_angle = CS.out.steeringAngleDeg + + self.last_angle = apply_angle + can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) + + # Longitudinal control (in sync with stock message, about 40Hz) + if self.CP.openpilotLongitudinalControl: + target_accel = actuators.accel + target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) + max_accel = 0 if target_accel < 0 else target_accel + min_accel = 0 if target_accel > 0 else target_accel + + while len(CS.das_control_counters) > 0: + can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft())) + + # Cancel on user steering override, since there is no steering torque blending + if hands_on_fault: + pcm_cancel_cmd = True + + if self.frame % 10 == 0 and pcm_cancel_cmd: + # Spam every possible counter value, otherwise it might not be accepted + for counter in range(16): + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter)) + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter)) + + # TODO: HUD control + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py new file mode 100644 index 00000000000000..0f373842f21a56 --- /dev/null +++ b/selfdrive/car/tesla/carstate.py @@ -0,0 +1,193 @@ +import copy +from collections import deque +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from selfdrive.car.interfaces import CarStateBase +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.button_states = {button.event_type: False for button in BUTTONS} + self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis']) + + # Needed by carcontroller + self.msg_stw_actn_req = None + self.hands_on_level = 0 + self.steer_warning = None + self.acc_state = 0 + self.das_control_counters = deque(maxlen=32) + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + # Vehicle speed + ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = (ret.vEgo < 0.1) + + # Gas pedal + ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0 + ret.gasPressed = (ret.gas > 0) + + # Brake pedal + ret.brake = 0 + ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) + + # Steering wheel + self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"] + self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None) + steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None) + + ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"] + ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate + ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"] + ret.steeringPressed = (self.hands_on_level > 0) + ret.steerFaultPermanent = steer_status == "EAC_FAULT" + ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) + + # Cruise state + cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None) + speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None) + + acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) + + ret.cruiseState.enabled = acc_enabled + if speed_units == "KPH": + ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS + elif speed_units == "MPH": + ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS + ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled) + ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special + + # Gear + ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] + + # Buttons + buttonEvents = [] + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + buttonEvents.append(event) + self.button_states[button.event_type] = state + ret.buttonEvents = buttonEvents + + # Doors + ret.doorOpen = any([(self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS]) + + # Blinkers + ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1) + ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) + + # Seatbelt + ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) + + # TODO: blindspot + + # AEB + ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1) + + # Messages needed by carcontroller + self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"]) + self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"] + self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"]) + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("ESP_vehicleSpeed", "ESP_B"), + ("DI_pedalPos", "DI_torque1"), + ("DI_brakePedal", "DI_torque2"), + ("StW_AnglHP", "STW_ANGLHP_STAT"), + ("StW_AnglHP_Spd", "STW_ANGLHP_STAT"), + ("EPAS_handsOnLevel", "EPAS_sysStatus"), + ("EPAS_torsionBarTorque", "EPAS_sysStatus"), + ("EPAS_internalSAS", "EPAS_sysStatus"), + ("EPAS_eacStatus", "EPAS_sysStatus"), + ("EPAS_eacErrorCode", "EPAS_sysStatus"), + ("DI_cruiseState", "DI_state"), + ("DI_digitalSpeed", "DI_state"), + ("DI_speedUnits", "DI_state"), + ("DI_gear", "DI_torque2"), + ("DOOR_STATE_FL", "GTW_carState"), + ("DOOR_STATE_FR", "GTW_carState"), + ("DOOR_STATE_RL", "GTW_carState"), + ("DOOR_STATE_RR", "GTW_carState"), + ("DOOR_STATE_FrontTrunk", "GTW_carState"), + ("BOOT_STATE", "GTW_carState"), + ("BC_indicatorLStatus", "GTW_carState"), + ("BC_indicatorRStatus", "GTW_carState"), + ("SDM_bcklDrivStatus", "SDM1"), + ("driverBrakeStatus", "BrakeMessage"), + + # We copy this whole message when spamming cancel + ("SpdCtrlLvr_Stat", "STW_ACTN_RQ"), + ("VSL_Enbl_Rq", "STW_ACTN_RQ"), + ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"), + ("DTR_Dist_Rq", "STW_ACTN_RQ"), + ("TurnIndLvr_Stat", "STW_ACTN_RQ"), + ("HiBmLvr_Stat", "STW_ACTN_RQ"), + ("WprWashSw_Psd", "STW_ACTN_RQ"), + ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"), + ("StW_Lvr_Stat", "STW_ACTN_RQ"), + ("StW_Cond_Flt", "STW_ACTN_RQ"), + ("StW_Cond_Psd", "STW_ACTN_RQ"), + ("HrnSw_Psd", "STW_ACTN_RQ"), + ("StW_Sw00_Psd", "STW_ACTN_RQ"), + ("StW_Sw01_Psd", "STW_ACTN_RQ"), + ("StW_Sw02_Psd", "STW_ACTN_RQ"), + ("StW_Sw03_Psd", "STW_ACTN_RQ"), + ("StW_Sw04_Psd", "STW_ACTN_RQ"), + ("StW_Sw05_Psd", "STW_ACTN_RQ"), + ("StW_Sw06_Psd", "STW_ACTN_RQ"), + ("StW_Sw07_Psd", "STW_ACTN_RQ"), + ("StW_Sw08_Psd", "STW_ACTN_RQ"), + ("StW_Sw09_Psd", "STW_ACTN_RQ"), + ("StW_Sw10_Psd", "STW_ACTN_RQ"), + ("StW_Sw11_Psd", "STW_ACTN_RQ"), + ("StW_Sw12_Psd", "STW_ACTN_RQ"), + ("StW_Sw13_Psd", "STW_ACTN_RQ"), + ("StW_Sw14_Psd", "STW_ACTN_RQ"), + ("StW_Sw15_Psd", "STW_ACTN_RQ"), + ("WprSw6Posn", "STW_ACTN_RQ"), + ("MC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ] + + checks = [ + # sig_address, frequency + ("ESP_B", 50), + ("DI_torque1", 100), + ("DI_torque2", 100), + ("STW_ANGLHP_STAT", 100), + ("EPAS_sysStatus", 25), + ("DI_state", 10), + ("STW_ACTN_RQ", 10), + ("GTW_carState", 10), + ("SDM1", 10), + ("BrakeMessage", 50), + ] + + return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.chassis) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address + ("DAS_accState", "DAS_control"), + ("DAS_aebEvent", "DAS_control"), + ("DAS_controlCounter", "DAS_control"), + ] + checks = [ + # sig_address, frequency + ("DAS_control", 40), + ] + return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py new file mode 100755 index 00000000000000..2eb29efb41da3b --- /dev/null +++ b/selfdrive/car/tesla/interface.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from selfdrive.car.tesla.values import CANBUS, CAR +from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "tesla" + + # There is no safe way to do steer blending with user torque, + # so the steering behaves like autopilot. This is not + # how openpilot should be, hence dashcamOnly + ret.dashcamOnly = True + + ret.steerControlType = car.CarParams.SteerControlType.angle + + # Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command + ret.longitudinalTuning.kpBP = [0] + ret.longitudinalTuning.kpV = [0] + ret.longitudinalTuning.kiBP = [0] + ret.longitudinalTuning.kiV = [0] + ret.stopAccel = 0.0 + ret.longitudinalActuatorDelayUpperBound = 0.5 # s + ret.radarTimeStep = (1.0 / 8) # 8Hz + + # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus + # If so, we assume that it is connected to the longitudinal harness. + if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): + ret.openpilotLongitudinalControl = True + ret.safetyConfigs = [ + get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL), + get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN), + ] + else: + ret.openpilotLongitudinalControl = False + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + + ret.steerLimitTimer = 1.0 + ret.steerActuatorDelay = 0.25 + + if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): + ret.mass = 2100. + STD_CARGO_KG + ret.wheelbase = 2.959 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 15.0 + else: + raise ValueError(f"Unsupported car: {candidate}") + + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + ret.events = self.create_common_events(ret).to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py new file mode 100755 index 00000000000000..a09f53e75824e7 --- /dev/null +++ b/selfdrive/car/tesla/radar_interface.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.tesla.values import DBC, CANBUS +from selfdrive.car.interfaces import RadarInterfaceBase + +RADAR_MSGS_A = list(range(0x310, 0x36E, 3)) +RADAR_MSGS_B = list(range(0x311, 0x36F, 3)) +NUM_POINTS = len(RADAR_MSGS_A) + +def get_radar_can_parser(CP): + # Status messages + signals = [ + ('RADC_HWFail', 'TeslaRadarSguInfo'), + ('RADC_SGUFail', 'TeslaRadarSguInfo'), + ('RADC_SensorDirty', 'TeslaRadarSguInfo'), + ] + + checks = [ + ('TeslaRadarSguInfo', 10), + ] + + # Radar tracks. There are also raw point clouds available, + # we don't use those. + for i in range(NUM_POINTS): + msg_id_a = RADAR_MSGS_A[i] + msg_id_b = RADAR_MSGS_B[i] + + # There is a bunch more info in the messages, + # but these are the only things actually used in openpilot + signals.extend([ + ('LongDist', msg_id_a), + ('LongSpeed', msg_id_a), + ('LatDist', msg_id_a), + ('LongAccel', msg_id_a), + ('Meas', msg_id_a), + ('Tracked', msg_id_a), + ('Index', msg_id_a), + + ('LatSpeed', msg_id_b), + ('Index2', msg_id_b), + ]) + + checks.extend([ + (msg_id_a, 8), + (msg_id_b, 8), + ]) + + return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, CANBUS.radar) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.rcp = get_radar_can_parser(CP) + self.updated_messages = set() + self.track_id = 0 + self.trigger_msg = RADAR_MSGS_B[-1] + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + values = self.rcp.update_strings(can_strings) + self.updated_messages.update(values) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + + # Errors + errors = [] + sgu_info = self.rcp.vl['TeslaRadarSguInfo'] + if not self.rcp.can_valid: + errors.append('canError') + if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']: + errors.append('fault') + ret.errors = errors + + # Radar tracks + for i in range(NUM_POINTS): + msg_a = self.rcp.vl[RADAR_MSGS_A[i]] + msg_b = self.rcp.vl[RADAR_MSGS_B[i]] + + # Make sure msg A and B are together + if msg_a['Index'] != msg_b['Index2']: + continue + + # Check if it's a valid track + if not msg_a['Tracked']: + if i in self.pts: + del self.pts[i] + continue + + # New track! + if i not in self.pts: + self.pts[i] = car.RadarData.RadarPoint.new_message() + self.pts[i].trackId = self.track_id + self.track_id += 1 + + # Parse track data + self.pts[i].dRel = msg_a['LongDist'] + self.pts[i].yRel = msg_a['LatDist'] + self.pts[i].vRel = msg_a['LongSpeed'] + self.pts[i].aRel = msg_a['LongAccel'] + self.pts[i].yvRel = msg_b['LatSpeed'] + self.pts[i].measured = bool(msg_a['Meas']) + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py new file mode 100644 index 00000000000000..e5d904f80efc50 --- /dev/null +++ b/selfdrive/car/tesla/teslacan.py @@ -0,0 +1,62 @@ +import copy +import crcmod + +from common.conversions import Conversions as CV +from selfdrive.car.tesla.values import CANBUS, CarControllerParams + + +class TeslaCAN: + def __init__(self, packer, pt_packer): + self.packer = packer + self.pt_packer = pt_packer + self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) + + @staticmethod + def checksum(msg_id, dat): + # TODO: get message ID from name instead + ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) + ret += sum(dat) + return ret & 0xFF + + def create_steering_control(self, angle, enabled, frame): + values = { + "DAS_steeringAngleRequest": -angle, + "DAS_steeringHapticRequest": 0, + "DAS_steeringControlType": 1 if enabled else 0, + "DAS_steeringControlCounter": (frame % 16), + } + + data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2] + values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3]) + return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values) + + def create_action_request(self, msg_stw_actn_req, cancel, bus, counter): + values = copy.copy(msg_stw_actn_req) + + if cancel: + values["SpdCtrlLvr_Stat"] = 1 + values["MC_STW_ACTN_RQ"] = counter + + data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2] + values["CRC_STW_ACTN_RQ"] = self.crc(data[:7]) + return self.packer.make_can_msg("STW_ACTN_RQ", bus, values) + + def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt): + messages = [] + values = { + "DAS_setSpeed": speed * CV.MS_TO_KPH, + "DAS_accState": acc_state, + "DAS_aebEvent": 0, + "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN, + "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX, + "DAS_accelMin": min_accel, + "DAS_accelMax": max_accel, + "DAS_controlCounter": cnt, + "DAS_controlChecksum": 0, + } + + for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]: + data = packer.make_can_msg("DAS_control", bus, values)[2] + values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) + messages.append(packer.make_can_msg("DAS_control", bus, values)) + return messages diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py new file mode 100644 index 00000000000000..296169587adeeb --- /dev/null +++ b/selfdrive/car/tesla/values.py @@ -0,0 +1,77 @@ +from collections import namedtuple +from typing import Dict, List, Union + +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo +from cereal import car + +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) + + +class CAR: + AP1_MODELS = 'TESLA AP1 MODEL S' + AP2_MODELS = 'TESLA AP2 MODEL S' + + +CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { + CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"), + CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"), +} + +FINGERPRINTS = { + CAR.AP2_MODELS: [ + { + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 + }, + ], + CAR.AP1_MODELS: [ + { + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4 + }, + ], +} + +DBC = { + CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), + CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), +} + +class CANBUS: + # Lateral harness + chassis = 0 + radar = 1 + autopilot_chassis = 2 + + # Longitudinal harness + powertrain = 4 + private = 5 + autopilot_powertrain = 6 + +GEAR_MAP = { + "DI_GEAR_INVALID": car.CarState.GearShifter.unknown, + "DI_GEAR_P": car.CarState.GearShifter.park, + "DI_GEAR_R": car.CarState.GearShifter.reverse, + "DI_GEAR_N": car.CarState.GearShifter.neutral, + "DI_GEAR_D": car.CarState.GearShifter.drive, + "DI_GEAR_SNA": car.CarState.GearShifter.unknown, +} + +DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"] + +# Make sure the message and addr is also in the CAN parser! +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]), + Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), + Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), +] + +class CarControllerParams: + RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + JERK_LIMIT_MAX = 8 + JERK_LIMIT_MIN = -8 + ACCEL_TO_SPEED_MULTIPLIER = 3 diff --git a/selfdrive/car/tests/big_cars_test.sh b/selfdrive/car/tests/big_cars_test.sh deleted file mode 100755 index bb6e82dd0ebbf5..00000000000000 --- a/selfdrive/car/tests/big_cars_test.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env bash - -SCRIPT_DIR=$(dirname "$0") -BASEDIR=$(realpath "$SCRIPT_DIR/../../../") -cd $BASEDIR - -export MAX_EXAMPLES=300 -export INTERNAL_SEG_CNT=300 -export INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt - -cd selfdrive/car/tests && pytest test_models.py test_car_interfaces.py diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py new file mode 100644 index 00000000000000..635f43cc8dec7d --- /dev/null +++ b/selfdrive/car/tests/routes.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +from collections import namedtuple + +from selfdrive.car.chrysler.values import CAR as CHRYSLER +from selfdrive.car.gm.values import CAR as GM +from selfdrive.car.ford.values import CAR as FORD +from selfdrive.car.honda.values import CAR as HONDA +from selfdrive.car.hyundai.values import CAR as HYUNDAI +from selfdrive.car.nissan.values import CAR as NISSAN +from selfdrive.car.mazda.values import CAR as MAZDA +from selfdrive.car.subaru.values import CAR as SUBARU +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN +from selfdrive.car.tesla.values import CAR as TESLA +from selfdrive.car.body.values import CAR as COMMA + +# TODO: add routes for these cars +non_tested_cars = [ + FORD.ESCAPE_MK4, + FORD.FOCUS_MK4, + GM.CADILLAC_ATS, + GM.HOLDEN_ASTRA, + GM.MALIBU, + HYUNDAI.ELANTRA_GT_I30, + HYUNDAI.GENESIS_G90, + HYUNDAI.KIA_OPTIMA_H, +] + +CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,)) + +routes = [ + CarTestRoute("efdf9af95e71cd84|2022-05-13--19-03-31", COMMA.BODY), + + CarTestRoute("0c94aa1e1296d7c6|2021-05-05--19-48-37", CHRYSLER.JEEP_CHEROKEE), + CarTestRoute("91dfedae61d7bd75|2021-05-22--20-07-52", CHRYSLER.JEEP_CHEROKEE_2019), + CarTestRoute("420a8e183f1aed48|2020-03-05--07-15-29", CHRYSLER.PACIFICA_2017_HYBRID), + CarTestRoute("43a685a66291579b|2021-05-27--19-47-29", CHRYSLER.PACIFICA_2018), + CarTestRoute("378472f830ee7395|2021-05-28--07-38-43", CHRYSLER.PACIFICA_2018_HYBRID), + CarTestRoute("8190c7275a24557b|2020-01-29--08-33-58", CHRYSLER.PACIFICA_2019_HYBRID), + CarTestRoute("3d84727705fecd04|2021-05-25--08-38-56", CHRYSLER.PACIFICA_2020), + CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500), + CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD, segment=6), + + CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6), + #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), + + CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), + CarTestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL), + CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.ESCALADE_ESV), + CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.VOLT), + CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.BOLT_EUV), + CarTestRoute("38aa7da107d5d252|2022-08-15--16-01-12", GM.SILVERADO), + + CarTestRoute("0e7a2ba168465df5|2020-10-18--14-14-22", HONDA.ACURA_RDX_3G), + CarTestRoute("a74b011b32b51b56|2020-07-26--17-09-36", HONDA.CIVIC), + CarTestRoute("a859a044a447c2b0|2020-03-03--18-42-45", HONDA.CRV_EU), + CarTestRoute("68aac44ad69f838e|2021-05-18--20-40-52", HONDA.CRV), + CarTestRoute("14fed2e5fa0aa1a5|2021-05-25--14-59-42", HONDA.CRV_HYBRID), + CarTestRoute("52f3e9ae60c0d886|2021-05-23--15-59-43", HONDA.FIT), + CarTestRoute("2c4292a5cd10536c|2021-08-19--21-32-15", HONDA.FREED), + CarTestRoute("03be5f2fd5c508d1|2020-04-19--18-44-15", HONDA.HRV), + CarTestRoute("917b074700869333|2021-05-24--20-40-20", HONDA.ACURA_ILX), + CarTestRoute("81722949a62ea724|2019-04-06--15-19-25", HONDA.ODYSSEY_CHN), + CarTestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.ACCORD), # 1.5T + CarTestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.ACCORD), # 2.0T + CarTestRoute("085ac1d942c35910|2021-03-25--20-11-15", HONDA.ACCORD), # 2021 with new style HUD msgs + CarTestRoute("07585b0da3c88459|2021-05-26--18-52-04", HONDA.ACCORDH), + CarTestRoute("f29e2b57a55e7ad5|2021-03-24--20-52-38", HONDA.ACCORDH), # 2021 with new style HUD msgs + CarTestRoute("1ad763dd22ef1a0e|2020-02-29--18-37-03", HONDA.CRV_5G), + CarTestRoute("0a96f86fcfe35964|2020-02-05--07-25-51", HONDA.ODYSSEY), + CarTestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL), + CarTestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.INSIGHT), + CarTestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.PILOT), + CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PASSPORT), + CarTestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.CIVIC_BOSCH), + CarTestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), + CarTestRoute("54fd8451b3974762|2021-04-01--14-50-10", HONDA.RIDGELINE), + CarTestRoute("2d5808fae0b38ac6|2021-09-01--17-14-11", HONDA.HONDA_E), + CarTestRoute("f44aa96ace22f34a|2021-12-22--06-22-31", HONDA.CIVIC_2022), + + CarTestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS), + CarTestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70), + CarTestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80), + CarTestRoute("4dbd55df87507948|2022-03-01--09-45-38", HYUNDAI.SANTA_FE), + CarTestRoute("bf43d9df2b660eb0|2021-09-23--14-16-37", HYUNDAI.SANTA_FE_2022), + CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022), + CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022, segment=1), + CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED), + CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA), + CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), + CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA), + CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF), + CarTestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.TUCSON), + CarTestRoute("36e10531feea61a4|2022-07-25--13-37-42", HYUNDAI.TUCSON_HYBRID_4TH_GEN), + CarTestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), + CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.PALISADE), + CarTestRoute("22de8111a8c5463c|2022-07-29--13-34-49", HYUNDAI.IONIQ_5), + CarTestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.IONIQ_PHEV_2019), + CarTestRoute("fa8db5869167f821|2021-06-10--22-50-10", HYUNDAI.IONIQ_PHEV), + CarTestRoute("2c5cf2dd6102e5da|2020-12-17--16-06-44", HYUNDAI.IONIQ_EV_2020), + CarTestRoute("610ebb9faaad6b43|2020-06-13--15-28-36", HYUNDAI.IONIQ_EV_LTD), + CarTestRoute("2c5cf2dd6102e5da|2020-06-26--16-00-08", HYUNDAI.IONIQ), + CarTestRoute("ab59fe909f626921|2021-10-18--18-34-28", HYUNDAI.IONIQ_HEV_2022), + CarTestRoute("22d955b2cd499c22|2020-08-10--19-58-21", HYUNDAI.KONA), + CarTestRoute("efc48acf44b1e64d|2021-05-28--21-05-04", HYUNDAI.KONA_EV), + CarTestRoute("ff973b941a69366f|2022-07-28--22-01-19", HYUNDAI.KONA_EV_2022, segment=11), + CarTestRoute("49f3c13141b6bc87|2021-07-28--08-05-13", HYUNDAI.KONA_HEV), + CarTestRoute("5dddcbca6eb66c62|2020-07-26--13-24-19", HYUNDAI.KIA_STINGER), + CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.VELOSTER), + CarTestRoute("d824e27e8c60172c|2022-05-19--16-15-28", HYUNDAI.KIA_EV6), + CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), + CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV), + CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV), + CarTestRoute("34a875f29f69841a|2021-07-29--13-02-09", HYUNDAI.KIA_NIRO_HEV_2021), + CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE), + CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.ELANTRA), + CarTestRoute("82e9cdd3f43bf83e|2021-05-15--02-42-51", HYUNDAI.ELANTRA_2021), + CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.ELANTRA_HEV_2021), + CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.SONATA_HYBRID), + CarTestRoute("715ac05b594e9c59|2021-10-27--23-24-56", HYUNDAI.GENESIS_G70_2020), + + CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.ALPHARD_TSS2), + CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.ALPHARDH_TSS2), + CarTestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.AVALON), + CarTestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.AVALON_2019), + CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALONH_2019), + CarTestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.AVALON_TSS2), + CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALONH_TSS2), + CarTestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.CAMRY), + CarTestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.CAMRY_TSS2), + CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRYH_TSS2), + CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.CAMRYH), + CarTestRoute("4e45c89c38e8ec4d|2021-05-02--02-49-28", TOYOTA.COROLLA), + CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2), + CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLAH_TSS2), + CarTestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.PRIUS), + CarTestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.RAV4), + CarTestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H), + CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), + CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.RAV4_TSS2_2022), + CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2), + CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), + CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), + CarTestRoute("da23c367491f53e2|2021-05-21--09-09-11", TOYOTA.LEXUS_CTH, segment=3), + CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ESH_TSS2), + CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ESH), + CarTestRoute("32696cea52831b02|2021-11-19--18-13-30", TOYOTA.LEXUS_RC), + CarTestRoute("886fcd8408d570e9|2020-01-29--02-18-55", TOYOTA.LEXUS_RX), + CarTestRoute("d27ad752e9b08d4f|2021-05-26--19-39-51", TOYOTA.LEXUS_RXH), + CarTestRoute("01b22eb2ed121565|2020-02-02--11-25-51", TOYOTA.LEXUS_RX_TSS2), + CarTestRoute("b74758c690a49668|2020-05-20--15-58-57", TOYOTA.LEXUS_RXH_TSS2), + CarTestRoute("ec429c0f37564e3c|2020-02-01--17-28-12", TOYOTA.LEXUS_NXH), + CarTestRoute("964c09eb11ca8089|2020-11-03--22-04-00", TOYOTA.LEXUS_NX), + CarTestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2), + CarTestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NXH_TSS2), + CarTestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.HIGHLANDER_TSS2), + CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.HIGHLANDERH_TSS2), + CarTestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.HIGHLANDER), + CarTestRoute("80d16a262e33d57f|2021-05-23--20-01-43", TOYOTA.HIGHLANDERH), + CarTestRoute("eb6acd681135480d|2019-06-20--20-00-00", TOYOTA.SIENNA), + CarTestRoute("2e07163a1ba9a780|2019-08-25--13-15-13", TOYOTA.LEXUS_IS), + CarTestRoute("0a0de17a1e6a2d15|2020-09-21--21-24-41", TOYOTA.PRIUS_TSS2), + CarTestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.MIRAI), + CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.CHR), + CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHRH), + CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), + + CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), + CarTestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.ATLAS_MK1), + CarTestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.GOLF_MK7), + CarTestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.JETTA_MK7), + CarTestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.PASSAT_MK8), + CarTestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.PASSAT_NMS), + CarTestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.POLO_MK6), + CarTestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.TAOS_MK1), + CarTestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.TCROSS_MK1), + CarTestRoute("2cef8a0b898f331a|2021-03-25--20-13-57", VOLKSWAGEN.TIGUAN_MK2), + CarTestRoute("a589dcc642fdb10a|2021-06-14--20-54-26", VOLKSWAGEN.TOURAN_MK2), + CarTestRoute("a459f4556782eba1|2021-09-19--09-48-00", VOLKSWAGEN.TRANSPORTER_T61), + CarTestRoute("0cd0b7f7e31a3853|2021-11-18--00-38-32", VOLKSWAGEN.TROC_MK1), + CarTestRoute("07667b885add75fd|2021-01-23--19-48-42", VOLKSWAGEN.AUDI_A3_MK3), + CarTestRoute("6c6b466346192818|2021-06-06--14-17-47", VOLKSWAGEN.AUDI_Q2_MK1), + CarTestRoute("0cd0b7f7e31a3853|2021-12-03--03-12-05", VOLKSWAGEN.AUDI_Q3_MK2), + CarTestRoute("8f205bdd11bcbb65|2021-03-26--01-00-17", VOLKSWAGEN.SEAT_ATECA_MK1), + CarTestRoute("fc6b6c9a3471c846|2021-05-27--13-39-56", VOLKSWAGEN.SEAT_LEON_MK3), + CarTestRoute("12d6ae3057c04b0d|2021-09-15--00-04-07", VOLKSWAGEN.SKODA_KAMIQ_MK1), + CarTestRoute("12d6ae3057c04b0d|2021-09-04--21-21-21", VOLKSWAGEN.SKODA_KAROQ_MK1), + CarTestRoute("90434ff5d7c8d603|2021-03-15--12-07-31", VOLKSWAGEN.SKODA_KODIAQ_MK1), + CarTestRoute("66e5edc3a16459c5|2021-05-25--19-00-29", VOLKSWAGEN.SKODA_OCTAVIA_MK3), + CarTestRoute("026b6d18fba6417f|2021-03-26--09-17-04", VOLKSWAGEN.SKODA_SCALA_MK1), + CarTestRoute("b2e9858e29db492b|2021-03-26--16-58-42", VOLKSWAGEN.SKODA_SUPERB_MK3), + + CarTestRoute("3c8f0c502e119c1c|2020-06-30--12-58-02", SUBARU.ASCENT), + CarTestRoute("c321c6b697c5a5ff|2020-06-23--11-04-33", SUBARU.FORESTER), + CarTestRoute("791340bc01ed993d|2019-03-10--16-28-08", SUBARU.IMPREZA), + CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020), + CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=3), + CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3), + # Pre-global, dashcam + CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.FORESTER_PREGLOBAL), + CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.LEGACY_PREGLOBAL), + CarTestRoute("5ab784f361e19b78|2020-06-08--16-30-41", SUBARU.OUTBACK_PREGLOBAL), + CarTestRoute("e19eb5d5353b1ac1|2020-08-09--14-37-56", SUBARU.OUTBACK_PREGLOBAL_2018), + + CarTestRoute("fbbfa6af821552b9|2020-03-03--08-09-43", NISSAN.XTRAIL), + CarTestRoute("5b7c365c50084530|2020-03-25--22-10-13", NISSAN.LEAF), + CarTestRoute("22c3dcce2dd627eb|2020-12-30--16-38-48", NISSAN.LEAF_IC), + CarTestRoute("059ab9162e23198e|2020-05-30--09-41-01", NISSAN.ROGUE), + CarTestRoute("b72d3ec617c0a90f|2020-12-11--15-38-17", NISSAN.ALTIMA), + + CarTestRoute("32a319f057902bb3|2020-04-27--15-18-58", MAZDA.CX5), + CarTestRoute("10b5a4b380434151|2020-08-26--17-11-45", MAZDA.CX9), + CarTestRoute("74f1038827005090|2020-08-26--20-05-50", MAZDA.MAZDA3), + CarTestRoute("fb53c640f499b73d|2021-06-01--04-17-56", MAZDA.MAZDA6), + CarTestRoute("f6d5b1a9d7a1c92e|2021-07-08--06-56-59", MAZDA.CX9_2021), + CarTestRoute("a4af1602d8e668ac|2022-02-03--12-17-07", MAZDA.CX5_2022), + + CarTestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS), + CarTestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS), + + # Segments that test specific issues + # Controls mismatch due to interceptor threshold + CarTestRoute("cfb32f0fb91b173b|2022-04-06--14-54-45", HONDA.CIVIC, segment=21), + CarTestRoute("5a8762b91fc70467|2022-04-14--21-26-20", TOYOTA.RAV4, segment=2), + # Controls mismatch due to standstill threshold + CarTestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.CRV_HYBRID, segment=22), +] diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py old mode 100644 new mode 100755 index 24d2faa0db14e9..aabf652c8ccfce --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -1,61 +1,73 @@ -import os -import hypothesis.strategies as st -from hypothesis import Phase, given, settings +#!/usr/bin/env python3 +import math +import unittest +import importlib from parameterized import parameterized from cereal import car -from opendbc.car import DT_CTRL -from opendbc.car.structs import CarParams -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.controls.lib.latcontrol_angle import LatControlAngle -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID -from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque -from openpilot.selfdrive.controls.lib.longcontrol import LongControl -from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator - -MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '60')) - - -class TestCarInterfaces: - # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause - # many generated examples to overrun when max_examples > ~20, don't use it - @parameterized.expand([(car,) for car in sorted(PLATFORMS)] + [MOCK.MOCK]) - @settings(max_examples=MAX_EXAMPLES, deadline=None, - phases=(Phase.reuse, Phase.generate, Phase.shrink)) - @given(data=st.data()) - def test_car_interfaces(self, car_name, data): - car_interface = get_fuzzy_car_interface(car_name, data.draw) - car_params = car_interface.CP.as_reader() - - cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) +from selfdrive.car import gen_empty_fingerprint +from selfdrive.car.fingerprints import all_known_cars +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS + +class TestCarInterfaces(unittest.TestCase): + + @parameterized.expand([(car,) for car in all_known_cars()]) + def test_car_interfaces(self, car_name): + if car_name in FINGERPRINTS: + fingerprint = FINGERPRINTS[car_name][0] + else: + fingerprint = {} + + CarInterface, CarController, CarState = interfaces[car_name] + fingerprints = gen_empty_fingerprint() + fingerprints.update({k: fingerprint for k in fingerprints.keys()}) + + car_fw = [] + + car_params = CarInterface.get_params(car_name, fingerprints, car_fw) + car_interface = CarInterface(car_params, CarController, CarState) + assert car_params + assert car_interface + + self.assertGreater(car_params.mass, 1) + self.assertGreater(car_params.maxLateralAccel, 0) + + if car_params.steerControlType != car.CarParams.SteerControlType.angle: + tuning = car_params.lateralTuning.which() + if tuning == 'pid': + self.assertTrue(len(car_params.lateralTuning.pid.kpV)) + elif tuning == 'torque': + kf = car_params.lateralTuning.torque.kf + self.assertTrue(not math.isnan(kf) and kf > 0) + self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction)) + elif tuning == 'indi': + self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) + # Run car interface - now_nanos = 0 - CC = car.CarControl.new_message(**cc_msg) - CC = CC.as_reader() + CC = car.CarControl.new_message() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) - now_nanos += DT_CTRL * 1e9 # 10 ms + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) - CC = car.CarControl.new_message(**cc_msg) + CC = car.CarControl.new_message() CC.enabled = True - CC.latActive = True - CC.longActive = True - CC = CC.as_reader() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) - now_nanos += DT_CTRL * 1e9 # 10ms - - # 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) - if car_params.steerControlType == CarParams.SteerControlType.angle: - LatControlAngle(car_params, car_interface, DT_CTRL) - elif car_params.lateralTuning.which() == 'pid': - LatControlPID(car_params, car_interface, DT_CTRL) - elif car_params.lateralTuning.which() == 'torque': - LatControlTorque(car_params, car_interface, DT_CTRL) + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) + + # Test radar interface + RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface + radar_interface = RadarInterface(car_params) + assert radar_interface + + # Run radar interface once + radar_interface.update([]) + if not car_params.radarOffCan and radar_interface.rcp is not None and \ + hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): + radar_interface._update([radar_interface.trigger_msg]) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py deleted file mode 100644 index aa70e49f5d6ed7..00000000000000 --- a/selfdrive/car/tests/test_cruise_speed.py +++ /dev/null @@ -1,151 +0,0 @@ -import pytest -import itertools -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 openpilot.common.constants import CV -from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver - -ButtonEvent = car.CarState.ButtonEvent -ButtonType = car.CarState.ButtonEvent.Type - - -def run_cruise_simulation(cruise, e2e, personality, t_end=20.): - man = Maneuver( - '', - duration=t_end, - initial_speed=max(cruise - 1., 0.0), - lead_relevancy=True, - initial_distance_lead=100, - cruise_values=[cruise], - prob_lead_values=[0.0], - breakpoints=[0.], - e2e=e2e, - personality=personality, - ) - valid, output = man.evaluate() - assert valid - return output[-1, 3] - - -@parameterized_class(("e2e", "personality", "speed"), itertools.product( - [True, False], # e2e - log.LongitudinalPersonality.schema.enumerants, # personality - [5,35])) # speed -class TestCruiseSpeed: - def test_cruise_speed(self): - print(f'Testing {self.speed} m/s') - cruise_speed = float(self.speed) - - simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) - 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,)]) -class TestVCruiseHelper: - def setup_method(self): - self.CP = car.CarParams(pcmCruise=self.pcm_cruise) - self.v_cruise_helper = VCruiseHelper(self.CP) - self.reset_cruise_speed_state() - - def reset_cruise_speed_state(self): - # Two resets previous cruise speed - for _ in range(2): - self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) - - def enable(self, v_ego, experimental_mode): - # Simulates user pressing set with a current speed - self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) - - def test_adjust_speed(self): - """ - Asserts speed changes on falling edges of buttons. - """ - - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - for btn in (ButtonType.accelCruise, ButtonType.decelCruise): - for pressed in (True, False): - CS = car.CarState(cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)] - - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) - - def test_rising_edge_enable(self): - """ - Some car interfaces may enable on rising edge of a button, - ensure we don't adjust speed if enabled changes mid-press. - """ - - # NOTE: enabled is always one frame behind the result from button press in controlsd - for enabled, pressed in ((False, False), - (False, True), - (True, False)): - CS = car.CarState(cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) - if pressed: - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - # Expected diff on enabling. Speed should not change on falling edge of pressed - assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last - - def test_resume_in_standstill(self): - """ - Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. - """ - - self.enable(0, False) - - for standstill in (True, False): - for pressed in (True, False): - CS = car.CarState(cruiseState={"available": True, "standstill": standstill}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - - # speed should only update if not at standstill and button falling edge - should_equal = standstill or pressed - assert should_equal == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) - - def test_set_gas_pressed(self): - """ - Asserts pressing set while enabled with gas pressed sets - the speed to the maximum of vEgo and current cruise speed. - """ - - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - # first decrement speed, then perform gas pressed logic - expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT - expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH) # clip to min of vEgo - expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)) - - CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - - # TODO: fix skipping first run due to enabled on rising edge exception - if v_ego == 0.0: - continue - assert expected_v_cruise_kph == self.v_cruise_helper.v_cruise_kph - - def test_initialize_v_cruise(self): - """ - Asserts allowed cruise speeds on enabling with SET. - """ - - for experimental_mode in (True, False): - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - assert not self.v_cruise_helper.v_cruise_initialized - - self.enable(float(v_ego), experimental_mode) - assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX - assert self.v_cruise_helper.v_cruise_initialized diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py old mode 100644 new mode 100755 index 6e13d55b29a52d..af58bb5e5931fd --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -1,22 +1,69 @@ -import os +#!/usr/bin/env python3 +import re +import unittest -from openpilot.common.basedir import BASEDIR -from opendbc.car.docs import generate_cars_md, get_all_car_docs -from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs -from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff -from openpilot.selfdrive.car.docs import CARS_MD_TEMPLATE +from selfdrive.car.car_helpers import interfaces, get_interface_attr +from selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_info +from selfdrive.car.docs_definitions import Column, Harness, Star +from selfdrive.car.honda.values import CAR as HONDA -class TestCarDocs: - @classmethod - def setup_class(cls): - cls.all_cars = get_all_car_docs() +class TestCarDocs(unittest.TestCase): + def setUp(self): + self.all_cars = get_all_car_info() def test_generator(self): - generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) + generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) + with open(CARS_MD_OUT, "r") as f: + current_cars_md = f.read() - def test_docs_diff(self): - dump_path = os.path.join(BASEDIR, "selfdrive", "car", "tests", "cars_dump") - dump_car_docs(dump_path) - print_car_docs_diff(dump_path) - os.remove(dump_path) + self.assertEqual(generated_cars_md, current_cars_md, + "Run selfdrive/car/docs.py to update the compatibility documentation") + + def test_missing_car_info(self): + all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys() + for platform in sorted(interfaces.keys()): + with self.subTest(platform=platform): + self.assertTrue(platform in all_car_info_platforms, "Platform: {} doesn't exist in CarInfo".format(platform)) + + def test_naming_conventions(self): + # Asserts market-standard car naming conventions by brand + for car in self.all_cars: + with self.subTest(car=car): + tokens = car.model.lower().split(" ") + if car.car_name == "hyundai": + self.assertNotIn("phev", tokens, "Use `Plug-in Hybrid`") + self.assertNotIn("hev", tokens, "Use `Hybrid`") + self.assertNotIn("ev", tokens, "Use `Electric`") + if "plug-in hybrid" in car.model.lower(): + self.assertIn("Plug-in Hybrid", car.model, "Use correct capitalization") + elif car.car_name == "toyota": + if "rav4" in tokens: + self.assertIn("RAV4", car.model, "Use correct capitalization") + + def test_torque_star(self): + # Asserts brand-specific assumptions around steering torque star + for car in self.all_cars: + with self.subTest(car=car): + # honda sanity check, it's the definition of a no torque star + if car.car_fingerprint in (HONDA.ACCORD, HONDA.CIVIC, HONDA.CRV, HONDA.ODYSSEY, HONDA.PILOT): + self.assertEqual(car.row[Column.STEERING_TORQUE], Star.EMPTY, f"{car.name} has full torque star") + elif car.car_name in ("toyota", "hyundai"): + self.assertNotEqual(car.row[Column.STEERING_TORQUE], Star.EMPTY, f"{car.name} has no torque star") + + def test_year_format(self): + for car in self.all_cars: + with self.subTest(car=car): + self.assertIsNone(re.search(r"\d{4}-\d{4}", car.name), f"Format years correctly: {car.name}") + + def test_harnesses(self): + for car in self.all_cars: + with self.subTest(car=car): + if car.name == "comma body": + raise unittest.SkipTest + + self.assertNotIn(car.harness, [None, Harness.none], f"Need to specify car harness: {car.name}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/tests/test_fingerprints.py b/selfdrive/car/tests/test_fingerprints.py new file mode 100755 index 00000000000000..26ade29e4a507b --- /dev/null +++ b/selfdrive/car/tests/test_fingerprints.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import os +import sys +from typing import Dict, List + +from common.basedir import BASEDIR + +# messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can) +# (addr, len) +CAN_IGNITION_MSGS = { + 'gm': [(0x1F1, 8), (0x160, 5)], + #'tesla' : [(0x348, 8)], +} + +def _get_fingerprints(): + # read all the folders in selfdrive/car and return a dict where: + # - keys are all the car names that which we have a fingerprint dict for + # - values are dicts of fingeprints for each trim + fingerprints = {} + for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: + car_name = car_folder.split('/')[-1] + try: + fingerprints[car_name] = __import__(f'selfdrive.car.{car_name}.values', fromlist=['FINGERPRINTS']).FINGERPRINTS + except (ImportError, OSError, AttributeError): + pass + + return fingerprints + + +def check_fingerprint_consistency(f1, f2): + # return false if it finds a fingerprint fully included in another + # max message worth checking is 1800, as above that they usually come too infrequently and not + # usable for fingerprinting + + max_msg = 1800 + + is_f1_in_f2 = True + for k in f1: + if (k not in f2 or f1[k] != f2[k]) and k < max_msg: + is_f1_in_f2 = False + + is_f2_in_f1 = True + for k in f2: + if (k not in f1 or f2[k] != f1[k]) and k < max_msg: + is_f2_in_f1 = False + + return not is_f1_in_f2 and not is_f2_in_f1 + + +def check_can_ignition_conflicts(fingerprints, brands): + # loops through all the fingerprints and exits if CAN ignition dedicated messages + # are found in unexpected fingerprints + + for brand_can, msgs_can in CAN_IGNITION_MSGS.items(): + for i, f in enumerate(fingerprints): + for msg_can in msgs_can: + if brand_can != brands[i] and msg_can[0] in f and msg_can[1] == f[msg_can[0]]: + print("CAN ignition dedicated msg %d with len %d found in %s fingerprints!" % (msg_can[0], msg_can[1], brands[i])) + print("TEST FAILED") + sys.exit(1) + + + +if __name__ == "__main__": + fingerprints = _get_fingerprints() + + fingerprints_flat: List[Dict] = [] + car_names = [] + brand_names = [] + for brand in fingerprints: + for car in fingerprints[brand]: + fingerprints_flat += fingerprints[brand][car] + for i in range(len(fingerprints[brand][car])): + car_names.append(car) + brand_names.append(brand) + + # first check if CAN ignition specific messages are unexpectedly included in other fingerprints + check_can_ignition_conflicts(fingerprints_flat, brand_names) + + valid = True + for idx1, f1 in enumerate(fingerprints_flat): + for idx2, f2 in enumerate(fingerprints_flat): + if idx1 < idx2 and not check_fingerprint_consistency(f1, f2): + valid = False + print(f"Those two fingerprints are inconsistent {car_names[idx1]} {car_names[idx2]}") + print("") + print(', '.join("%d: %d" % v for v in sorted(f1.items()))) + print("") + print(', '.join("%d: %d" % v for v in sorted(f2.items()))) + print("") + + print(f"Found {len(fingerprints_flat)} individual fingerprints") + if not valid or len(fingerprints_flat) == 0: + print("TEST FAILED") + sys.exit(1) + else: + print("TEST SUCCESSFUL") diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py new file mode 100755 index 00000000000000..f0d2744a982880 --- /dev/null +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +import random +import unittest +from parameterized import parameterized + +from cereal import car +from selfdrive.car.car_helpers import get_interface_attr, interfaces +from selfdrive.car.fingerprints import FW_VERSIONS +from selfdrive.car.fw_versions import FW_QUERY_CONFIGS, match_fw_to_car + +CarFw = car.CarParams.CarFw +Ecu = car.CarParams.Ecu + +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} +VERSIONS = get_interface_attr("FW_VERSIONS", ignore_none=True) + + +class TestFwFingerprint(unittest.TestCase): + def assertFingerprints(self, candidates, expected): + candidates = list(candidates) + self.assertEqual(len(candidates), 1, f"got more than one candidate: {candidates}") + self.assertEqual(candidates[0], expected) + + @parameterized.expand([(b, c, e[c]) for b, e in VERSIONS.items() for c in e]) + def test_fw_fingerprint(self, brand, car_model, ecus): + CP = car.CarParams.new_message() + for _ in range(200): + fw = [] + for ecu, fw_versions in ecus.items(): + if not len(fw_versions): + raise unittest.SkipTest("Car model has no FW versions") + ecu_name, addr, sub_addr = ecu + fw.append({"ecu": ecu_name, "fwVersion": random.choice(fw_versions), 'brand': brand, + "address": addr, "subAddress": 0 if sub_addr is None else sub_addr}) + CP.carFw = fw + _, matches = match_fw_to_car(CP.carFw) + self.assertFingerprints(matches, car_model) + + def test_no_duplicate_fw_versions(self): + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model): + for ecu, ecu_fw in ecus.items(): + with self.subTest(ecu): + duplicates = {fw for fw in ecu_fw if ecu_fw.count(fw) > 1} + self.assertFalse(len(duplicates), f"{car_model}: Duplicate FW versions: Ecu.{ECU_NAME[ecu[0]]}, {duplicates}") + + def test_blacklisted_ecus(self): + blacklisted_addrs = (0x7c4, 0x7d0) # includes A/C ecu and an unknown ecu + for car_model, ecus in FW_VERSIONS.items(): + with self.subTest(car_model=car_model): + CP = interfaces[car_model][0].get_params(car_model) + if CP.carName == 'subaru': + for ecu in ecus.keys(): + self.assertNotIn(ecu[1], blacklisted_addrs, f'{car_model}: Blacklisted ecu: (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])})') + + elif CP.carName == "chrysler": + # Some HD trucks have a combined TCM and ECM + if CP.carFingerprint.startswith("RAM HD"): + for ecu in ecus.keys(): + self.assertNotEqual(ecu[0], Ecu.transmission, f"{car_model}: Blacklisted ecu: (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])})") + + def test_missing_versions_and_configs(self): + brand_versions = set(VERSIONS.keys()) + brand_configs = set(FW_QUERY_CONFIGS.keys()) + if len(brand_configs - brand_versions): + with self.subTest(): + self.fail(f"Brands do not implement FW_VERSIONS: {brand_configs - brand_versions}") + + if len(brand_versions - brand_configs): + with self.subTest(): + self.fail(f"Brands do not implement FW_QUERY_CONFIG: {brand_versions - brand_configs}") + + def test_fw_request_ecu_whitelist(self): + for brand, config in FW_QUERY_CONFIGS.items(): + with self.subTest(brand=brand): + whitelisted_ecus = set([ecu for r in config.requests for ecu in r.whitelist_ecus]) + brand_ecus = set([fw[0] for car_fw in VERSIONS[brand].values() for fw in car_fw]) + + # each ecu in brand's fw versions needs to be whitelisted at least once + ecus_not_whitelisted = brand_ecus - whitelisted_ecus + + ecu_strings = ", ".join([f'Ecu.{ECU_NAME[ecu]}' for ecu in ecus_not_whitelisted]) + self.assertFalse(len(whitelisted_ecus) and len(ecus_not_whitelisted), + f'{brand.title()}: FW query whitelist missing ecus: {ecu_strings}') + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py old mode 100644 new mode 100755 index 94f5b332319ebc..fa07db92db86df --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -1,170 +1,120 @@ -import time +#!/usr/bin/env python3 +# pylint: disable=E1101 import os -import pytest -import random -import unittest # noqa: TID251 +import importlib +import unittest from collections import defaultdict, Counter -import hypothesis.strategies as st -from hypothesis import Phase, given, settings +from typing import List, Optional, Tuple from parameterized import parameterized_class -from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs -from opendbc.car.can_definitions import CanData -from opendbc.car.car_helpers import FRAME_FINGERPRINT, interfaces -from opendbc.car.fingerprints import MIGRATION -from opendbc.car.honda.values import CAR as HONDA, HondaFlags -from opendbc.car.structs import car -from opendbc.car.tests.routes import non_tested_cars, routes, CarTestRoute -from opendbc.car.values import Platform, PLATFORMS -from opendbc.safety.tests.libsafety import libsafety_py -from openpilot.common.basedir import BASEDIR -from openpilot.selfdrive.pandad import can_capnp_to_list -from openpilot.selfdrive.test.helpers import read_segment_list -from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT -from openpilot.tools.lib.logreader import LogReader, LogsUnavailable, openpilotci_source, internal_source, comma_api_source -from openpilot.tools.lib.route import SegmentName - -SafetyModel = car.CarParams.SafetyModel -SteerControlType = structs.CarParams.SteerControlType +from cereal import log, car +from common.realtime import DT_CTRL +from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp +from selfdrive.car.fingerprints import all_known_cars +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.gm.values import CAR as GM +from selfdrive.car.honda.values import CAR as HONDA, HONDA_BOSCH +from selfdrive.car.hyundai.values import CAR as HYUNDAI +from selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute +from selfdrive.test.openpilotci import get_url +from tools.lib.logreader import LogReader +from tools.lib.route import Route + +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import package_can_msg + +PandaType = log.PandaState.PandaType NUM_JOBS = int(os.environ.get("NUM_JOBS", "1")) JOB_ID = int(os.environ.get("JOB_ID", "0")) -INTERNAL_SEG_LIST = os.environ.get("INTERNAL_SEG_LIST", "") -INTERNAL_SEG_CNT = int(os.environ.get("INTERNAL_SEG_CNT", "0")) -MAX_EXAMPLES = int(os.environ.get("MAX_EXAMPLES", "300")) -CI = os.environ.get("CI", None) is not None - - -def get_test_cases() -> list[tuple[str, CarTestRoute | None]]: - # build list of test cases - test_cases = [] - if not len(INTERNAL_SEG_LIST): - routes_by_car = defaultdict(set) - for r in routes: - routes_by_car[str(r.car_model)].add(r) - - for i, c in enumerate(sorted(PLATFORMS)): - if i % NUM_JOBS == JOB_ID: - test_cases.extend(sorted((c, r) for r in routes_by_car.get(c, (None,)))) - - else: - segment_list = read_segment_list(os.path.join(BASEDIR, INTERNAL_SEG_LIST)) - segment_list = random.sample(segment_list, INTERNAL_SEG_CNT or len(segment_list)) - for platform, segment in segment_list: - platform = MIGRATION.get(platform, platform) - segment_name = SegmentName(segment) - test_cases.append((platform, CarTestRoute(segment_name.route_name.canonical_name, platform, - segment=segment_name.segment_num))) - return test_cases - - -@pytest.mark.slow -@pytest.mark.shared_download_cache -class TestCarModelBase(unittest.TestCase): - platform: Platform | None = None - test_route: CarTestRoute | None = None - - can_msgs: list[tuple[int, list[CanData]]] - fingerprint: dict[int, dict[int, int]] - elm_frame: int | None - car_safety_mode_frame: int | None - @classmethod - def get_testing_data_from_logreader(cls, lr): - car_fw = [] - can_msgs = [] - cls.elm_frame = None - cls.car_safety_mode_frame = None - cls.fingerprint = gen_empty_fingerprint() - alpha_long = False - for msg in lr: - if msg.which() == "can": - can = can_capnp_to_list((msg.as_builder().to_bytes(),))[0] - can_msgs.append((can[0], [CanData(*can) for can in can[1]])) - if len(can_msgs) <= FRAME_FINGERPRINT: - for m in msg.can: - if m.src < 64: - cls.fingerprint[m.src][m.address] = len(m.dat) - - elif msg.which() == "carParams": - car_fw = msg.carParams.carFw - if msg.carParams.openpilotLongitudinalControl: - alpha_long = True - if cls.platform is None: - live_fingerprint = msg.carParams.carFingerprint - cls.platform = MIGRATION.get(live_fingerprint, live_fingerprint) - - # Log which can frame the panda safety mode left ELM327, for CAN validity checks - elif msg.which() == 'pandaStates': - for ps in msg.pandaStates: - if cls.elm_frame is None and ps.safetyModel != SafetyModel.elm327: - cls.elm_frame = len(can_msgs) - if cls.car_safety_mode_frame is None and ps.safetyModel not in \ - (SafetyModel.elm327, SafetyModel.noOutput): - cls.car_safety_mode_frame = len(can_msgs) - - elif msg.which() == 'pandaStateDEPRECATED': - if cls.elm_frame is None and msg.pandaStateDEPRECATED.safetyModel != SafetyModel.elm327: - cls.elm_frame = len(can_msgs) - if cls.car_safety_mode_frame is None and msg.pandaStateDEPRECATED.safetyModel not in \ - (SafetyModel.elm327, SafetyModel.noOutput): - cls.car_safety_mode_frame = len(can_msgs) - - assert len(can_msgs) > int(50 / DT_CTRL), "no can data found" - return car_fw, can_msgs, alpha_long +ignore_addr_checks_valid = [ + GM.BUICK_REGAL, + HYUNDAI.GENESIS_G70_2020, +] - @classmethod - def get_testing_data(cls): - test_segs = (2, 1, 0) - if cls.test_route.segment is not None: - test_segs = (cls.test_route.segment,) +# build list of test cases +routes_by_car = defaultdict(set) +for r in routes: + routes_by_car[r.car_model].add(r) - for seg in test_segs: - segment_range = f"{cls.test_route.route}/{seg}" +test_cases: List[Tuple[str, Optional[CarTestRoute]]] = [] +for i, c in enumerate(sorted(all_known_cars())): + if i % NUM_JOBS == JOB_ID: + test_cases.extend((c, r) for r in routes_by_car.get(c, (None, ))) - try: - sources = [internal_source] if len(INTERNAL_SEG_LIST) else [openpilotci_source, comma_api_source] - lr = LogReader(segment_range, sources=sources, sort_by_time=True) - return cls.get_testing_data_from_logreader(lr) - except (LogsUnavailable, AssertionError): - pass +SKIP_ENV_VAR = "SKIP_LONG_TESTS" - raise Exception(f"Route: {repr(cls.test_route.route)} with segments: {test_segs} not found or no CAN msgs found. Is it uploaded and public?") +class TestCarModelBase(unittest.TestCase): + car_model = None + test_route = None + ci = True + @unittest.skipIf(SKIP_ENV_VAR in os.environ, f"Long running test skipped. Unset {SKIP_ENV_VAR} to run") @classmethod def setUpClass(cls): if cls.__name__ == 'TestCarModel' or cls.__name__.endswith('Base'): raise unittest.SkipTest + if 'FILTER' in os.environ: + if not cls.car_model.startswith(tuple(os.environ.get('FILTER').split(','))): + raise unittest.SkipTest + if cls.test_route is None: - if cls.platform in non_tested_cars: - print(f"Skipping tests for {cls.platform}: missing route") + if cls.car_model in non_tested_cars: + print(f"Skipping tests for {cls.car_model}: missing route") raise unittest.SkipTest - raise Exception(f"missing test route for {cls.platform}") + raise Exception(f"missing test route for {cls.car_model}") - car_fw, cls.can_msgs, alpha_long = cls.get_testing_data() + experimental_long = False + test_segs = (2, 1, 0) + if cls.test_route.segment is not None: + test_segs = (cls.test_route.segment,) - # if relay is expected to be open in the route - cls.openpilot_enabled = cls.car_safety_mode_frame is not None + for seg in test_segs: + try: + if cls.ci: + lr = LogReader(get_url(cls.test_route.route, seg)) + else: + lr = LogReader(Route(cls.test_route.route).log_paths()[seg]) + except Exception: + continue - cls.CarInterface = interfaces[cls.platform] - cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) + car_fw = [] + can_msgs = [] + fingerprint = defaultdict(dict) + for msg in lr: + if msg.which() == "can": + for m in msg.can: + if m.src < 64: + fingerprint[m.src][m.address] = len(m.dat) + can_msgs.append(msg) + elif msg.which() == "carParams": + car_fw = msg.carParams.carFw + if msg.carParams.openpilotLongitudinalControl: + experimental_long = True + if cls.car_model is None and not cls.ci: + cls.car_model = msg.carParams.carFingerprint + + if len(can_msgs) > int(50 / DT_CTRL): + break + else: + raise Exception(f"Route: {repr(cls.test_route.route)} with segments: {test_segs} not found or no CAN msgs found. Is it uploaded?") + + cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime) + + cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model] + cls.CP = cls.CarInterface.get_params(cls.car_model, fingerprint, car_fw, experimental_long) assert cls.CP - assert cls.CP.carFingerprint == cls.platform - - os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT - - @classmethod - def tearDownClass(cls): - del cls.can_msgs + assert cls.CP.carFingerprint == cls.car_model def setUp(self): - self.CI = self.CarInterface(self.CP.copy()) + self.CI = self.CarInterface(self.CP, self.CarController, self.CarState) assert self.CI # TODO: check safetyModel is in release panda build - self.safety = libsafety_py.libsafety + self.safety = libpandasafety_py.libpandasafety cfg = self.CP.safetyConfigs[-1] set_status = self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam) @@ -178,205 +128,77 @@ def test_car_params(self): # make sure car params are within a valid range self.assertGreater(self.CP.mass, 1) - if self.CP.steerControlType != SteerControlType.angle: + if self.CP.steerControlType != car.CarParams.SteerControlType.angle: tuning = self.CP.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) elif tuning == 'torque': - self.assertTrue(self.CP.lateralTuning.torque.latAccelFactor > 0) + self.assertTrue(self.CP.lateralTuning.torque.kf > 0) + elif tuning == 'indi': + self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) else: raise Exception("unknown tuning") def test_car_interface(self): # TODO: also check for checksum violations from can parser can_invalid_cnt = 0 - CC = structs.CarControl().as_reader() + can_valid = False + CC = car.CarControl.new_message() for i, msg in enumerate(self.can_msgs): - CS = self.CI.update(msg) - self.CI.apply(CC, msg[0]) + CS = self.CI.update(CC, (msg.as_builder().to_bytes(),)) + self.CI.apply(CC) + + if CS.canValid: + can_valid = True # wait max of 2s for low frequency msgs to be seen - if i > 250: + if i > 200 or can_valid: can_invalid_cnt += not CS.canValid self.assertEqual(can_invalid_cnt, 0) def test_radar_interface(self): - RI = self.CarInterface.RadarInterface(self.CP) + os.environ['NO_RADAR_SLEEP'] = "1" + RadarInterface = importlib.import_module(f'selfdrive.car.{self.CP.carName}.radar_interface').RadarInterface + RI = RadarInterface(self.CP) assert RI - # Since OBD port is multiplexed to bus 1 (commonly radar bus) while fingerprinting, - # start parsing CAN messages after we've left ELM mode and can expect CAN traffic error_cnt = 0 - for i, msg in enumerate(self.can_msgs[self.elm_frame:]): - rr: structs.RadarData | None = RI.update(msg) + for i, msg in enumerate(self.can_msgs): + rr = RI.update((msg.as_builder().to_bytes(),)) if rr is not None and i > 50: - error_cnt += rr.errors.canError + error_cnt += car.RadarData.Error.canError in rr.errors self.assertEqual(error_cnt, 0) - def test_panda_safety_rx_checks(self): + def test_panda_safety_rx_valid(self): if self.CP.dashcamOnly: self.skipTest("no need to check panda safety for dashcamOnly") - start_ts = self.can_msgs[0][0] + start_ts = self.can_msgs[0].logMonoTime failed_addrs = Counter() for can in self.can_msgs: # update panda timer - t = (can[0] - start_ts) / 1e3 + t = (can.logMonoTime - start_ts) / 1e3 self.safety.set_timer(int(t)) # run all msgs through the safety RX hook - for msg in can[1]: + for msg in can.can: if msg.src >= 64: continue - to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) + to_send = package_can_msg([msg.address, 0, msg.dat, msg.src % 4]) if self.safety.safety_rx_hook(to_send) != 1: failed_addrs[hex(msg.address)] += 1 # ensure all msgs defined in the addr checks are valid - self.safety.safety_tick_current_safety_config() - if t > 1e6: - self.assertTrue(self.safety.safety_config_valid()) - - # Don't check relay malfunction on disabled routes (relay closed), - # or before fingerprinting is done (elm327 and noOutput) - if self.openpilot_enabled and t / 1e4 > self.car_safety_mode_frame: - self.assertFalse(self.safety.get_relay_malfunction()) - else: - self.safety.set_relay_malfunction(False) - + if self.car_model not in ignore_addr_checks_valid: + self.safety.safety_tick_current_rx_checks() + if t > 1e6: + self.assertTrue(self.safety.addr_checks_valid()) self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}") - # ensure RX checks go invalid after small time with no traffic - self.safety.set_timer(int(t + (2*1e6))) - self.safety.safety_tick_current_safety_config() - self.assertFalse(self.safety.safety_config_valid()) - - def test_panda_safety_tx_cases(self, data=None): - """Asserts we can tx common messages""" - if self.CP.dashcamOnly: - self.skipTest("no need to check panda safety for dashcamOnly") - - if self.CP.notCar: - self.skipTest("Skipping test for notCar") - - def test_car_controller(car_control): - now_nanos = 0 - msgs_sent = 0 - CI = self.CarInterface(self.CP) - for _ in range(round(10.0 / DT_CTRL)): # make sure we hit the slowest messages - CI.update([]) - _, sendcan = CI.apply(car_control, now_nanos) - - now_nanos += DT_CTRL * 1e9 - msgs_sent += len(sendcan) - for addr, dat, bus in sendcan: - to_send = libsafety_py.make_CANPacket(addr, bus % 4, dat) - self.assertTrue(self.safety.safety_tx_hook(to_send), (addr, dat, bus)) - - # Make sure we attempted to send messages - self.assertGreater(msgs_sent, 50) - - # Make sure we can send all messages while inactive - CC = structs.CarControl() - test_car_controller(CC.as_reader()) - - # Test cancel + general messages (controls_allowed=False & cruise_engaged=True) - self.safety.set_cruise_engaged_prev(True) - CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(cancel=True)) - test_car_controller(CC.as_reader()) - - # Test resume + general messages (controls_allowed=True & cruise_engaged=True) - self.safety.set_controls_allowed(True) - CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(resume=True)) - test_car_controller(CC.as_reader()) - - # Skip stdout/stderr capture with pytest, causes elevated memory usage - @pytest.mark.nocapture - @settings(max_examples=MAX_EXAMPLES, deadline=None, - phases=(Phase.reuse, Phase.generate, Phase.shrink)) - @given(data=st.data()) - def test_panda_safety_carstate_fuzzy(self, data): - """ - For each example, pick a random CAN message on the bus and fuzz its data, - checking for panda state mismatches. - """ - - if self.CP.dashcamOnly: - self.skipTest("no need to check panda safety for dashcamOnly") - - valid_addrs = [(addr, bus, size) for bus, addrs in self.fingerprint.items() for addr, size in addrs.items()] - address, bus, size = data.draw(st.sampled_from(valid_addrs)) - - msg_strategy = st.binary(min_size=size, max_size=size) - msgs = data.draw(st.lists(msg_strategy, min_size=20)) - - vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar - - for n, dat in enumerate(msgs): - # due to panda updating state selectively, only edges are expected to match - # TODO: warm up CarState with real CAN messages to check edge of both sources - # (eg. toyota's gasPressed is the inverse of a signal being set) - prev_panda_gas = self.safety.get_gas_pressed_prev() - prev_panda_brake = self.safety.get_brake_pressed_prev() - prev_panda_regen_braking = self.safety.get_regen_braking_prev() - prev_panda_steering_disengage = self.safety.get_steering_disengage_prev() - prev_panda_vehicle_moving = self.safety.get_vehicle_moving() - prev_panda_vehicle_speed_min = self.safety.get_vehicle_speed_min() - prev_panda_vehicle_speed_max = self.safety.get_vehicle_speed_max() - prev_panda_cruise_engaged = self.safety.get_cruise_engaged_prev() - prev_panda_acc_main_on = self.safety.get_acc_main_on() - - to_send = libsafety_py.make_CANPacket(address, bus, dat) - self.safety.safety_rx_hook(to_send) - - can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])] - CS = self.CI.update(can) - if n < 5: # CANParser warmup time - continue - - if self.safety.get_gas_pressed_prev() != prev_panda_gas: - self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev()) - - if self.safety.get_brake_pressed_prev() != prev_panda_brake: - # TODO: remove this exception once this mismatch is resolved - brake_pressed = CS.brakePressed - if CS.brakePressed and not self.safety.get_brake_pressed_prev(): - if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05: - brake_pressed = False - - self.assertEqual(brake_pressed, self.safety.get_brake_pressed_prev()) - - if self.safety.get_regen_braking_prev() != prev_panda_regen_braking: - self.assertEqual(CS.regenBraking, self.safety.get_regen_braking_prev()) - - if self.safety.get_steering_disengage_prev() != prev_panda_steering_disengage: - self.assertEqual(CS.steeringDisengage, self.safety.get_steering_disengage_prev()) - - if self.safety.get_vehicle_moving() != prev_panda_vehicle_moving and not self.CP.notCar: - self.assertEqual(not CS.standstill, self.safety.get_vehicle_moving()) - - # check vehicle speed if angle control car or available - if self.safety.get_vehicle_speed_min() > 0 or self.safety.get_vehicle_speed_max() > 0: - vehicle_speed_seen = True - - if vehicle_speed_seen and (self.safety.get_vehicle_speed_min() != prev_panda_vehicle_speed_min or - self.safety.get_vehicle_speed_max() != prev_panda_vehicle_speed_max): - v_ego_raw = CS.vEgoRaw / self.CP.wheelSpeedFactor - self.assertFalse(v_ego_raw > (self.safety.get_vehicle_speed_max() + 1e-3) or - v_ego_raw < (self.safety.get_vehicle_speed_min() - 1e-3)) - - if not (self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH)): - if self.safety.get_cruise_engaged_prev() != prev_panda_cruise_engaged: - self.assertEqual(CS.cruiseState.enabled, self.safety.get_cruise_engaged_prev()) - - if self.CP.brand == "honda": - if self.safety.get_acc_main_on() != prev_panda_acc_main_on: - self.assertEqual(CS.cruiseState.available, self.safety.get_acc_main_on()) - def test_panda_safety_carstate(self): """ Assert that panda safety matches openpilot's carState @@ -384,79 +206,65 @@ def test_panda_safety_carstate(self): if self.CP.dashcamOnly: self.skipTest("no need to check panda safety for dashcamOnly") + CC = car.CarControl.new_message() + # warm up pass, as initial states may be different for can in self.can_msgs[:300]: - self.CI.update(can) - for msg in filter(lambda m: m.src < 64, can[1]): - to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + to_send = package_can_msg(msg) self.safety.safety_rx_hook(to_send) + self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) + + if not self.CP.pcmCruise: + self.safety.set_controls_allowed(0) controls_allowed_prev = False CS_prev = car.CarState.new_message() - checks = defaultdict(int) - vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar - for idx, can in enumerate(self.can_msgs): - CS = self.CI.update(can).as_reader() - for msg in filter(lambda m: m.src < 64, can[1]): - to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) + checks = defaultdict(lambda: 0) + for can in self.can_msgs: + CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + msg = list(msg) + msg[3] %= 4 + to_send = package_can_msg(msg) ret = self.safety.safety_rx_hook(to_send) - self.assertEqual(1, ret, f"safety rx failed ({ret=}): {(msg.address, msg.src % 4)}") - - # Skip first frame so CS_prev is properly initialized - if idx == 0: - CS_prev = CS - # Button may be left pressed in warm up period - if not self.CP.pcmCruise: - self.safety.set_controls_allowed(0) - continue + self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") # TODO: check rest of panda's carstate (steering, ACC main on, etc.) checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() - checks['standstill'] += (CS.standstill == self.safety.get_vehicle_moving()) and not self.CP.notCar - - # check vehicle speed if angle control car or available - if self.safety.get_vehicle_speed_min() > 0 or self.safety.get_vehicle_speed_max() > 0: - vehicle_speed_seen = True - - if vehicle_speed_seen: - v_ego_raw = CS.vEgoRaw / self.CP.wheelSpeedFactor - checks['vEgoRaw'] += (v_ego_raw > (self.safety.get_vehicle_speed_max() + 1e-3) or - v_ego_raw < (self.safety.get_vehicle_speed_min() - 1e-3)) + checks['cruiseState'] += CS.cruiseState.enabled and not CS.cruiseState.available + if self.CP.carName in ("honda", "toyota"): + # TODO: fix standstill mismatches for other makes + checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving() # TODO: remove this exception once this mismatch is resolved brake_pressed = CS.brakePressed if CS.brakePressed and not self.safety.get_brake_pressed_prev(): - if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05: + if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05: brake_pressed = False checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() - checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev() - checks['steeringDisengage'] += CS.steeringDisengage != self.safety.get_steering_disengage_prev() if self.CP.pcmCruise: # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state. # On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but # openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages). - if self.CP.brand == "honda" and not (self.CP.flags & HondaFlags.BOSCH): + if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH: # only the rising edges are expected to match if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled: checks['controlsAllowed'] += not self.safety.get_controls_allowed() else: checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed() - - # TODO: fix notCar mismatch - if not self.CP.notCar: - checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev() else: - # Check for user button enable on rising edge of controls allowed - button_enable = CS.buttonEnable and (not CS.brakePressed or CS.standstill) + # Check for enable events on rising edge of controls allowed + button_enable = any(evt.enable for evt in CS.events) mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev) checks['controlsAllowed'] += mismatch controls_allowed_prev = self.safety.get_controls_allowed() if button_enable and not mismatch: self.safety.set_controls_allowed(False) - if self.CP.brand == "honda": + if self.CP.carName == "honda": checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on() CS_prev = CS @@ -465,8 +273,7 @@ def test_panda_safety_carstate(self): self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}") -@parameterized_class(('platform', 'test_route'), get_test_cases()) -@pytest.mark.xdist_group_class_property('test_route') +@parameterized_class(('car_model', 'test_route'), test_cases) class TestCarModel(TestCarModelBase): pass diff --git a/selfdrive/car/tests/test_models_segs.txt b/selfdrive/car/tests/test_models_segs.txt deleted file mode 100644 index c983fb08e7f009..00000000000000 --- a/selfdrive/car/tests/test_models_segs.txt +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0810a361ec5b5f5f9a2ee73b89ffb2df62ef40e8feff7e97ecb62f80fa53f6f5 -size 124950 diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml new file mode 100644 index 00000000000000..fcc762c2b85750 --- /dev/null +++ b/selfdrive/car/torque_data/override.yaml @@ -0,0 +1,37 @@ +legend: [LAT_ACCEL_FACTOR, MAX_LAT_ACCEL_MEASURED, FRICTION] +### angle control +# Nissan appears to have torque +NISSAN X-TRAIL 2017: [.nan, 1.5, .nan] +NISSAN ALTIMA 2020: [.nan, 1.5, .nan] +NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan] +NISSAN LEAF 2018: [.nan, 1.5, .nan] +NISSAN ROGUE 2019: [.nan, 1.5, .nan] + +# Tesla has high torque +TESLA AP1 MODEL S: [.nan, 2.5, .nan] +TESLA AP2 MODEL S: [.nan, 2.5, .nan] + +# Guess +FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] +FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] +FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] +### + +# No steering wheel +COMMA BODY: [.nan, 1000, .nan] + +# Totally new cars +KIA EV6 2022: [3.5, 2.5, 0.0] +RAM 1500 5TH GEN: [2.0, 2.0, 0.0] +RAM HD 5TH GEN: [1.4, 1.4, 0.0] +SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11] +CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] +CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112] +VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1] +HYUNDAI TUCSON HYBRID 4TH GEN: [2.5, 2.5, 0.0] + +# Dashcam or fallback configured as ideal car +mock: [10.0, 10, 0.0] + +# Manually checked +HONDA CIVIC 2022: [2.5, 1.2, 0.15] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml new file mode 100644 index 00000000000000..160f605488b7f7 --- /dev/null +++ b/selfdrive/car/torque_data/params.yaml @@ -0,0 +1,96 @@ +ACURA ILX 2016: [1.524988973896102, 0.519011053086259, 0.34236219253028] +ACURA RDX 2018: [0.9987728568686902, 0.5323765166196301, 0.303218805715844] +ACURA RDX 2020: [1.4314459806646749, 0.33874701282109954, 0.18048847083897598] +AUDI A3 3RD GEN: [1.5122414863077502, 1.7443517531719404, 0.15194151892450905] +AUDI Q3 2ND GEN: [1.4439223359448605, 1.2254955789112076, 0.1413798895978097] +CHEVROLET VOLT PREMIER 2017: [1.5961527626411784, 1.8422651988094612, 0.1572393918005158] +CHRYSLER PACIFICA 2018: [1.593387270257916, 1.3366521181047952, 0.13776367250652022] +CHRYSLER PACIFICA 2020: [1.4323553627965695, 1.509076559398423, 0.14328246159386085] +CHRYSLER PACIFICA HYBRID 2017: [1.3032470208409048, 1.06831764583744, 0.13287170990024627] +CHRYSLER PACIFICA HYBRID 2018: [1.6068280248761635, 1.2943025830995154, 0.1358557824293823] +CHRYSLER PACIFICA HYBRID 2019: [1.4624643614072217, 1.1958788168371808, 0.15748488008472716] +GENESIS G70 2018: [3.8520195946707947, 2.354697063349854, 0.06830285485626221] +GMC ACADIA DENALI 2018: [1.3181430320331884, 1.1853735340610179, 0.3450592280031644] +HONDA ACCORD 2018: [1.7135052593468778, 0.3461280068322071, 0.21579936052863807] +HONDA ACCORD HYBRID 2018: [1.6651615004829625, 0.30322180951193245, 0.2083000440586149] +HONDA CIVIC (BOSCH) 2019: [1.691708637466905, 0.40132900729454185, 0.25460295304024094] +HONDA CIVIC 2016: [1.6528895627785531, 0.4018518740819229, 0.25458812851328544] +HONDA CR-V 2016: [0.7667141440182675, 0.5927571534745969, 0.40909087636157127] +HONDA CR-V 2017: [2.01323205142022, 0.2700612209345081, 0.2238412881331528] +HONDA CR-V HYBRID 2019: [2.072034634644233, 0.7152085160516978, 0.20237105008376083] +HONDA FIT 2018: [1.5719981427109775, 0.5712761407108976, 0.110773383324281] +HONDA HRV 2019: [2.0661212805710205, 0.7521343418694775, 0.17760375789242094] +HONDA INSIGHT 2019: [1.5201671214069354, 0.5660229120683284, 0.25808042580281876] +HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] +HONDA PASSPORT 2021: [1.5305538930036766, 0.7956063674638759, 0.19599407381531284] +HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] +HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] +HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171] +HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] +HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] +HYUNDAI IONIQ PLUG-IN HYBRID 2019: [2.970807902012267, 1.6312321830002083, 0.1088964990357482] +HYUNDAI KONA ELECTRIC 2019: [4.398306735170212, 3.2961956260770484, 0.08651833437845884] +HYUNDAI PALISADE 2020: [2.544642494803999, 1.8721703683337008, 0.1301424599248651] +HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.1207019341823945] +HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963] +HYUNDAI SANTA FE PlUG-IN HYBRID 2022: [1.6953050513611045, 1.5837614296206861, 0.12672855941458458] +HYUNDAI SONATA 2019: [2.2200457811703953, 1.2967330275895228, 0.14039920986586393] +HYUNDAI SONATA 2020: [3.284505627881726, 2.1259108157250735, 0.08452048323586728] +HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382] +JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185] +JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003] +KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716] +KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] +KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558] +KIA STINGER GT2 2018: [2.7499043387418967, 1.849652021986449, 0.12048334239559202] +LEXUS ES 2019: [2.0203086922726112, 2.134803912579666, 0.12757526789308554] +LEXUS ES HYBRID 2019: [2.392442298703042, 1.863360677810788, 0.17690002108856212] +LEXUS NX 2018: [2.302625600642627, 2.1382378491466625, 0.14986840878892838] +LEXUS NX 2020: [2.4331999786982936, 2.1045680431705414, 0.14099899317761067] +LEXUS NX HYBRID 2018: [2.4025593501080955, 1.8080446063815507, 0.15349361249519017] +LEXUS RX 2016: [1.5876816543130423, 1.0427699298523752, 0.21334066732397142] +LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.2093316728710659] +LEXUS RX HYBRID 2017: [1.6984261557042386, 1.3211501880159107, 0.1820354534928893] +LEXUS RX HYBRID 2020: [1.5522309889823778, 1.255230465866663, 0.2220954003055114] +MAZDA CX-9 2021: [1.7601682915983443, 1.0889677335154337, 0.17713792194297195] +SKODA SUPERB 3RD GEN: [1.166437404652981, 1.1686163012668165, 0.12194533036948708] +SUBARU FORESTER 2019: [3.6617001649776793, 2.342197172531713, 0.11075960785398745] +SUBARU IMPREZA LIMITED 2019: [1.0670704910352047, 0.8234374840709592, 0.20986563268614938] +SUBARU IMPREZA SPORT 2020: [2.6068223389108303, 2.134872342760203, 0.15261513193561627] +TOYOTA AVALON 2016: [2.5185770183845646, 1.7153346784214922, 0.10603968787111022] +TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492] +TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605] +TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193] +TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509] +TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.13519250664782062] +TOYOTA CAMRY 2021: [2.6922769557433055, 2.3476510120007434, 0.1450430192989234] +TOYOTA CAMRY HYBRID 2018: [2.0974120828287774, 1.7996193116697359, 0.13823613467632756] +TOYOTA CAMRY HYBRID 2021: [2.6426668350384457, 2.3901492458927986, 0.16103875108816076] +TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652] +TOYOTA COROLLA HYBRID TSS2 2019: [2.3287672277252005, 1.8118712531729109, 0.2215868445753317] +TOYOTA COROLLA TSS2 2019: [2.4204464833010175, 1.9258612322678952, 0.20670411068012526] +TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796] +TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457] +TOYOTA HIGHLANDER HYBRID 2018: [1.9421825202382728, 1.6433903296845025, 0.16928956792275918] +TOYOTA HIGHLANDER HYBRID 2020: [2.103373061114133, 2.104015182965606, 0.14447040132184993] +TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565] +TOYOTA PRIUS 2017: [2.0183401513314294, 1.5023147650693636, 0.20856908464957724] +TOYOTA PRIUS TSS2 2021: [2.327639738920072, 1.9104337425537743, 0.2030762265549664] +TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975] +TOYOTA RAV4 2019: [2.5038362866776835, 2.0993589721530252, 0.1552425356342368] +TOYOTA RAV4 2019 8965: [2.5084506298290377, 2.4216520504763475, 0.11992835265067918] +TOYOTA RAV4 2019 x02: [2.7209621987605024, 2.2148637653781593, 0.10862567142268198] +TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] +TOYOTA RAV4 HYBRID 2019: [2.2271858492309153, 2.074844961405639, 0.14382216826893632] +TOYOTA RAV4 HYBRID 2019 8965: [2.1077397198131336, 1.8162215166877735, 0.13891369391200137] +TOYOTA RAV4 HYBRID 2019 x02: [2.803624333289342, 2.272367966572498, 0.11364569214387774] +TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.1565442715453653] +TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736] +TOYOTA SIENNA 2018: [1.8660896232147548, 1.3208264576110418, 0.18799149615227198] +VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032] +VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] +VOLKSWAGEN GOLF 7TH GEN: [1.3750394140491293, 1.5814743077200641, 0.2018321939386586] +VOLKSWAGEN JETTA 7TH GEN: [1.2271623034089392, 1.216955117387, 0.19437384688370712] +VOLKSWAGEN PASSAT 8TH GEN: [1.3432120736752917, 1.7087275587362314, 0.19444383787326647] +VOLKSWAGEN TIGUAN 2ND GEN: [0.9711965500094828, 1.0001565939459098, 0.1465626137072916] +legend: [LAT_ACCEL_FACTOR, MAX_LAT_ACCEL_MEASURED, FRICTION] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml new file mode 100644 index 00000000000000..de64a5544c111f --- /dev/null +++ b/selfdrive/car/torque_data/substitute.yaml @@ -0,0 +1,79 @@ +MAZDA 3: MAZDA CX-9 2021 +MAZDA 6: MAZDA CX-9 2021 +MAZDA CX-5: MAZDA CX-9 2021 +MAZDA CX-5 2022: MAZDA CX-9 2021 +MAZDA CX-9: MAZDA CX-9 2021 + +TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018 +TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018 +TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017 +TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022 +TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018 +LEXUS IS 2018: LEXUS NX 2018 +LEXUS CT HYBRID 2018 : LEXUS NX 2018 +LEXUS ES HYBRID 2018: TOYOTA CAMRY HYBRID 2018 +LEXUS NX HYBRID 2020: LEXUS NX 2020 +LEXUS RC 2020: LEXUS NX 2020 +TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019 +TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022 + +KIA OPTIMA SX 2019 & 2016: HYUNDAI SONATA 2020 +KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020 +KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020 +KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020 +KIA SELTOS 2021: HYUNDAI SONATA 2020 +KIA NIRO HYBRID 2019: KIA NIRO EV 2020 +KIA NIRO HYBRID 2021: KIA NIRO EV 2020 +HYUNDAI VELOSTER 2019: HYUNDAI SONATA 2019 +HYUNDAI I30 N LINE 2019 & GT 2018 DCT: HYUNDAI SONATA 2019 +HYUNDAI KONA 2020: HYUNDAI KONA ELECTRIC 2019 +HYUNDAI KONA HYBRID 2020: HYUNDAI KONA ELECTRIC 2019 +HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019 +HYUNDAI IONIQ 5 2022: KIA EV6 2022 +HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019 +HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019 +HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019 +HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019 +HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020 +HYUNDAI ELANTRA 2021: HYUNDAI SONATA 2020 +HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019 +HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 +GENESIS G90 2017: GENESIS G70 2018 +GENESIS G80 2017: GENESIS G70 2018 +GENESIS G70 2020: HYUNDAI SONATA 2020 + +HONDA FREED 2020: HONDA ODYSSEY 2018 +HONDA CR-V EU 2016: HONDA CR-V 2016 +HONDA CIVIC SEDAN 1.6 DIESEL 2019: HONDA CIVIC (BOSCH) 2019 +HONDA E 2020: HONDA CIVIC (BOSCH) 2019 +HONDA ODYSSEY CHN 2019: HONDA ODYSSEY 2018 + +BUICK REGAL ESSENCE 2018: CHEVROLET VOLT PREMIER 2017 +CADILLAC ESCALADE ESV 2016: CHEVROLET VOLT PREMIER 2017 +CADILLAC ATS Premium Performance 2018: CHEVROLET VOLT PREMIER 2017 +CHEVROLET MALIBU PREMIER 2017: CHEVROLET VOLT PREMIER 2017 +HOLDEN ASTRA RS-V BK 2017: CHEVROLET VOLT PREMIER 2017 + +SKODA OCTAVIA 3RD GEN: SKODA SUPERB 3RD GEN +SKODA SCALA 1ST GEN: SKODA SUPERB 3RD GEN +SKODA KODIAQ 1ST GEN: SKODA SUPERB 3RD GEN +SKODA KAROQ 1ST GEN: SKODA SUPERB 3RD GEN +SKODA KAMIQ 1ST GEN: SKODA SUPERB 3RD GEN +VOLKSWAGEN T-ROC 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN +VOLKSWAGEN T-CROSS 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN +VOLKSWAGEN TOURAN 2ND GEN: VOLKSWAGEN TIGUAN 2ND GEN +VOLKSWAGEN TRANSPORTER T6.1: VOLKSWAGEN TIGUAN 2ND GEN +AUDI Q2 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN +VOLKSWAGEN TAOS 1ST GEN: VOLKSWAGEN TIGUAN 2ND GEN +VOLKSWAGEN POLO 6TH GEN: VOLKSWAGEN GOLF 7TH GEN +SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN +SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN + +SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN + +# Old subarus don't have much data guessing it's like low torque impreza +SUBARU OUTBACK 2018 - 2019: SUBARU IMPREZA LIMITED 2019 +SUBARU OUTBACK 2015 - 2017: SUBARU IMPREZA LIMITED 2019 +SUBARU FORESTER 2017 - 2018: SUBARU IMPREZA LIMITED 2019 +SUBARU LEGACY 2015 - 2018: SUBARU IMPREZA LIMITED 2019 +SUBARU ASCENT LIMITED 2019: SUBARU FORESTER 2019 diff --git a/system/sensord/tests/__init__.py b/selfdrive/car/toyota/__init__.py similarity index 100% rename from system/sensord/tests/__init__.py rename to selfdrive/car/toyota/__init__.py diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py new file mode 100644 index 00000000000000..b34a31a01c18a5 --- /dev/null +++ b/selfdrive/car/toyota/carcontroller.py @@ -0,0 +1,164 @@ +from cereal import car +from common.numpy_fast import clip, interp +from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg +from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ + create_accel_command, create_acc_cancel_command, \ + create_fcw_command, create_lta_steer_command +from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ + MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams +from opendbc.can.packer import CANPacker + +VisualAlert = car.CarControl.HUDControl.VisualAlert + +# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long +MAX_STEER_RATE = 100 # deg/s +MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut + +# EPS allows user torque above threshold for 50 frames before permanently faulting +MAX_USER_TORQUE = 500 + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.torque_rate_limits = CarControllerParams(self.CP) + self.frame = 0 + self.last_steer = 0 + self.alert_active = False + self.last_standstill = False + self.standstill_req = False + self.steer_rate_counter = 0 + + self.packer = CANPacker(dbc_name) + self.gas = 0 + self.accel = 0 + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel + lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE + + # gas and brake + if self.CP.enableGasInterceptor and CC.longActive: + MAX_INTERCEPTOR_GAS = 0.5 + # RAV4 has very sensitive gas pedal + if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) + elif self.CP.carFingerprint in (CAR.COROLLA,): + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) + else: + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) + # offset for creep and windbrake + pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) + pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) + interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) + else: + interceptor_gas_cmd = 0. + pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + + # steer torque + new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) + + # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault + if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: + self.steer_rate_counter += 1 + else: + self.steer_rate_counter = 0 + + apply_steer_req = 1 + if not lat_active: + apply_steer = 0 + apply_steer_req = 0 + elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: + apply_steer_req = 0 + self.steer_rate_counter = 0 + + # TODO: probably can delete this. CS.pcm_acc_status uses a different signal + # than CS.cruiseState.enabled. confirm they're not meaningfully different + if not CC.enabled and CS.pcm_acc_status: + pcm_cancel_cmd = 1 + + # on entering standstill, send standstill request + if CS.out.standstill and not self.last_standstill and self.CP.carFingerprint not in NO_STOP_TIMER_CAR: + self.standstill_req = True + if CS.pcm_acc_status != 8: + # pcm entered standstill or it's disabled + self.standstill_req = False + + self.last_steer = apply_steer + self.last_standstill = CS.out.standstill + + can_sends = [] + + # *** control msgs *** + # print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) + + # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; + # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed + # on consecutive messages + can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) + if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: + can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2)) + + # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda + # if self.frame % 2 == 0: + # can_sends.append(create_steer_command(self.packer, 0, 0, self.frame // 2)) + # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2)) + + # we can spam can to cancel the system even if we are using lat only control + if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: + lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + + # Lexus IS uses a different cancellation message + if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + can_sends.append(create_acc_cancel_command(self.packer)) + elif self.CP.openpilotLongitudinalControl: + can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) + self.accel = pcm_accel_cmd + else: + can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) + + if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: + # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) + self.gas = interceptor_gas_cmd + + if self.CP.carFingerprint != CAR.PRIUS_V: + # ui mesg is at 1Hz but we send asap if: + # - there is something to display + # - there is something to stop displaying + fcw_alert = hud_control.visualAlert == VisualAlert.fcw + steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + + send_ui = False + if ((fcw_alert or steer_alert) and not self.alert_active) or \ + (not (fcw_alert or steer_alert) and self.alert_active): + send_ui = True + self.alert_active = not self.alert_active + elif pcm_cancel_cmd: + # forcing the pcm to disengage causes a bad fault sound so play a good sound instead + send_ui = True + + if self.frame % 100 == 0 or send_ui: + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, + hud_control.rightLaneVisible, hud_control.leftLaneDepart, + hud_control.rightLaneDepart, CC.enabled)) + + if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: + can_sends.append(create_fcw_command(self.packer, fcw_alert)) + + # *** static msgs *** + for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS: + if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars: + can_sends.append(make_can_msg(addr, vl, bus)) + + new_actuators = actuators.copy() + new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.accel = self.accel + new_actuators.gas = self.gas + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py new file mode 100644 index 00000000000000..0cfba2b09ff81d --- /dev/null +++ b/selfdrive/car/toyota/carstate.py @@ -0,0 +1,265 @@ +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import mean +from common.filter_simple import FirstOrderFilter +from common.realtime import DT_CTRL +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] + self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. + + # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + # the signal is zeroed to where the steering angle is at start. + # Need to apply an offset as soon as the steering angle measurements are both received + self.accurate_steer_angle_seen = False + self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) + + self.low_speed_lockout = False + self.acc_type = 1 + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], + cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) + ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0 + ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1 + + ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 + ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 + if self.CP.enableGasInterceptor: + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 + ret.gasPressed = ret.gas > 805 + else: + # TODO: find a new, common signal + msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL" + ret.gas = cp.vl[msg]["GAS_PEDAL"] + ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 + + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"], + cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], + ) + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + + ret.standstill = ret.vEgoRaw == 0 + + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] + torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + + # On some cars, the angle measurement is non-zero while initializing + if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]): + self.accurate_steer_angle_seen = True + + if self.accurate_steer_angle_seen: + # Offset seems to be invalid for large steering angles + if abs(ret.steeringAngleDeg) < 90 and cp.can_valid: + self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg) + + if self.angle_offset.initialized: + ret.steeringAngleOffsetDeg = self.angle_offset.x + ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x + + ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"] + + can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 + ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 + + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale + # we could use the override bit from dbc, but it's triggered at too high torque values + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + # steer rate fault, goes to 21 or 25 for 1 frame, then 9 for ~2 seconds + ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 21, 25) + # 17 is a fault from a prolonged high torque delta between cmd and user + ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17 + + if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 + ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS + cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"] + else: + ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 + ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS + cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"] + + # UI_SET_SPEED is always non-zero when main is on, hide until first enable + if ret.cruiseState.speed != 0: + is_metric = cp.vl["BODY_CONTROL_STATE_2"]["UNITS"] in (1, 2) + conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS + ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor + + cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp + + if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR): + self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] + ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"]) + + # some TSS2 cars have low speed lockout permanently set, so ignore on those cars + # these cars are identified by an ACC_TYPE value of 2. + # TODO: it is possible to avoid the lockout and gain stop and go if you + # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1 + if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in (CAR.LEXUS_IS, CAR.LEXUS_RC)) or \ + (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1): + self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 + + self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] + if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: + # ignore standstill in hybrid vehicles, since pcm allows to restart without + # receiving any special command. Also if interceptor is detected + ret.cruiseState.standstill = False + else: + ret.cruiseState.standstill = self.pcm_acc_status == 7 + ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) + ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) + + ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) + ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 + + if not self.CP.enableDsu: + ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + + if self.CP.enableBsm: + ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) + ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), + ("GEAR", "GEAR_PACKET"), + ("BRAKE_PRESSED", "BRAKE_MODULE"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("DOOR_OPEN_FL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_FR", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"), + ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"), + ("PARKING_BRAKE", "BODY_CONTROL_STATE"), + ("UNITS", "BODY_CONTROL_STATE_2"), + ("TC_DISABLED", "ESP_CONTROL"), + ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"), + ("STEER_FRACTION", "STEER_ANGLE_SENSOR"), + ("STEER_RATE", "STEER_ANGLE_SENSOR"), + ("CRUISE_ACTIVE", "PCM_CRUISE"), + ("CRUISE_STATE", "PCM_CRUISE"), + ("GAS_RELEASED", "PCM_CRUISE"), + ("UI_SET_SPEED", "PCM_CRUISE_SM"), + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), + ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE_INITIALIZING", "STEER_TORQUE_SENSOR"), + ("TURN_SIGNALS", "BLINKERS_STATE"), + ("LKA_STATE", "EPS_STATUS"), + ("AUTO_HIGH_BEAM", "LIGHT_STALK"), + ] + + checks = [ + ("GEAR_PACKET", 1), + ("LIGHT_STALK", 1), + ("BLINKERS_STATE", 0.15), + ("BODY_CONTROL_STATE", 3), + ("BODY_CONTROL_STATE_2", 2), + ("ESP_CONTROL", 3), + ("EPS_STATUS", 25), + ("BRAKE_MODULE", 40), + ("WHEEL_SPEEDS", 80), + ("STEER_ANGLE_SENSOR", 80), + ("PCM_CRUISE", 33), + ("PCM_CRUISE_SM", 1), + ("STEER_TORQUE_SENSOR", 50), + ] + + if CP.flags & ToyotaFlags.HYBRID: + signals.append(("GAS_PEDAL", "GAS_PEDAL_HYBRID")) + checks.append(("GAS_PEDAL_HYBRID", 33)) + else: + signals.append(("GAS_PEDAL", "GAS_PEDAL")) + checks.append(("GAS_PEDAL", 33)) + + if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): + signals.append(("MAIN_ON", "DSU_CRUISE")) + signals.append(("SET_SPEED", "DSU_CRUISE")) + signals.append(("UI_SET_SPEED", "PCM_CRUISE_ALT")) + checks.append(("DSU_CRUISE", 5)) + checks.append(("PCM_CRUISE_ALT", 1)) + else: + signals.append(("MAIN_ON", "PCM_CRUISE_2")) + signals.append(("SET_SPEED", "PCM_CRUISE_2")) + signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) + checks.append(("PCM_CRUISE_2", 33)) + + # add gas interceptor reading if we are using it + if CP.enableGasInterceptor: + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) + checks.append(("GAS_SENSOR", 50)) + + if CP.enableBsm: + signals += [ + ("L_ADJACENT", "BSM"), + ("L_APPROACHING", "BSM"), + ("R_ADJACENT", "BSM"), + ("R_APPROACHING", "BSM"), + ] + checks.append(("BSM", 1)) + + if CP.carFingerprint in RADAR_ACC_CAR: + signals += [ + ("ACC_TYPE", "ACC_CONTROL"), + ("FCW", "ACC_HUD"), + ] + checks += [ + ("ACC_CONTROL", 33), + ("ACC_HUD", 1), + ] + + if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu: + signals += [ + ("FORCE", "PRE_COLLISION"), + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ] + checks += [ + ("PRE_COLLISION", 33), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [] + + if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + signals += [ + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ("FORCE", "PRE_COLLISION"), + ("ACC_TYPE", "ACC_CONTROL"), + ("FCW", "ACC_HUD"), + ] + checks += [ + ("PRE_COLLISION", 33), + ("ACC_CONTROL", 33), + ("ACC_HUD", 1), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py new file mode 100644 index 00000000000000..28912645ac74ff --- /dev/null +++ b/selfdrive/car/toyota/interface.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python3 +from cereal import car +from common.conversions import Conversions as CV +from panda import Panda +from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune +from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase + +EventName = car.CarEvent.EventName + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "toyota" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] + ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] + + if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH): + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE + + ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + ret.steerLimitTimer = 0.4 + ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop + + stop_and_go = False + steering_angle_deadzone_deg = 0.0 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) + + if candidate == CAR.PRIUS: + stop_and_go = True + ret.wheelbase = 2.70 + ret.steerRatio = 15.74 # unknown end-to-end spec + tire_stiffness_factor = 0.6371 # hand-tune + ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG + # Only give steer angle deadzone to for bad angle sensor prius + for fw in car_fw: + if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': + steering_angle_deadzone_deg = 1.0 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) + + elif candidate == CAR.PRIUS_V: + stop_and_go = True + ret.wheelbase = 2.78 + ret.steerRatio = 17.4 + tire_stiffness_factor = 0.5533 + ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) + + elif candidate in (CAR.RAV4, CAR.RAV4H): + stop_and_go = True if (candidate in CAR.RAV4H) else False + ret.wheelbase = 2.65 + ret.steerRatio = 16.88 # 14.5 is spec end-to-end + tire_stiffness_factor = 0.5533 + ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + + elif candidate == CAR.COROLLA: + ret.wheelbase = 2.70 + ret.steerRatio = 18.27 + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + + elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): + stop_and_go = True + ret.wheelbase = 2.79 + ret.steerRatio = 16. # 14.8 is spec end-to-end + ret.wheelSpeedFactor = 1.035 + tire_stiffness_factor = 0.5533 + ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + + elif candidate in (CAR.CHR, CAR.CHRH): + stop_and_go = True + ret.wheelbase = 2.63906 + ret.steerRatio = 13.6 + tire_stiffness_factor = 0.7933 + ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_F) + + elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + stop_and_go = True + ret.wheelbase = 2.82448 + ret.steerRatio = 13.7 + tire_stiffness_factor = 0.7933 + ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + if candidate not in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + + elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): + stop_and_go = True + ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in + ret.steerRatio = 16.0 + tire_stiffness_factor = 0.8 + ret.mass = 4516. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + set_lat_tune(ret.lateralTuning, LatTunes.PID_G) + + elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2): + # starting from 2019, all Avalon variants have stop and go + # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf + stop_and_go = candidate != CAR.AVALON + ret.wheelbase = 2.82 + ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download + tire_stiffness_factor = 0.7983 + ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + set_lat_tune(ret.lateralTuning, LatTunes.PID_H) + + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): + stop_and_go = True + ret.wheelbase = 2.68986 + ret.steerRatio = 14.3 + tire_stiffness_factor = 0.7933 + ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid + set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + + # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. + # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 + for fw in car_fw: + if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']): + set_lat_tune(ret.lateralTuning, LatTunes.PID_I) + break + + elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2): + stop_and_go = True + ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback + ret.steerRatio = 13.9 + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG + + elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): + stop_and_go = True + ret.wheelbase = 2.8702 + ret.steerRatio = 16.0 # not optimized + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + + elif candidate == CAR.SIENNA: + stop_and_go = True + ret.wheelbase = 3.03 + ret.steerRatio = 15.5 + tire_stiffness_factor = 0.444 + ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_J) + + elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): + ret.wheelbase = 2.79908 + ret.steerRatio = 13.3 + tire_stiffness_factor = 0.444 + ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_L) + + elif candidate == CAR.LEXUS_CTH: + stop_and_go = True + ret.wheelbase = 2.60 + ret.steerRatio = 18.6 + tire_stiffness_factor = 0.517 + ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + set_lat_tune(ret.lateralTuning, LatTunes.PID_M) + + elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NXH, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2): + stop_and_go = True + ret.wheelbase = 2.66 + ret.steerRatio = 14.7 + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + + elif candidate == CAR.PRIUS_TSS2: + stop_and_go = True + ret.wheelbase = 2.70002 # from toyota online sepc. + ret.steerRatio = 13.4 # True steerRatio from older prius + tire_stiffness_factor = 0.6371 # hand-tune + ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_N) + + elif candidate == CAR.MIRAI: + stop_and_go = True + ret.wheelbase = 2.91 + ret.steerRatio = 14.8 + tire_stiffness_factor = 0.8 + ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + + elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2): + stop_and_go = True + ret.wheelbase = 3.00 + ret.steerRatio = 14.2 + tire_stiffness_factor = 0.444 + ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.PID_J) + + ret.centerToFront = ret.wheelbase * 0.44 + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR + # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it + smartDsu = 0x2FF in fingerprint[0] + # In TSS2 cars the camera does long control + found_ecus = [fw.ecu for fw in car_fw] + ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not smartDsu) + ret.enableGasInterceptor = 0x201 in fingerprint[0] + # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") + ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + + if not ret.openpilotLongitudinalControl: + ret.autoResumeSng = False + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL + + # we can't use the fingerprint to detect this reliably, since + # the EV gas pedal signal can take a couple seconds to appear + if candidate in EV_HYBRID_CAR: + ret.flags |= ToyotaFlags.HYBRID.value + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED + + if candidate in TSS2_CAR or ret.enableGasInterceptor: + set_long_tune(ret.longitudinalTuning, LongTunes.TSS2) + if candidate in TSS2_CAR: + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + else: + set_long_tune(ret.longitudinalTuning, LongTunes.TSS) + + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + # events + events = self.create_common_events(ret) + + if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: + events.add(EventName.lowSpeedLockout) + if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: + events.add(EventName.belowEngageSpeed) + if c.actuators.accel > 0.3: + # some margin on the actuator to not false trigger cancellation while stopping + events.add(EventName.speedTooLow) + if ret.vEgo < 0.001: + # while in standstill, send a user alert + events.add(EventName.manualRestart) + + ret.events = events.to_msg() + + return ret + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py new file mode 100755 index 00000000000000..8c87704ff27743 --- /dev/null +++ b/selfdrive/car/toyota/radar_interface.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +from opendbc.can.parser import CANParser +from cereal import car +from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR +from selfdrive.car.interfaces import RadarInterfaceBase + + +def _create_radar_can_parser(car_fingerprint): + if DBC[car_fingerprint]['radar'] is None: + return None + + if car_fingerprint in TSS2_CAR: + RADAR_A_MSGS = list(range(0x180, 0x190)) + RADAR_B_MSGS = list(range(0x190, 0x1a0)) + else: + RADAR_A_MSGS = list(range(0x210, 0x220)) + RADAR_B_MSGS = list(range(0x220, 0x230)) + + msg_a_n = len(RADAR_A_MSGS) + msg_b_n = len(RADAR_B_MSGS) + + signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, + RADAR_A_MSGS * 5 + RADAR_B_MSGS)) + + checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n))) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.track_id = 0 + self.radar_ts = CP.radarTimeStep + + if CP.carFingerprint in TSS2_CAR: + self.RADAR_A_MSGS = list(range(0x180, 0x190)) + self.RADAR_B_MSGS = list(range(0x190, 0x1a0)) + else: + self.RADAR_A_MSGS = list(range(0x210, 0x220)) + self.RADAR_B_MSGS = list(range(0x220, 0x230)) + + self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS} + + self.rcp = _create_radar_can_parser(CP.carFingerprint) + self.trigger_msg = self.RADAR_B_MSGS[-1] + self.updated_messages = set() + + # No radar dbc for cars without DSU which are not TSS 2.0 + # TODO: make a adas dbc file for dsu-less models + self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR + + def update(self, can_strings): + if self.no_radar or self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + rr = self._update(self.updated_messages) + self.updated_messages.clear() + + return rr + + def _update(self, updated_messages): + ret = car.RadarData.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + for ii in sorted(updated_messages): + if ii in self.RADAR_A_MSGS: + cpt = self.rcp.vl[ii] + + if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']: + self.valid_cnt[ii] = 0 # reset counter + if cpt['VALID'] and cpt['LONG_DIST'] < 255: + self.valid_cnt[ii] += 1 + else: + self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) + + score = self.rcp.vl[ii+16]['SCORE'] + # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST'] + + # radar point only valid if it's a valid measurement and score is above 50 + if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): + if ii not in self.pts or cpt['NEW_TRACK']: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car + self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['REL_SPEED'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = bool(cpt['VALID']) + else: + if ii in self.pts: + del self.pts[ii] + + ret.points = list(self.pts.values()) + return ret diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py new file mode 100644 index 00000000000000..7ab3ab3e78a5e0 --- /dev/null +++ b/selfdrive/car/toyota/toyotacan.py @@ -0,0 +1,99 @@ +def create_steer_command(packer, steer, steer_req): + """Creates a CAN message for the Toyota Steer Command.""" + + values = { + "STEER_REQUEST": steer_req, + "STEER_TORQUE_CMD": steer, + "SET_ME_1": 1, + } + return packer.make_can_msg("STEERING_LKA", 0, values) + + +def create_lta_steer_command(packer, steer, steer_req, raw_cnt): + """Creates a CAN message for the Toyota LTA Steer Command.""" + + values = { + "COUNTER": raw_cnt + 128, + "SETME_X1": 1, + "SETME_X3": 3, + "PERCENTAGE": 100, + "SETME_X64": 0x64, + "ANGLE": 0, + "STEER_ANGLE_CMD": steer, + "STEER_REQUEST": steer_req, + "STEER_REQUEST_2": steer_req, + "BIT": 0, + } + return packer.make_can_msg("STEERING_LTA", 0, values) + + +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type): + # TODO: find the exact canceling bit that does not create a chime + values = { + "ACCEL_CMD": accel, + "ACC_TYPE": acc_type, + "DISTANCE": 0, + "MINI_CAR": lead, + "PERMIT_BRAKING": 1, + "RELEASE_STANDSTILL": not standstill_req, + "CANCEL_REQ": pcm_cancel, + "ALLOW_LONG_PRESS": 1, + } + return packer.make_can_msg("ACC_CONTROL", 0, values) + + +def create_acc_cancel_command(packer): + values = { + "GAS_RELEASED": 0, + "CRUISE_ACTIVE": 0, + "STANDSTILL_ON": 0, + "ACCEL_NET": 0, + "CRUISE_STATE": 0, + "CANCEL_REQ": 1, + } + return packer.make_can_msg("PCM_CRUISE", 0, values) + + +def create_fcw_command(packer, fcw): + values = { + "PCS_INDICATOR": 1, + "FCW": fcw, + "SET_ME_X20": 0x20, + "SET_ME_X10": 0x10, + "PCS_OFF": 1, + "PCS_SENSITIVITY": 0, + } + return packer.make_can_msg("ACC_HUD", 0, values) + + +def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled): + values = { + "TWO_BEEPS": chime, + "LDA_ALERT": steer, + "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, + "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, + "BARRIERS" : 1 if enabled else 0, + + # static signals + "SET_ME_X02": 2, + "SET_ME_X01": 1, + "LKAS_STATUS": 1, + "REPEATED_BEEPS": 0, + "LANE_SWAY_FLD": 7, + "LANE_SWAY_BUZZER": 0, + "LANE_SWAY_WARNING": 0, + "LDA_FRONT_CAMERA_BLOCKED": 0, + "TAKE_CONTROL": 0, + "LANE_SWAY_SENSITIVITY": 2, + "LANE_SWAY_TOGGLE": 1, + "LDA_ON_MESSAGE": 0, + "LDA_SPEED_TOO_LOW": 0, + "LDA_SA_TOGGLE": 1, + "LDA_SENSITIVITY": 2, + "LDA_UNAVAILABLE": 0, + "LDA_MALFUNCTION": 0, + "LDA_UNAVAILABLE_QUIET": 0, + "ADJUSTING_CAMERA": 0, + "LDW_EXIST": 1, + } + return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py new file mode 100644 index 00000000000000..b73ab4c8c9b660 --- /dev/null +++ b/selfdrive/car/toyota/tunes.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +from enum import Enum + +class LongTunes(Enum): + TSS2 = 0 + TSS = 1 + +class LatTunes(Enum): + INDI_PRIUS = 0 + LQR_RAV4 = 1 + PID_A = 2 + PID_B = 3 + PID_C = 4 + PID_D = 5 + PID_E = 6 + PID_F = 7 + PID_G = 8 + PID_I = 9 + PID_H = 10 + PID_J = 11 + PID_K = 12 + PID_L = 13 + PID_M = 14 + PID_N = 15 + + +###### LONG ###### +def set_long_tune(tune, name): + # Improved longitudinal tune + if name == LongTunes.TSS2: + tune.deadzoneBP = [0., 8.05] + tune.deadzoneV = [.0, .14] + tune.kpBP = [0., 5., 20.] + tune.kpV = [1.3, 1.0, 0.7] + tune.kiBP = [0., 5., 12., 20., 27.] + tune.kiV = [.35, .23, .20, .17, .1] + # Default longitudinal tune + elif name == LongTunes.TSS: + tune.deadzoneBP = [0., 9.] + tune.deadzoneV = [.0, .15] + tune.kpBP = [0., 5., 35.] + tune.kiBP = [0., 35.] + tune.kpV = [3.6, 2.4, 1.5] + tune.kiV = [0.54, 0.36] + else: + raise NotImplementedError('This longitudinal tune does not exist') + + +###### LAT ###### +def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): + if 'PID' in str(name): + tune.init('pid') + tune.pid.kiBP = [0.0] + tune.pid.kpBP = [0.0] + if name == LatTunes.PID_A: + tune.pid.kpV = [0.2] + tune.pid.kiV = [0.05] + tune.pid.kf = 0.00003 + elif name == LatTunes.PID_C: + tune.pid.kpV = [0.6] + tune.pid.kiV = [0.1] + tune.pid.kf = 0.00006 + elif name == LatTunes.PID_D: + tune.pid.kpV = [0.6] + tune.pid.kiV = [0.1] + tune.pid.kf = 0.00007818594 + elif name == LatTunes.PID_F: + tune.pid.kpV = [0.723] + tune.pid.kiV = [0.0428] + tune.pid.kf = 0.00006 + elif name == LatTunes.PID_G: + tune.pid.kpV = [0.18] + tune.pid.kiV = [0.015] + tune.pid.kf = 0.00012 + elif name == LatTunes.PID_H: + tune.pid.kpV = [0.17] + tune.pid.kiV = [0.03] + tune.pid.kf = 0.00006 + elif name == LatTunes.PID_I: + tune.pid.kpV = [0.15] + tune.pid.kiV = [0.05] + tune.pid.kf = 0.00004 + elif name == LatTunes.PID_J: + tune.pid.kpV = [0.19] + tune.pid.kiV = [0.02] + tune.pid.kf = 0.00007818594 + elif name == LatTunes.PID_L: + tune.pid.kpV = [0.3] + tune.pid.kiV = [0.05] + tune.pid.kf = 0.00006 + elif name == LatTunes.PID_M: + tune.pid.kpV = [0.3] + tune.pid.kiV = [0.05] + tune.pid.kf = 0.00007 + elif name == LatTunes.PID_N: + tune.pid.kpV = [0.35] + tune.pid.kiV = [0.15] + tune.pid.kf = 0.00007818594 + else: + raise NotImplementedError('This PID tune does not exist') + else: + raise NotImplementedError('This lateral tune does not exist') diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py new file mode 100644 index 00000000000000..94fbdc8bf2e494 --- /dev/null +++ b/selfdrive/car/toyota/values.py @@ -0,0 +1,2030 @@ +from collections import defaultdict +from dataclasses import dataclass +from enum import Enum, IntFlag +from typing import Dict, List, Union + +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries + +Ecu = car.CarParams.Ecu +MIN_ACC_SPEED = 19. * CV.MPH_TO_MS +PEDAL_TRANSITION = 10. * CV.MPH_TO_MS + + +class CarControllerParams: + ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons + ACCEL_MIN = -3.5 # m/s2 + + STEER_MAX = 1500 + STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + + def __init__(self, CP): + if CP.lateralTuning.which == 'torque': + self.STEER_DELTA_UP = 15 # 1.0s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + else: + self.STEER_DELTA_UP = 10 # 1.5s time to peak torque + self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) + + +class ToyotaFlags(IntFlag): + HYBRID = 1 + + +class CAR: + # Toyota + ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" + ALPHARDH_TSS2 = "TOYOTA ALPHARD HYBRID 2021" + AVALON = "TOYOTA AVALON 2016" + AVALON_2019 = "TOYOTA AVALON 2019" + AVALONH_2019 = "TOYOTA AVALON HYBRID 2019" + AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5 + AVALONH_TSS2 = "TOYOTA AVALON HYBRID 2022" + CAMRY = "TOYOTA CAMRY 2018" + CAMRYH = "TOYOTA CAMRY HYBRID 2018" + CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 + CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021" + CHR = "TOYOTA C-HR 2018" + CHRH = "TOYOTA C-HR HYBRID 2018" + COROLLA = "TOYOTA COROLLA 2017" + COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" + # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid + COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019" + HIGHLANDER = "TOYOTA HIGHLANDER 2017" + HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" + HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" + HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020" + PRIUS = "TOYOTA PRIUS 2017" + PRIUS_V = "TOYOTA PRIUS v 2017" + PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" + RAV4 = "TOYOTA RAV4 2017" + RAV4H = "TOYOTA RAV4 HYBRID 2017" + RAV4_TSS2 = "TOYOTA RAV4 2019" + RAV4_TSS2_2022 = "TOYOTA RAV4 2022" + RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" + RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022" + MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 + SIENNA = "TOYOTA SIENNA 2018" + + # Lexus + LEXUS_CTH = "LEXUS CT HYBRID 2018" + LEXUS_ESH = "LEXUS ES HYBRID 2018" + LEXUS_ES_TSS2 = "LEXUS ES 2019" + LEXUS_ESH_TSS2 = "LEXUS ES HYBRID 2019" + LEXUS_IS = "LEXUS IS 2018" + LEXUS_NX = "LEXUS NX 2018" + LEXUS_NXH = "LEXUS NX HYBRID 2018" + LEXUS_NX_TSS2 = "LEXUS NX 2020" + LEXUS_NXH_TSS2 = "LEXUS NX HYBRID 2020" + LEXUS_RC = "LEXUS RC 2020" + LEXUS_RX = "LEXUS RX 2016" + LEXUS_RXH = "LEXUS RX HYBRID 2017" + LEXUS_RX_TSS2 = "LEXUS RX 2020" + LEXUS_RXH_TSS2 = "LEXUS RX HYBRID 2020" + + +class Footnote(Enum): + DSU = CarFootnote( + "When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace " + + "stock Adaptive Cruise Control (ACC). NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).", + Column.LONGITUDINAL) + CAMRY = CarFootnote( + "openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.", + Column.FSR_LONGITUDINAL) + + +@dataclass +class ToyotaCarInfo(CarInfo): + package: str = "All" + harness: Enum = Harness.toyota + + +CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { + # Toyota + CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"), + CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"), + CAR.AVALON: [ + ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Avalon 2017-18", footnotes=[Footnote.DSU]), + ], + CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21", footnotes=[Footnote.DSU]), + CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21", footnotes=[Footnote.DSU]), + CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"), + CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"), + CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), + CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), + CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-22", footnotes=[Footnote.CAMRY]), + CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-22"), + CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"), + CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), + CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19", footnotes=[Footnote.DSU]), + CAR.COROLLA_TSS2: [ + ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-21", min_enable_speed=7.5), + ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + ], + CAR.COROLLAH_TSS2: [ + ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), + ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), + ToyotaCarInfo("Lexus UX Hybrid 2019-22"), + ], + CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo", footnotes=[Footnote.DSU]), + CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-22"), + CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19", footnotes=[Footnote.DSU]), + CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-22"), + CAR.PRIUS: [ + ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]), + ], + CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED, footnotes=[Footnote.DSU]), + CAR.PRIUS_TSS2: [ + ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ], + CAR.RAV4: [ + ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota RAV4 2017-18", footnotes=[Footnote.DSU]) + ], + CAR.RAV4H: [ + ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU]) + ], + CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), + CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"), + CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), + CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), + CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), + CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU], min_enable_speed=MIN_ACC_SPEED), + + # Lexus + CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+", footnotes=[Footnote.DSU]), + CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+", footnotes=[Footnote.DSU]), + CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"), + CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-22", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), + CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"), + CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"), + CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"), + CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"), + CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"), +} + +# (addr, cars, bus, 1/freq*100, vl) +STATIC_DSU_MSGS = [ + (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), + (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), + (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), + (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), + (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), + (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), + (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), + (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), +] + +TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01' +TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01' + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST], + [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE], + bus=0, + ), + Request( + [StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST], + [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE], + bus=0, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], + bus=0, + ), + ], + non_essential_ecus={ + # FIXME: On some models, abs can sometimes be missing + Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS], + # On some models, the engine can show on two different addresses + Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.LEXUS_IS], + } +) + +FW_VERSIONS = { + CAR.AVALON: { + (Ecu.abs, 0x7b0, None): [ + b'F152607060\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881510701300\x00\x00\x00\x00', + b'881510705100\x00\x00\x00\x00', + b'881510705200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41051\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0230721100\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230721200\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702000\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0701100\x00\x00\x00\x00', + b'8646F0703000\x00\x00\x00\x00', + ], + }, + CAR.AVALON_2019: { + (Ecu.abs, 0x7b0, None): [ + b'F152607140\x00\x00\x00\x00\x00\x00', + b'F152607171\x00\x00\x00\x00\x00\x00', + b'F152607110\x00\x00\x00\x00\x00\x00', + b'F152607180\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881510703200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41080\x00\x00\x00\x00\x00\x00', + b'8965B07010\x00\x00\x00\x00\x00\x00', + b'8965B41090\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896630725200\x00\x00\x00\x00', + b'\x01896630725300\x00\x00\x00\x00', + b'\x01896630735100\x00\x00\x00\x00', + b'\x01896630738000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0702100\x00\x00\x00\x00', + ], + }, + CAR.AVALONH_2019: { + (Ecu.abs, 0x7b0, None): [ + b'F152641040\x00\x00\x00\x00\x00\x00', + b'F152641061\x00\x00\x00\x00\x00\x00', + b'F152641050\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881510704200\x00\x00\x00\x00', + b'881514107100\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B07010\x00\x00\x00\x00\x00\x00', + b'8965B41090\x00\x00\x00\x00\x00\x00', + b'8965B41070\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0702100\x00\x00\x00\x00', + ], + }, + CAR.AVALON_TSS2: { + (Ecu.abs, 0x7b0, None): [ + b'\x01F152607240\x00\x00\x00\x00\x00\x00', + b'\x01F152607280\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41110\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896630742000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.AVALONH_TSS2: { + (Ecu.abs, 0x7b0, None): [ + b'F152641080\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41110\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x018966306Q6000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F6201200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.CAMRY: { + (Ecu.engine, 0x700, None): [ + b'\x018966306L3100\x00\x00\x00\x00', + b'\x018966306L4200\x00\x00\x00\x00', + b'\x018966306L5200\x00\x00\x00\x00', + b'\x018966306P8000\x00\x00\x00\x00', + b'\x018966306Q3100\x00\x00\x00\x00', + b'\x018966306Q4000\x00\x00\x00\x00', + b'\x018966306Q4100\x00\x00\x00\x00', + b'\x018966306Q4200\x00\x00\x00\x00', + b'\x018966333Q9200\x00\x00\x00\x00', + b'\x018966333P3100\x00\x00\x00\x00', + b'\x018966333P3200\x00\x00\x00\x00', + b'\x018966333P4200\x00\x00\x00\x00', + b'\x018966333P4300\x00\x00\x00\x00', + b'\x018966333P4400\x00\x00\x00\x00', + b'\x018966333P4500\x00\x00\x00\x00', + b'\x018966333P4700\x00\x00\x00\x00', + b'\x018966333P4900\x00\x00\x00\x00', + b'\x018966333Q6000\x00\x00\x00\x00', + b'\x018966333Q6200\x00\x00\x00\x00', + b'\x018966333Q6300\x00\x00\x00\x00', + b'\x018966333W6000\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821F0601200 ', + b'8821F0601300 ', + b'8821F0602000 ', + b'8821F0603300 ', + b'8821F0604100 ', + b'8821F0605200 ', + b'8821F0607200 ', + b'8821F0608000 ', + b'8821F0608200 ', + b'8821F0609100 ', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152606210\x00\x00\x00\x00\x00\x00', + b'F152606230\x00\x00\x00\x00\x00\x00', + b'F152606270\x00\x00\x00\x00\x00\x00', + b'F152606290\x00\x00\x00\x00\x00\x00', + b'F152606410\x00\x00\x00\x00\x00\x00', + b'F152633540\x00\x00\x00\x00\x00\x00', + b'F152633A10\x00\x00\x00\x00\x00\x00', + b'F152633A20\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33540\x00\x00\x00\x00\x00\x00', + b'8965B33542\x00\x00\x00\x00\x00\x00', + b'8965B33580\x00\x00\x00\x00\x00\x00', + b'8965B33581\x00\x00\x00\x00\x00\x00', + b'8965B33621\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 + b'8821F0601200 ', + b'8821F0601300 ', + b'8821F0602000 ', + b'8821F0603300 ', + b'8821F0604100 ', + b'8821F0605200 ', + b'8821F0607200 ', + b'8821F0608000 ', + b'8821F0608200 ', + b'8821F0609100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0601200 ', + b'8646F0601300 ', + b'8646F0601400 ', + b'8646F0603400 ', + b'8646F0604100 ', + b'8646F0605000 ', + b'8646F0606000 ', + b'8646F0606100 ', + b'8646F0607100 ', + ], + }, + CAR.CAMRYH: { + (Ecu.engine, 0x700, None): [ + b'\x018966306Q6000\x00\x00\x00\x00', + b'\x018966333N1100\x00\x00\x00\x00', + b'\x018966333N4300\x00\x00\x00\x00', + b'\x018966333X0000\x00\x00\x00\x00', + b'\x018966333X4000\x00\x00\x00\x00', + b'\x01896633T16000\x00\x00\x00\x00', + b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306B2500\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306N8200\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306N8300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306N8400\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306R5000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306R5000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966306R6000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306R6000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966306S0000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152633214\x00\x00\x00\x00\x00\x00', + b'F152633660\x00\x00\x00\x00\x00\x00', + b'F152633712\x00\x00\x00\x00\x00\x00', + b'F152633713\x00\x00\x00\x00\x00\x00', + b'F152633B51\x00\x00\x00\x00\x00\x00', + b'F152633B60\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821F0601200 ', + b'8821F0601300 ', + b'8821F0603400 ', + b'8821F0604000 ', + b'8821F0604100 ', + b'8821F0604200 ', + b'8821F0605200 ', + b'8821F0606200 ', + b'8821F0607200 ', + b'8821F0608000 ', + b'8821F0608200 ', + b'8821F0609000 ', + b'8821F0609100 ', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33540\x00\x00\x00\x00\x00\x00', + b'8965B33542\x00\x00\x00\x00\x00\x00', + b'8965B33550\x00\x00\x00\x00\x00\x00', + b'8965B33551\x00\x00\x00\x00\x00\x00', + b'8965B33580\x00\x00\x00\x00\x00\x00', + b'8965B33581\x00\x00\x00\x00\x00\x00', + b'8965B33611\x00\x00\x00\x00\x00\x00', + b'8965B33621\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 + b'8821F0601200 ', + b'8821F0601300 ', + b'8821F0603400 ', + b'8821F0604000 ', + b'8821F0604100 ', + b'8821F0604200 ', + b'8821F0605200 ', + b'8821F0606200 ', + b'8821F0607200 ', + b'8821F0608000 ', + b'8821F0608200 ', + b'8821F0609000 ', + b'8821F0609100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0601200 ', + b'8646F0601300 ', + b'8646F0601400 ', + b'8646F0603400 ', + b'8646F0603500 ', + b'8646F0604100 ', + b'8646F0605000 ', + b'8646F0606000 ', + b'8646F0606100 ', + b'8646F0607000 ', + b'8646F0607100 ', + ], + }, + CAR.CAMRY_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B33630\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F152606370\x00\x00\x00\x00\x00\x00', + b'\x01F152606390\x00\x00\x00\x00\x00\x00', + b'\x01F152606400\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x018966306Q5000\x00\x00\x00\x00', + b'\x018966306T3100\x00\x00\x00\x00', + b'\x018966306T3200\x00\x00\x00\x00', + b'\x018966306T4000\x00\x00\x00\x00', + b'\x018966306T4100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F6201200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + ], + }, + CAR.CAMRYH_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B33630\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152633D00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x018966306Q6000\x00\x00\x00\x00', + b'\x018966306Q7000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 15): [ + b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 109): [ + b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.CHR: { + (Ecu.engine, 0x700, None): [ + b'\x01896631021100\x00\x00\x00\x00', + b'\x01896631017100\x00\x00\x00\x00', + b'\x01896631017200\x00\x00\x00\x00', + b'\x0189663F413100\x00\x00\x00\x00', + b'\x0189663F414100\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821F0W01000 ', + b'8821F0W01100 ', + b'8821FF401600 ', + b'8821FF404000 ', + b'8821FF404100 ', + b'8821FF405100 ', + b'8821FF406000 ', + b'8821FF407100 ', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152610020\x00\x00\x00\x00\x00\x00', + b'F152610153\x00\x00\x00\x00\x00\x00', + b'F152610210\x00\x00\x00\x00\x00\x00', + b'F1526F4034\x00\x00\x00\x00\x00\x00', + b'F1526F4044\x00\x00\x00\x00\x00\x00', + b'F1526F4073\x00\x00\x00\x00\x00\x00', + b'F1526F4121\x00\x00\x00\x00\x00\x00', + b'F1526F4122\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B10011\x00\x00\x00\x00\x00\x00', + b'8965B10040\x00\x00\x00\x00\x00\x00', + b'8965B10070\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x0331036000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00', + b'\x033F401200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F0W01000 ', + b'8821FF401600 ', + b'8821FF404000 ', + b'8821FF404100 ', + b'8821FF405100 ', + b'8821FF406000 ', + b'8821FF407100 ', + b'8821F0W01100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646FF401700 ', + b'8646FF401800 ', + b'8646FF404000 ', + b'8646FF406000 ', + b'8646FF407000 ', + ], + }, + CAR.CHRH: { + (Ecu.engine, 0x700, None): [ + b'\x0289663F405100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896631013200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x0189663F438000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152610012\x00\x00\x00\x00\x00\x00', + b'F152610013\x00\x00\x00\x00\x00\x00', + b'F152610014\x00\x00\x00\x00\x00\x00', + b'F152610040\x00\x00\x00\x00\x00\x00', + b'F152610190\x00\x00\x00\x00\x00\x00', + b'F152610200\x00\x00\x00\x00\x00\x00', + b'F152610230\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821F0W01000 ', + b'8821FF402300 ', + b'8821FF402400 ', + b'8821FF404000 ', + b'8821FF404100 ', + b'8821FF405000 ', + b'8821FF406000 ', + b'8821FF407100 ', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B10011\x00\x00\x00\x00\x00\x00', + b'8965B10020\x00\x00\x00\x00\x00\x00', + b'8965B10040\x00\x00\x00\x00\x00\x00', + b'8965B10050\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F0W01000 ', + b'8821FF402300 ', + b'8821FF402400 ', + b'8821FF404000 ', + b'8821FF404100 ', + b'8821FF405000 ', + b'8821FF406000 ', + b'8821FF407100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646FF401700 ', + b'8646FF402100 ', + b'8646FF404000 ', + b'8646FF406000 ', + b'8646FF407000 ', + ], + }, + CAR.COROLLA: { + (Ecu.engine, 0x7e0, None): [ + b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC2100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC2200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC2300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC3000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC3100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC3200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZC3300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0330ZC1200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881510201100\x00\x00\x00\x00', + b'881510201200\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152602190\x00\x00\x00\x00\x00\x00', + b'F152602191\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B02181\x00\x00\x00\x00\x00\x00', + b'8965B02191\x00\x00\x00\x00\x00\x00', + b'8965B48150\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0201101\x00\x00\x00\x00', + b'8646F0201200\x00\x00\x00\x00', + ], + }, + CAR.COROLLA_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x01896630ZG2000\x00\x00\x00\x00', + b'\x01896630ZG5000\x00\x00\x00\x00', + b'\x01896630ZG5100\x00\x00\x00\x00', + b'\x01896630ZG5200\x00\x00\x00\x00', + b'\x01896630ZG5300\x00\x00\x00\x00', + b'\x01896630ZP1000\x00\x00\x00\x00', + b'\x01896630ZP2000\x00\x00\x00\x00', + b'\x01896630ZQ5000\x00\x00\x00\x00', + b'\x01896630ZU9000\x00\x00\x00\x00', + b'\x01896630ZX4000\x00\x00\x00\x00', + b'\x018966312L8000\x00\x00\x00\x00', + b'\x018966312M0000\x00\x00\x00\x00', + b'\x018966312M9000\x00\x00\x00\x00', + b'\x018966312P9000\x00\x00\x00\x00', + b'\x018966312P9100\x00\x00\x00\x00', + b'\x018966312P9200\x00\x00\x00\x00', + b'\x018966312P9300\x00\x00\x00\x00', + b'\x018966312Q2300\x00\x00\x00\x00', + b'\x018966312Q8000\x00\x00\x00\x00', + b'\x018966312R0000\x00\x00\x00\x00', + b'\x018966312R0100\x00\x00\x00\x00', + b'\x018966312R1000\x00\x00\x00\x00', + b'\x018966312R1100\x00\x00\x00\x00', + b'\x018966312R3100\x00\x00\x00\x00', + b'\x018966312S5000\x00\x00\x00\x00', + b'\x018966312S7000\x00\x00\x00\x00', + b'\x018966312W3000\x00\x00\x00\x00', + b'\x018966312W9000\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0230A10000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230A11000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x03312K7000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02312U5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x018965B12350\x00\x00\x00\x00\x00\x00', + b'\x018965B12470\x00\x00\x00\x00\x00\x00', + b'\x018965B12490\x00\x00\x00\x00\x00\x00', + b'\x018965B12500\x00\x00\x00\x00\x00\x00', + b'\x018965B12520\x00\x00\x00\x00\x00\x00', + b'\x018965B12530\x00\x00\x00\x00\x00\x00', + b'\x018965B1255000\x00\x00\x00\x00', + b'8965B12361\x00\x00\x00\x00\x00\x00', + b'8965B16011\x00\x00\x00\x00\x00\x00', + b'\x018965B12510\x00\x00\x00\x00\x00\x00', + b'\x018965B1256000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F152602280\x00\x00\x00\x00\x00\x00', + b'\x01F152602560\x00\x00\x00\x00\x00\x00', + b'\x01F152602590\x00\x00\x00\x00\x00\x00', + b'\x01F152602650\x00\x00\x00\x00\x00\x00', + b"\x01F15260A010\x00\x00\x00\x00\x00\x00", + b'\x01F15260A050\x00\x00\x00\x00\x00\x00', + b'\x01F152612641\x00\x00\x00\x00\x00\x00', + b'\x01F152612651\x00\x00\x00\x00\x00\x00', + b'\x01F152612B10\x00\x00\x00\x00\x00\x00', + b'\x01F152612B51\x00\x00\x00\x00\x00\x00', + b'\x01F152612B60\x00\x00\x00\x00\x00\x00', + b'\x01F152612B61\x00\x00\x00\x00\x00\x00', + b'\x01F152612B62\x00\x00\x00\x00\x00\x00', + b'\x01F152612B71\x00\x00\x00\x00\x00\x00', + b'\x01F152612B81\x00\x00\x00\x00\x00\x00', + b'\x01F152612B90\x00\x00\x00\x00\x00\x00', + b'\x01F152612C00\x00\x00\x00\x00\x00\x00', + b'F152602191\x00\x00\x00\x00\x00\x00', + b'\x01F152612862\x00\x00\x00\x00\x00\x00', + b'\x01F152612B91\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.COROLLAH_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x01896630ZJ1000\x00\x00\x00\x00', + b'\x01896630ZU8000\x00\x00\x00\x00', + b'\x01896637621000\x00\x00\x00\x00', + b'\x01896637624000\x00\x00\x00\x00', + b'\x01896637626000\x00\x00\x00\x00', + b'\x01896637648000\x00\x00\x00\x00', + b'\x01896637643000\x00\x00\x00\x00', + b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00', + b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x038966312T3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B12361\x00\x00\x00\x00\x00\x00', + b'8965B12451\x00\x00\x00\x00\x00\x00', + b'8965B16011\x00\x00\x00\x00\x00\x00', + b'8965B76012\x00\x00\x00\x00\x00\x00', + b'8965B76050\x00\x00\x00\x00\x00\x00', + b'\x018965B12350\x00\x00\x00\x00\x00\x00', + b'\x018965B12470\x00\x00\x00\x00\x00\x00', + b'\x018965B12490\x00\x00\x00\x00\x00\x00', + b'\x018965B12500\x00\x00\x00\x00\x00\x00', + b'\x018965B12510\x00\x00\x00\x00\x00\x00', + b'\x018965B12520\x00\x00\x00\x00\x00\x00', + b'\x018965B12530\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152612590\x00\x00\x00\x00\x00\x00', + b'F152612691\x00\x00\x00\x00\x00\x00', + b'F152612692\x00\x00\x00\x00\x00\x00', + b'F152612700\x00\x00\x00\x00\x00\x00', + b'F152612710\x00\x00\x00\x00\x00\x00', + b'F152612790\x00\x00\x00\x00\x00\x00', + b'F152612800\x00\x00\x00\x00\x00\x00', + b'F152612820\x00\x00\x00\x00\x00\x00', + b'F152612840\x00\x00\x00\x00\x00\x00', + b'F152612842\x00\x00\x00\x00\x00\x00', + b'F152612890\x00\x00\x00\x00\x00\x00', + b'F152612A00\x00\x00\x00\x00\x00\x00', + b'F152612A10\x00\x00\x00\x00\x00\x00', + b'F152612D00\x00\x00\x00\x00\x00\x00', + b'F152616011\x00\x00\x00\x00\x00\x00', + b'F152616060\x00\x00\x00\x00\x00\x00', + b'F152642540\x00\x00\x00\x00\x00\x00', + b'F152676293\x00\x00\x00\x00\x00\x00', + b'F152676303\x00\x00\x00\x00\x00\x00', + b'F152676304\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b"\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00", + b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F7603200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.HIGHLANDER: { + (Ecu.engine, 0x700, None): [ + b'\x01896630E09000\x00\x00\x00\x00', + b'\x01896630E43000\x00\x00\x00\x00', + b'\x01896630E43100\x00\x00\x00\x00', + b'\x01896630E43200\x00\x00\x00\x00', + b'\x01896630E44200\x00\x00\x00\x00', + b'\x01896630E45000\x00\x00\x00\x00', + b'\x01896630E45100\x00\x00\x00\x00', + b'\x01896630E45200\x00\x00\x00\x00', + b'\x01896630E46000\x00\x00\x00\x00', + b'\x01896630E46200\x00\x00\x00\x00', + b'\x01896630E74000\x00\x00\x00\x00', + b'\x01896630E75000\x00\x00\x00\x00', + b'\x01896630E76000\x00\x00\x00\x00', + b'\x01896630E77000\x00\x00\x00\x00', + b'\x01896630E83000\x00\x00\x00\x00', + b'\x01896630E84000\x00\x00\x00\x00', + b'\x01896630E85000\x00\x00\x00\x00', + b'\x01896630E86000\x00\x00\x00\x00', + b'\x01896630E88000\x00\x00\x00\x00', + b'\x01896630EA0000\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B48140\x00\x00\x00\x00\x00\x00', + b'8965B48150\x00\x00\x00\x00\x00\x00', + b'8965B48210\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'], + (Ecu.dsu, 0x791, None): [ + b'881510E01100\x00\x00\x00\x00', + b'881510E01200\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0E01200\x00\x00\x00\x00', + b'8646F0E01300\x00\x00\x00\x00', + ], + }, + CAR.HIGHLANDERH: { + (Ecu.eps, 0x7a1, None): [ + b'8965B48160\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152648541\x00\x00\x00\x00\x00\x00', + b'F152648542\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0230E40000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230E40100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230EA2000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0230EA2100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0E01200\x00\x00\x00\x00', + b'8646F0E01300\x00\x00\x00\x00', + ], + }, + CAR.HIGHLANDER_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B48241\x00\x00\x00\x00\x00\x00', + b'8965B48310\x00\x00\x00\x00\x00\x00', + b'8965B48320\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15260E051\x00\x00\x00\x00\x00\x00', + b'\x01F15260E061\x00\x00\x00\x00\x00\x00', + b'\x01F15260E110\x00\x00\x00\x00\x00\x00', + b'\x01F15260E170\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896630E62100\x00\x00\x00\x00', + b'\x01896630E62200\x00\x00\x00\x00', + b'\x01896630E64100\x00\x00\x00\x00', + b'\x01896630E64200\x00\x00\x00\x00', + b'\x01896630EB1000\x00\x00\x00\x00', + b'\x01896630EB1100\x00\x00\x00\x00', + b'\x01896630EB1200\x00\x00\x00\x00', + b'\x01896630EB2000\x00\x00\x00\x00', + b'\x01896630EB2100\x00\x00\x00\x00', + b'\x01896630EB2200\x00\x00\x00\x00', + b'\x01896630EC4000\x00\x00\x00\x00', + b'\x01896630ED9000\x00\x00\x00\x00', + b'\x01896630ED9100\x00\x00\x00\x00', + b'\x01896630EE1000\x00\x00\x00\x00', + b'\x01896630EE1100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.HIGHLANDERH_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B48241\x00\x00\x00\x00\x00\x00', + b'8965B48310\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15264872300\x00\x00\x00\x00', + b'\x01F15264872400\x00\x00\x00\x00', + b'\x01F15264872500\x00\x00\x00\x00', + b'\x01F15264873500\x00\x00\x00\x00', + b'\x01F152648C6300\x00\x00\x00\x00', + b'\x01F152648J4000\x00\x00\x00\x00', + b'\x01F152648J6000\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896630E67000\x00\x00\x00\x00', + b'\x01896630EA1000\x00\x00\x00\x00', + b'\x01896630EE4000\x00\x00\x00\x00', + b'\x01896630EE4100\x00\x00\x00\x00', + b'\x01896630EE6000\x00\x00\x00\x00', + b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201200\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_IS: { + (Ecu.engine, 0x700, None): [ + b'\x018966353M7000\x00\x00\x00\x00', + b'\x018966353M7100\x00\x00\x00\x00', + b'\x018966353Q2000\x00\x00\x00\x00', + b'\x018966353Q2300\x00\x00\x00\x00', + b'\x018966353Q4000\x00\x00\x00\x00', + b'\x018966353R1100\x00\x00\x00\x00', + b'\x018966353R7100\x00\x00\x00\x00', + b'\x018966353R8100\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0232480000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152653300\x00\x00\x00\x00\x00\x00', + b'F152653301\x00\x00\x00\x00\x00\x00', + b'F152653310\x00\x00\x00\x00\x00\x00', + b'F152653330\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881515306200\x00\x00\x00\x00', + b'881515306400\x00\x00\x00\x00', + b'881515306500\x00\x00\x00\x00', + b'881515307400\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B53270\x00\x00\x00\x00\x00\x00', + b'8965B53271\x00\x00\x00\x00\x00\x00', + b'8965B53280\x00\x00\x00\x00\x00\x00', + b'8965B53281\x00\x00\x00\x00\x00\x00', + b'8965B53311\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F5301101\x00\x00\x00\x00', + b'8646F5301200\x00\x00\x00\x00', + b'8646F5301300\x00\x00\x00\x00', + b'8646F5301400\x00\x00\x00\x00', + ], + }, + CAR.PRIUS: { + (Ecu.engine, 0x700, None): [ + b'\x02896634761000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634761100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634761200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634762000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634763000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634763100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634765000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634765100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634769000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634769100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634769200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634770000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634774000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634774100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634774200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634782000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896634784000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347A0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347A5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347A8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347B0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x03896634759100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', + b'\x03896634759300\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', + b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701002\x00\x00\x00\x00', + b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', + b'\x03896634760100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', + b'\x03896634760300\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', + b'\x03896634768000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703001\x00\x00\x00\x00', + b'\x03896634768000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', + b'\x03896634768100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', + b'\x03896634785000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4705001\x00\x00\x00\x00', + b'\x03896634785000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', + b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4705001\x00\x00\x00\x00', + b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', + b'\x03896634789000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', + b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', + b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707001\x00\x00\x00\x00', + b'\x038966347B6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', + b'\x038966347B7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B47021\x00\x00\x00\x00\x00\x00', + b'8965B47022\x00\x00\x00\x00\x00\x00', + b'8965B47023\x00\x00\x00\x00\x00\x00', + b'8965B47050\x00\x00\x00\x00\x00\x00', + b'8965B47060\x00\x00\x00\x00\x00\x00', # This is the EPS with good angle sensor + ], + (Ecu.abs, 0x7b0, None): [ + b'F152647290\x00\x00\x00\x00\x00\x00', + b'F152647300\x00\x00\x00\x00\x00\x00', + b'F152647310\x00\x00\x00\x00\x00\x00', + b'F152647414\x00\x00\x00\x00\x00\x00', + b'F152647415\x00\x00\x00\x00\x00\x00', + b'F152647416\x00\x00\x00\x00\x00\x00', + b'F152647417\x00\x00\x00\x00\x00\x00', + b'F152647470\x00\x00\x00\x00\x00\x00', + b'F152647490\x00\x00\x00\x00\x00\x00', + b'F152647682\x00\x00\x00\x00\x00\x00', + b'F152647683\x00\x00\x00\x00\x00\x00', + b'F152647684\x00\x00\x00\x00\x00\x00', + b'F152647862\x00\x00\x00\x00\x00\x00', + b'F152647863\x00\x00\x00\x00\x00\x00', + b'F152647864\x00\x00\x00\x00\x00\x00', + b'F152647865\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514702300\x00\x00\x00\x00', + b'881514702400\x00\x00\x00\x00', + b'881514703100\x00\x00\x00\x00', + b'881514704100\x00\x00\x00\x00', + b'881514706000\x00\x00\x00\x00', + b'881514706100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702000\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4701300\x00\x00\x00\x00', + b'8646F4702001\x00\x00\x00\x00', + b'8646F4702100\x00\x00\x00\x00', + b'8646F4702200\x00\x00\x00\x00', + b'8646F4705000\x00\x00\x00\x00', + b'8646F4705200\x00\x00\x00\x00', + ], + }, + CAR.PRIUS_V: { + (Ecu.abs, 0x7b0, None): [ + b'F152647280\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0234781000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514705100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4703300\x00\x00\x00\x00', + ], + }, + CAR.RAV4: { + (Ecu.engine, 0x7e0, None): [ + b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q1100\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q1200\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q1300\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q2000\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q2100\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q2200\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342Q4000\x00\x00\x00\x00\x00\x00\x00\x0054215000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B42063\x00\x00\x00\x00\x00\x00', + b'8965B42073\x00\x00\x00\x00\x00\x00', + b'8965B42082\x00\x00\x00\x00\x00\x00', + b'8965B42083\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F15260R102\x00\x00\x00\x00\x00\x00', + b'F15260R103\x00\x00\x00\x00\x00\x00', + b'F152642493\x00\x00\x00\x00\x00\x00', + b'F152642492\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514201200\x00\x00\x00\x00', + b'881514201300\x00\x00\x00\x00', + b'881514201400\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702000\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4201100\x00\x00\x00\x00', + b'8646F4201200\x00\x00\x00\x00', + b'8646F4202001\x00\x00\x00\x00', + b'8646F4202100\x00\x00\x00\x00', + b'8646F4204000\x00\x00\x00\x00', + ], + }, + CAR.RAV4H: { + (Ecu.engine, 0x7e0, None): [ + b'\x02342N9000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342N9100\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02342P0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B42102\x00\x00\x00\x00\x00\x00', + b'8965B42103\x00\x00\x00\x00\x00\x00', + b'8965B42112\x00\x00\x00\x00\x00\x00', + b'8965B42162\x00\x00\x00\x00\x00\x00', + b'8965B42163\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152642090\x00\x00\x00\x00\x00\x00', + b'F152642110\x00\x00\x00\x00\x00\x00', + b'F152642120\x00\x00\x00\x00\x00\x00', + b'F152642400\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514202200\x00\x00\x00\x00', + b'881514202300\x00\x00\x00\x00', + b'881514202400\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702000\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4201100\x00\x00\x00\x00', + b'8646F4201200\x00\x00\x00\x00', + b'8646F4202001\x00\x00\x00\x00', + b'8646F4202100\x00\x00\x00\x00', + b'8646F4204000\x00\x00\x00\x00', + ], + }, + CAR.RAV4_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x01896630R58000\x00\x00\x00\x00', + b'\x01896630R58100\x00\x00\x00\x00', + b'\x018966342E2000\x00\x00\x00\x00', + b'\x018966342M8000\x00\x00\x00\x00', + b'\x018966342S9000\x00\x00\x00\x00', + b'\x018966342T1000\x00\x00\x00\x00', + b'\x018966342T6000\x00\x00\x00\x00', + b'\x018966342T9000\x00\x00\x00\x00', + b'\x018966342U4000\x00\x00\x00\x00', + b'\x018966342U4100\x00\x00\x00\x00', + b'\x018966342U5100\x00\x00\x00\x00', + b'\x018966342V0000\x00\x00\x00\x00', + b'\x018966342V3000\x00\x00\x00\x00', + b'\x018966342V3100\x00\x00\x00\x00', + b'\x018966342V3200\x00\x00\x00\x00', + b'\x01896634A05000\x00\x00\x00\x00', + b'\x01896634A19000\x00\x00\x00\x00', + b'\x01896634A19100\x00\x00\x00\x00', + b'\x01896634A20000\x00\x00\x00\x00', + b'\x01896634A20100\x00\x00\x00\x00', + b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A22100\x00\x00\x00\x00', + b'\x01896634A30000\x00\x00\x00\x00', + b'\x01896634A44000\x00\x00\x00\x00', + b'\x01896634A45000\x00\x00\x00\x00', + b'\x01896634A46000\x00\x00\x00\x00', + b'\x028966342M7000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00', + b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', + b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', + b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15260R210\x00\x00\x00\x00\x00\x00', + b'\x01F15260R220\x00\x00\x00\x00\x00\x00', + b'\x01F15260R290\x00\x00\x00\x00\x00\x00', + b'\x01F15260R300\x00\x00\x00\x00\x00\x00', + b'\x01F15260R302\x00\x00\x00\x00\x00\x00', + b'\x01F152642551\x00\x00\x00\x00\x00\x00', + b'\x01F152642561\x00\x00\x00\x00\x00\x00', + b'\x01F152642700\x00\x00\x00\x00\x00\x00', + b'\x01F152642701\x00\x00\x00\x00\x00\x00', + b'\x01F152642710\x00\x00\x00\x00\x00\x00', + b'\x01F152642711\x00\x00\x00\x00\x00\x00', + b'\x01F152642750\x00\x00\x00\x00\x00\x00', + b'\x01F152642751\x00\x00\x00\x00\x00\x00', + b'\x01F15260R292\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B42170\x00\x00\x00\x00\x00\x00', + b'8965B42171\x00\x00\x00\x00\x00\x00', + b'8965B42180\x00\x00\x00\x00\x00\x00', + b'8965B42181\x00\x00\x00\x00\x00\x00', + b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', + b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00', + b'\x028965B0R01400\x00\x00\x00\x008965B0R02400\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + ], + }, + CAR.RAV4_TSS2_2022: { + (Ecu.abs, 0x7b0, None): [ + b'\x01F15260R350\x00\x00\x00\x00\x00\x00', + b'\x01F15260R361\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896634AA0000\x00\x00\x00\x00', + b'\x01896634AA1000\x00\x00\x00\x00', + b'\x01896634A88000\x00\x00\x00\x00', + b'\x01896634A89000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F0R01100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', + ], + }, + CAR.RAV4H_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x01896634A15000\x00\x00\x00\x00', + b'\x018966342M5000\x00\x00\x00\x00', + b'\x018966342W8000\x00\x00\x00\x00', + b'\x018966342X5000\x00\x00\x00\x00', + b'\x018966342X6000\x00\x00\x00\x00', + b'\x01896634A25000\x00\x00\x00\x00', + b'\x018966342W5000\x00\x00\x00\x00', + b'\x018966342W7000\x00\x00\x00\x00', + b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A13000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A23000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152642291\x00\x00\x00\x00\x00\x00', + b'F152642290\x00\x00\x00\x00\x00\x00', + b'F152642322\x00\x00\x00\x00\x00\x00', + b'F152642330\x00\x00\x00\x00\x00\x00', + b'F152642331\x00\x00\x00\x00\x00\x00', + b'F152642531\x00\x00\x00\x00\x00\x00', + b'F152642532\x00\x00\x00\x00\x00\x00', + b'F152642520\x00\x00\x00\x00\x00\x00', + b'F152642521\x00\x00\x00\x00\x00\x00', + b'F152642540\x00\x00\x00\x00\x00\x00', + b'F152642541\x00\x00\x00\x00\x00\x00', + b'F152642542\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B42170\x00\x00\x00\x00\x00\x00', + b'8965B42171\x00\x00\x00\x00\x00\x00', + b'8965B42180\x00\x00\x00\x00\x00\x00', + b'8965B42181\x00\x00\x00\x00\x00\x00', + b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', + b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00', + b'\x028965B0R01400\x00\x00\x00\x008965B0R02400\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + ], + }, + CAR.RAV4H_TSS2_2022: { + (Ecu.abs, 0x7b0, None): [ + b'\x01F15264283100\x00\x00\x00\x00', + b'\x01F15264286200\x00\x00\x00\x00', + b'\x01F15264286100\x00\x00\x00\x00', + b'\x01F15264283200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', + b'8965B42182\x00\x00\x00\x00\x00\x00', + b'8965B42172\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896634A02001\x00\x00\x00\x00', + b'\x01896634A03000\x00\x00\x00\x00', + b'\x01896634A08000\x00\x00\x00\x00', + b'\x01896634A61000\x00\x00\x00\x00', + b'\x01896634A62000\x00\x00\x00\x00', + b'\x01896634A62100\x00\x00\x00\x00', + b'\x01896634A63000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F0R01100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', + ], + }, + CAR.SIENNA: { + (Ecu.engine, 0x700, None): [ + b'\x01896630832100\x00\x00\x00\x00', + b'\x01896630832200\x00\x00\x00\x00', + b'\x01896630838000\x00\x00\x00\x00', + b'\x01896630838100\x00\x00\x00\x00', + b'\x01896630842000\x00\x00\x00\x00', + b'\x01896630843000\x00\x00\x00\x00', + b'\x01896630851000\x00\x00\x00\x00', + b'\x01896630851100\x00\x00\x00\x00', + b'\x01896630851200\x00\x00\x00\x00', + b'\x01896630852000\x00\x00\x00\x00', + b'\x01896630852100\x00\x00\x00\x00', + b'\x01896630859000\x00\x00\x00\x00', + b'\x01896630860000\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B45070\x00\x00\x00\x00\x00\x00', + b'8965B45080\x00\x00\x00\x00\x00\x00', + b'8965B45082\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152608130\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881510801100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702200\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F0801100\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_CTH: { + (Ecu.dsu, 0x791, None): [ + b'881517601100\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152676144\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0237635000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F7601100\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_ES_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x018966306U6000\x00\x00\x00\x00', + b'\x01896630EC9100\x00\x00\x00\x00', + b'\x018966333T5000\x00\x00\x00\x00', + b'\x018966333T5100\x00\x00\x00\x00', + b'\x018966333X6000\x00\x00\x00\x00', + b'\x01896633T07000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F152606281\x00\x00\x00\x00\x00\x00', + b'\x01F152606340\x00\x00\x00\x00\x00\x00', + b'\x01F152606461\x00\x00\x00\x00\x00\x00', + b'\x01F15260E031\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33252\x00\x00\x00\x00\x00\x00', + b'8965B33590\x00\x00\x00\x00\x00\x00', + b'8965B33690\x00\x00\x00\x00\x00\x00', + b'8965B33721\x00\x00\x00\x00\x00\x00', + b'8965B48271\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_ESH_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', + b'\x01896633T38000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152633423\x00\x00\x00\x00\x00\x00', + b'F152633680\x00\x00\x00\x00\x00\x00', + b'F152633681\x00\x00\x00\x00\x00\x00', + b'F152633F50\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33252\x00\x00\x00\x00\x00\x00', + b'8965B33590\x00\x00\x00\x00\x00\x00', + b'8965B33690\x00\x00\x00\x00\x00\x00', + b'8965B33721\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_ESH: { + (Ecu.engine, 0x7e0, None): [ + b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152633171\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881513310400\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33512\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701100\x00\x00\x00\x00', + b'8821F4701300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F3302001\x00\x00\x00\x00', + b'8646F3302200\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_NX: { + (Ecu.engine, 0x700, None): [ + b'\x01896637850000\x00\x00\x00\x00', + b'\x01896637851000\x00\x00\x00\x00', + b'\x01896637852000\x00\x00\x00\x00', + b'\x01896637854000\x00\x00\x00\x00', + b'\x01896637878000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152678130\x00\x00\x00\x00\x00\x00', + b'F152678140\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881517803100\x00\x00\x00\x00', + b'881517803300\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78060\x00\x00\x00\x00\x00\x00', + b'8965B78080\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F7801100\x00\x00\x00\x00', + b'8646F7801300\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_NX_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x018966378B2100\x00\x00\x00\x00', + b'\x018966378B3000\x00\x00\x00\x00', + b'\x018966378G3000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F152678221\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78120\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b"\x018821F3301400\x00\x00\x00\x00", + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F7803100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_NXH_TSS2: { + (Ecu.engine, 0x7e0, None): [ + b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152678210\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78120\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_NXH: { + (Ecu.engine, 0x7e0, None): [ + b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237842000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237880000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152678160\x00\x00\x00\x00\x00\x00', + b'F152678170\x00\x00\x00\x00\x00\x00', + b'F152678171\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881517804300\x00\x00\x00\x00', + b'881517804100\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B78060\x00\x00\x00\x00\x00\x00', + b'8965B78080\x00\x00\x00\x00\x00\x00', + b'8965B78100\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + b'8821F4702100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F7801300\x00\x00\x00\x00', + b'8646F7801100\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_RC: { + (Ecu.engine, 0x7e0, None): [ + b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152624221\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881512409100\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B24081\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F2402200\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_RX: { + (Ecu.engine, 0x700, None): [ + b'\x01896630E36200\x00\x00\x00\x00', + b'\x01896630E36300\x00\x00\x00\x00', + b'\x01896630E37200\x00\x00\x00\x00', + b'\x01896630E37300\x00\x00\x00\x00', + b'\x01896630E41000\x00\x00\x00\x00', + b'\x01896630E41100\x00\x00\x00\x00', + b'\x01896630E41200\x00\x00\x00\x00', + b'\x01896630E41500\x00\x00\x00\x00', + b'\x01896630EA3100\x00\x00\x00\x00', + b'\x01896630EA3400\x00\x00\x00\x00', + b'\x01896630EA4100\x00\x00\x00\x00', + b'\x01896630EA4300\x00\x00\x00\x00', + b'\x01896630EA4400\x00\x00\x00\x00', + b'\x01896630EA6300\x00\x00\x00\x00', + b'\x018966348R1300\x00\x00\x00\x00', + b'\x018966348R8500\x00\x00\x00\x00', + b'\x018966348W1300\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152648472\x00\x00\x00\x00\x00\x00', + b'F152648473\x00\x00\x00\x00\x00\x00', + b'F152648492\x00\x00\x00\x00\x00\x00', + b'F152648493\x00\x00\x00\x00\x00\x00', + b'F152648474\x00\x00\x00\x00\x00\x00', + b'F152648630\x00\x00\x00\x00\x00\x00', + b'F152648494\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514810300\x00\x00\x00\x00', + b'881514810500\x00\x00\x00\x00', + b'881514810700\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B0E011\x00\x00\x00\x00\x00\x00', + b'8965B0E012\x00\x00\x00\x00\x00\x00', + b'8965B48102\x00\x00\x00\x00\x00\x00', + b'8965B48111\x00\x00\x00\x00\x00\x00', + b'8965B48112\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701000\x00\x00\x00\x00', + b'8821F4701100\x00\x00\x00\x00', + b'8821F4701200\x00\x00\x00\x00', + b'8821F4701300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4801100\x00\x00\x00\x00', + b'8646F4801200\x00\x00\x00\x00', + b'8646F4802001\x00\x00\x00\x00', + b'8646F4802100\x00\x00\x00\x00', + b'8646F4802200\x00\x00\x00\x00', + b'8646F4809000\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_RXH: { + (Ecu.engine, 0x7e0, None): [ + b'\x02348J7000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348N0000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348Q4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348Q4100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348T1100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348T3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348V6000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348Z3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152648361\x00\x00\x00\x00\x00\x00', + b'F152648501\x00\x00\x00\x00\x00\x00', + b'F152648502\x00\x00\x00\x00\x00\x00', + b'F152648504\x00\x00\x00\x00\x00\x00', + b'F152648740\x00\x00\x00\x00\x00\x00', + b'F152648A30\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514811300\x00\x00\x00\x00', + b'881514811500\x00\x00\x00\x00', + b'881514811700\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B0E011\x00\x00\x00\x00\x00\x00', + b'8965B0E012\x00\x00\x00\x00\x00\x00', + b'8965B48111\x00\x00\x00\x00\x00\x00', + b'8965B48112\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701000\x00\x00\x00\x00', + b'8821F4701100\x00\x00\x00\x00', + b'8821F4701200\x00\x00\x00\x00', + b'8821F4701300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4801200\x00\x00\x00\x00', + b'8646F4802001\x00\x00\x00\x00', + b'8646F4802100\x00\x00\x00\x00', + b'8646F4802200\x00\x00\x00\x00', + b'8646F4809000\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_RX_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x01896630EA9000\x00\x00\x00\x00', + b'\x01896630EB0000\x00\x00\x00\x00', + b'\x01896630EC9000\x00\x00\x00\x00', + b'\x01896630ED0000\x00\x00\x00\x00', + b'\x01896630ED0100\x00\x00\x00\x00', + b'\x01896630ED6000\x00\x00\x00\x00', + b'\x018966348W5100\x00\x00\x00\x00', + b'\x018966348W9000\x00\x00\x00\x00', + b'\x01896634D12000\x00\x00\x00\x00', + b'\x01896634D12100\x00\x00\x00\x00', + b'\x01896634D43000\x00\x00\x00\x00', + b'\x01896634D44000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15260E031\x00\x00\x00\x00\x00\x00', + b'\x01F15260E041\x00\x00\x00\x00\x00\x00', + b'\x01F152648781\x00\x00\x00\x00\x00\x00', + b'\x01F152648801\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B48261\x00\x00\x00\x00\x00\x00', + b'8965B48271\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4810400\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.LEXUS_RXH_TSS2: { + (Ecu.engine, 0x7e0, None): [ + b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152648831\x00\x00\x00\x00\x00\x00', + b'F152648891\x00\x00\x00\x00\x00\x00', + b'F152648D00\x00\x00\x00\x00\x00\x00', + b'F152648D60\x00\x00\x00\x00\x00\x00', + b'F152648811\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B48271\x00\x00\x00\x00\x00\x00', + b'8965B48261\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, + CAR.PRIUS_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x028966347B1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347C4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', + b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', + b'\x038966347C5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', + b'\x038966347C5100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152647500\x00\x00\x00\x00\x00\x00', + b'F152647510\x00\x00\x00\x00\x00\x00', + b'F152647520\x00\x00\x00\x00\x00\x00', + b'F152647521\x00\x00\x00\x00\x00\x00', + b'F152647531\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B47070\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4707000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4710000\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + b'\x028646F4712000\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', + ], + }, + CAR.MIRAI: { + (Ecu.abs, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',], + (Ecu.abs, 0x7B0, None): [ # a second ABS ECU + b'\x01F15266203200\x00\x00\x00\x00', + b'\x01F15266203500\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7A1, None): [b'\x028965B6204100\x00\x00\x00\x008965B6203100\x00\x00\x00\x00',], + (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F6201200\x00\x00\x00\x00',], + (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',], + }, + CAR.ALPHARD_TSS2: { + (Ecu.engine, 0x7e0, None): [ + b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B58040\x00\x00\x00\x00\x00\x00', + b'8965B58052\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, + CAR.ALPHARDH_TSS2: { + (Ecu.engine, 0x7e0, None): [ + b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B58040\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'F152658341\x00\x00\x00\x00\x00\x00' + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646FV201000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], + }, +} + +STEER_THRESHOLD = 100 + +DBC = { + CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), + CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), + CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_NXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.ALPHARDH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), +} + +# These cars have non-standard EPS torque scale factors. All others are 73 +EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) + +# Toyota/Lexus Safety Sense 2.0 and 2.5 +TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} + +NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} + +# these cars have a radar which sends ACC messages instead of the camera +RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022} + +EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, + CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, + CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} + +# no resume button press required +NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py new file mode 100755 index 00000000000000..fba0c54eba165f --- /dev/null +++ b/selfdrive/car/vin.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import re +import traceback + +import cereal.messaging as messaging +from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from selfdrive.car.fw_query_definitions import StdQueries +from system.swaglog import cloudlog + +VIN_UNKNOWN = "0" * 17 +VIN_RE = "[A-HJ-NPR-Z0-9]{17}" + + +def is_valid_vin(vin: str): + return re.fullmatch(VIN_RE, vin) is not None + + +def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): + addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1] # engine, VMCU, 29-bit engine, PGM-FI + for i in range(retry): + for request, response in ((StdQueries.UDS_VIN_REQUEST, StdQueries.UDS_VIN_RESPONSE), (StdQueries.OBD_VIN_REQUEST, StdQueries.OBD_VIN_RESPONSE)): + try: + query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], debug=debug) + for (addr, rx_addr), vin in query.get_data(timeout).items(): + + # Honda Bosch response starts with a length, trim to correct length + if vin.startswith(b'\x11'): + vin = vin[1:18] + + return addr[0], rx_addr, vin.decode() + print(f"vin query retry ({i+1}) ...") + except Exception: + cloudlog.warning(f"VIN query exception: {traceback.format_exc()}") + + return 0, 0, VIN_UNKNOWN + + +if __name__ == "__main__": + import time + sendcan = messaging.pub_sock('sendcan') + logcan = messaging.sub_sock('can') + time.sleep(1) + addr, vin_rx_addr, vin = get_vin(logcan, sendcan, 1, debug=False) + print(f'TX: {hex(addr)}, RX: {hex(vin_rx_addr)}, VIN: {vin}') diff --git a/system/tests/__init__.py b/selfdrive/car/volkswagen/__init__.py similarity index 100% rename from system/tests/__init__.py rename to selfdrive/car/volkswagen/__init__.py diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py new file mode 100644 index 00000000000000..5624c3dd5fcfde --- /dev/null +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -0,0 +1,106 @@ +from cereal import car +from opendbc.can.packer import CANPacker +from common.numpy_fast import clip +from common.conversions import Conversions as CV +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.volkswagen import mqbcan, pqcan +from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.CCP = CarControllerParams(CP) + self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan + self.packer_pt = CANPacker(dbc_name) + + self.apply_steer_last = 0 + self.gra_acc_counter_last = None + self.frame = 0 + self.hcaSameTorqueCount = 0 + self.hcaEnabledFrameCount = 0 + + def update(self, CC, CS, ext_bus): + actuators = CC.actuators + hud_control = CC.hudControl + + can_sends = [] + + # **** Steering Controls ************************************************ # + + if self.frame % self.CCP.HCA_STEP == 0: + # Logic to avoid HCA state 4 "refused": + # * Don't steer unless HCA is in state 3 "ready" or 5 "active" + # * Don't steer at standstill + # * Don't send > 3.00 Newton-meters torque + # * Don't send the same torque for > 6 seconds + # * Don't send uninterrupted steering for > 360 seconds + # One frame of HCA disabled is enough to reset the timer, without zeroing the + # torque value. Do that anytime we happen to have 0 torque, or failing that, + # when exceeding ~1/3 the 360 second timer. + + if CC.latActive: + new_steer = int(round(actuators.steer * self.CCP.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) + if apply_steer == 0: + hcaEnabled = False + self.hcaEnabledFrameCount = 0 + else: + self.hcaEnabledFrameCount += 1 + if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.HCA_STEP): # 118s + hcaEnabled = False + self.hcaEnabledFrameCount = 0 + else: + hcaEnabled = True + if self.apply_steer_last == apply_steer: + self.hcaSameTorqueCount += 1 + if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.HCA_STEP): # 1.9s + apply_steer -= (1, -1)[apply_steer < 0] + self.hcaSameTorqueCount = 0 + else: + self.hcaSameTorqueCount = 0 + else: + hcaEnabled = False + apply_steer = 0 + + self.apply_steer_last = apply_steer + can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled)) + + # **** Acceleration Controls ******************************************** # + + if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: + tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 + can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel)) + + # **** HUD Controls ***************************************************** # + + if self.frame % self.CCP.LDW_STEP == 0: + hud_alert = 0 + if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw): + hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"] + can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, + CS.out.steeringPressed, hud_alert, hud_control)) + + if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? + can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, + hud_control.leadVisible)) + + # **** Stock ACC Button Controls **************************************** # + + gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last + if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume): + counter = (CS.gra_stock_values["COUNTER"] + 1) % 16 + can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter, + cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) + + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX + + self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py new file mode 100644 index 00000000000000..facc740a153a15 --- /dev/null +++ b/selfdrive/car/volkswagen/carstate.py @@ -0,0 +1,516 @@ +import numpy as np +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car.interfaces import CarStateBase +from opendbc.can.parser import CANParser +from selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ + CarControllerParams + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.CCP = CarControllerParams(CP) + self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} + + def create_button_events(self, pt_cp, buttons): + button_events = [] + + for button in buttons: + state = pt_cp.vl[button.can_addr][button.can_msg] in button.values + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + button_events.append(event) + self.button_states[button.event_type] = state + + return button_events + + def update(self, pt_cp, cam_cp, ext_cp, trans_type): + if self.CP.carFingerprint in PQ_CARS: + return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) + + ret = car.CarState.new_message() + # Update vehicle speed and acceleration from ABS wheel speeds. + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"], + pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"], + pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"], + pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"], + ) + + ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgo < 0.1 + + # Update steering angle, rate, yaw rate, and driver input torque. VW send + # the sign/direction in a separate signal so they must be recombined. + ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] + ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] + ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] + ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE + ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD + + # Verify EPS readiness to accept steering commands + hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) + ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") + ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + + # Update gas, brakes, and gearshift. + ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0 + ret.gasPressed = ret.gas > 0 + ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well + + # Update gear and/or clutch position data. + if trans_type == TransmissionType.automatic: + ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"], None)) + elif trans_type == TransmissionType.direct: + ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["EV_Gearshift"]["GearPosition"], None)) + elif trans_type == TransmissionType.manual: + ret.clutchPressed = not pt_cp.vl["Motor_14"]["MO_Kuppl_schalter"] + if bool(pt_cp.vl["Gateway_72"]["BCM1_Rueckfahrlicht_Schalter"]): + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.drive + + # Update door and trunk/hatch lid open status. + ret.doorOpen = any([pt_cp.vl["Gateway_72"]["ZV_FT_offen"], + pt_cp.vl["Gateway_72"]["ZV_BT_offen"], + pt_cp.vl["Gateway_72"]["ZV_HFS_offen"], + pt_cp.vl["Gateway_72"]["ZV_HBFS_offen"], + pt_cp.vl["Gateway_72"]["ZV_HD_offen"]]) + + # Update seatbelt fastened status. + ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3 + + # Consume blind-spot monitoring info/warning LED states, if available. + # Infostufe: BSM LED on, Warnung: BSM LED flashing + if self.CP.enableBsm: + ret.leftBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"]) + ret.rightBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"]) + + # Consume factory LDW data relevant for factory SWA (Lane Change Assist) + # and capture it for forwarding to the blind spot radar controller + self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {} + + # Stock FCW is considered active if the release bit for brake-jerk warning + # is set. Stock AEB considered active if the partial braking or target + # braking release bits are set. + # Refer to VW Self Study Program 890253: Volkswagen Driver Assistance + # Systems, chapter on Front Assist with Braking: Golf Family for all MQB + ret.stockFcw = bool(ext_cp.vl["ACC_10"]["AWV2_Freigabe"]) + ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) + + # Update ACC radar status. + if pt_cp.vl["TSK_06"]["TSK_Status"] == 2: + # ACC okay and enabled, but not currently engaged + ret.cruiseState.available = True + ret.cruiseState.enabled = False + elif pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5): + # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) + ret.cruiseState.available = True + ret.cruiseState.enabled = True + else: + # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) + ret.cruiseState.available = False + ret.cruiseState.enabled = False + ret.cruiseState.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) + + # Update ACC setpoint. When the setpoint is zero or there's an error, the + # radar sends a set-speed of ~90.69 m/s / 203mph. + if self.CP.pcmCruise: + ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS + if ret.cruiseState.speed > 90: + ret.cruiseState.speed = 0 + + # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough + ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) + ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) + ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) + self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] + + # Additional safety checks performed in CarInterface. + ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + + return ret + + def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): + ret = car.CarState.new_message() + # Update vehicle speed and acceleration from ABS wheel speeds. + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__VR_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__HL_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__HR_4_1"], + ) + + # vEgo obtained from Bremse_1 vehicle speed rather than Bremse_3 wheel speeds because Bremse_3 isn't present on NSF + ret.vEgoRaw = pt_cp.vl["Bremse_1"]["Geschwindigkeit_neu__Bremse_1_"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgo < 0.1 + + # Update steering angle, rate, yaw rate, and driver input torque. VW send + # the sign/direction in a separate signal so they must be recombined. + ret.steeringAngleDeg = pt_cp.vl["Lenkhilfe_3"]["LH3_BLW"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"])] + ret.steeringRateDeg = pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])] + ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] + ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE + ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD + + # Verify EPS readiness to accept steering commands + hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) + ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") + ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + + # Update gas, brakes, and gearshift. + ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 + ret.gasPressed = ret.gas > 0 + ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"]) + ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) + + # Update gear and/or clutch position data. + if trans_type == TransmissionType.automatic: + ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"], None)) + elif trans_type == TransmissionType.manual: + ret.clutchPressed = not pt_cp.vl["Motor_1"]["Kupplungsschalter"] + reverse_light = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Rueckfahr"]) + if reverse_light: + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.drive + + # Update door and trunk/hatch lid open status. + ret.doorOpen = any([pt_cp.vl["Gate_Komf_1"]["GK1_Fa_Tuerkont"], + pt_cp.vl["Gate_Komf_1"]["BSK_BT_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HL_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HR_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HD_Hauptraste"]]) + + # Update seatbelt fastened status. + ret.seatbeltUnlatched = not bool(pt_cp.vl["Airbag_1"]["Gurtschalter_Fahrer"]) + + # Consume blind-spot monitoring info/warning LED states, if available. + # Infostufe: BSM LED on, Warnung: BSM LED flashing + if self.CP.enableBsm: + ret.leftBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_li"]) + ret.rightBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_re"]) + + # Consume factory LDW data relevant for factory SWA (Lane Change Assist) + # and capture it for forwarding to the blind spot radar controller + self.ldw_stock_values = cam_cp.vl["LDW_Status"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {} + + # Stock FCW is considered active if the release bit for brake-jerk warning + # is set. Stock AEB considered active if the partial braking or target + # braking release bits are set. + # Refer to VW Self Study Program 890253: Volkswagen Driver Assistance + # Systems, chapters on Front Assist with Braking and City Emergency + # Braking for the 2016 Passat NMS + # TODO: deferred until we can collect data on pre-MY2016 behavior, AWV message may be shorter with fewer signals + ret.stockFcw = False + ret.stockAeb = False + + # Update ACC radar status. + ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) + ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) + if self.CP.pcmCruise: + ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7) + # TODO: update opendbc with PQ TSK state for OP long accFaulted + + # Update ACC setpoint. When the setpoint reads as 255, the driver has not + # yet established an ACC setpoint, so treat it as zero. + ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anziege"]["ACA_V_Wunsch"] * CV.KPH_TO_MS + if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint + ret.cruiseState.speed = 0 + + # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], + pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) + ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) + self.gra_stock_values = pt_cp.vl["GRA_Neu"] + + # Additional safety checks performed in CarInterface. + ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + + return ret + + @staticmethod + def get_can_parser(CP): + if CP.carFingerprint in PQ_CARS: + return CarState.get_can_parser_pq(CP) + + signals = [ + # sig_name, sig_address + ("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle + ("LWI_VZ_Lenkradwinkel", "LWI_01"), # Steering angle sign + ("LWI_Lenkradw_Geschw", "LWI_01"), # Absolute steering rate + ("LWI_VZ_Lenkradw_Geschw", "LWI_01"), # Steering rate sign + ("ESP_VL_Radgeschw_02", "ESP_19"), # ABS wheel speed, front left + ("ESP_VR_Radgeschw_02", "ESP_19"), # ABS wheel speed, front right + ("ESP_HL_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear left + ("ESP_HR_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear right + ("ESP_Gierrate", "ESP_02"), # Absolute yaw rate + ("ESP_VZ_Gierrate", "ESP_02"), # Yaw rate sign + ("ZV_FT_offen", "Gateway_72"), # Door open, driver + ("ZV_BT_offen", "Gateway_72"), # Door open, passenger + ("ZV_HFS_offen", "Gateway_72"), # Door open, rear left + ("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right + ("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open + ("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval + ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval + ("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver + ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger + ("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed + ("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied + ("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value + ("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input + ("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign + ("EPS_HCA_Status", "LH_EPS_03"), # EPS HCA control status + ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled + ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation + ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator + ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off + ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel + ("GRA_Tip_Setzen", "GRA_ACC_01"), # ACC button, set + ("GRA_Tip_Hoch", "GRA_ACC_01"), # ACC button, increase or accel + ("GRA_Tip_Runter", "GRA_ACC_01"), # ACC button, decrease or decel + ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume + ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj + ("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type + ("GRA_Codierung", "GRA_ACC_01"), # ACC button configuration/coding + ("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type + ("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type + ("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter + ] + + checks = [ + # sig_address, frequency + ("LWI_01", 100), # From J500 Steering Assist with integrated sensors + ("LH_EPS_03", 100), # From J500 Steering Assist with integrated sensors + ("ESP_19", 100), # From J104 ABS/ESP controller + ("ESP_05", 50), # From J104 ABS/ESP controller + ("ESP_21", 50), # From J104 ABS/ESP controller + ("Motor_20", 50), # From J623 Engine control module + ("TSK_06", 50), # From J623 Engine control module + ("ESP_02", 50), # From J104 ABS/ESP controller + ("GRA_ACC_01", 33), # From J533 CAN gateway (via LIN from steering wheel controls) + ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) + ("Airbag_02", 5), # From J234 Airbag control module + ("Kombi_01", 2), # From J285 Instrument cluster + ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) + ] + + if CP.transmissionType == TransmissionType.automatic: + signals.append(("GE_Fahrstufe", "Getriebe_11")) # Auto trans gear selector position + checks.append(("Getriebe_11", 20)) # From J743 Auto transmission control module + elif CP.transmissionType == TransmissionType.direct: + signals.append(("GearPosition", "EV_Gearshift")) # EV gear selector position + checks.append(("EV_Gearshift", 10)) # From J??? unknown EV control module + elif CP.transmissionType == TransmissionType.manual: + signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch + ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM + checks.append(("Motor_14", 10)) # From J623 Engine control module + + if CP.networkLocation == NetworkLocation.fwdCamera: + # Radars are here on CANBUS.pt + signals += MqbExtraSignals.fwd_radar_signals + checks += MqbExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += MqbExtraSignals.bsm_radar_signals + checks += MqbExtraSignals.bsm_radar_checks + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt) + + @staticmethod + def get_cam_can_parser(CP): + if CP.carFingerprint in PQ_CARS: + return CarState.get_cam_can_parser_pq(CP) + + signals = [] + checks = [] + + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + # sig_name, sig_address + ("LDW_SW_Warnung_links", "LDW_02"), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_02"), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_02"), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_02"), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_02"), # Lane departure, time to line crossing + ] + checks += [ + # sig_address, frequency + ("LDW_02", 10) # From R242 Driver assistance camera + ] + else: + # Radars are here on CANBUS.cam + signals += MqbExtraSignals.fwd_radar_signals + checks += MqbExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += MqbExtraSignals.bsm_radar_signals + checks += MqbExtraSignals.bsm_radar_checks + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam) + + @staticmethod + def get_can_parser_pq(CP): + signals = [ + # sig_name, sig_address, default + ("LH3_BLW", "Lenkhilfe_3"), # Absolute steering angle + ("LH3_BLWSign", "Lenkhilfe_3"), # Steering angle sign + ("LH3_LM", "Lenkhilfe_3"), # Absolute driver torque input + ("LH3_LMSign", "Lenkhilfe_3"), # Driver torque input sign + ("LH2_Sta_HCA", "Lenkhilfe_2"), # Steering rack HCA status + ("Lenkradwinkel_Geschwindigkeit", "Lenkwinkel_1"), # Absolute steering rate + ("Lenkradwinkel_Geschwindigkeit_S", "Lenkwinkel_1"), # Steering rate sign + ("Geschwindigkeit_neu__Bremse_1_", "Bremse_1"), # Vehicle speed from ABS + ("Radgeschw__VL_4_1", "Bremse_3"), # ABS wheel speed, front left + ("Radgeschw__VR_4_1", "Bremse_3"), # ABS wheel speed, front right + ("Radgeschw__HL_4_1", "Bremse_3"), # ABS wheel speed, rear left + ("Radgeschw__HR_4_1", "Bremse_3"), # ABS wheel speed, rear right + ("Giergeschwindigkeit", "Bremse_5"), # Absolute yaw rate + ("Vorzeichen_der_Giergeschwindigk", "Bremse_5"), # Yaw rate sign + ("Gurtschalter_Fahrer", "Airbag_1"), # Seatbelt status, driver + ("Gurtschalter_Beifahrer", "Airbag_1"), # Seatbelt status, passenger + ("Bremstestschalter", "Motor_2"), # Brake pedal pressed (brake light test switch) + ("Bremslichtschalter", "Motor_2"), # Brakes applied (brake light switch) + ("Bremsdruck", "Bremse_5"), # Brake pressure applied + ("Vorzeichen_Bremsdruck", "Bremse_5"), # Brake pressure applied sign (???) + ("Fahrpedal_Rohsignal", "Motor_3"), # Accelerator pedal value + ("ESP_Passiv_getastet", "Bremse_1"), # Stability control disabled + ("GRA_Hauptschalter", "Motor_5"), # ACC main switch + ("GRA_Status", "Motor_2"), # ACC engagement status + ("GK1_Fa_Tuerkont", "Gate_Komf_1"), # Door open, driver + ("BSK_BT_geoeffnet", "Gate_Komf_1"), # Door open, passenger + ("BSK_HL_geoeffnet", "Gate_Komf_1"), # Door open, rear left + ("BSK_HR_geoeffnet", "Gate_Komf_1"), # Door open, rear right + ("BSK_HD_Hauptraste", "Gate_Komf_1"), # Trunk or hatch open + ("GK1_Blinker_li", "Gate_Komf_1"), # Left turn signal on + ("GK1_Blinker_re", "Gate_Komf_1"), # Right turn signal on + ("Bremsinfo", "Kombi_1"), # Manual handbrake applied + ("GRA_Hauptschalt", "GRA_Neu"), # ACC button, on/off + ("GRA_Typ_Hauptschalt", "GRA_Neu"), # ACC button, momentary vs latching + ("GRA_Kodierinfo", "GRA_Neu"), # ACC button, configuration + ("GRA_Abbrechen", "GRA_Neu"), # ACC button, cancel + ("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set + ("GRA_Up_lang", "GRA_Neu"), # ACC button, increase or accel, long press + ("GRA_Down_lang", "GRA_Neu"), # ACC button, decrease or decel, long press + ("GRA_Up_kurz", "GRA_Neu"), # ACC button, increase or accel, short press + ("GRA_Down_kurz", "GRA_Neu"), # ACC button, decrease or decel, short press + ("GRA_Recall", "GRA_Neu"), # ACC button, resume + ("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj + ("COUNTER", "GRA_Neu"), # ACC button, message counter + ("GRA_Sender", "GRA_Neu"), # ACC button, CAN message originator + ] + + checks = [ + # sig_address, frequency + ("Bremse_1", 100), # From J104 ABS/ESP controller + ("Bremse_3", 100), # From J104 ABS/ESP controller + ("Lenkhilfe_3", 100), # From J500 Steering Assist with integrated sensors + ("Lenkwinkel_1", 100), # From J500 Steering Assist with integrated sensors + ("Motor_3", 100), # From J623 Engine control module + ("Airbag_1", 50), # From J234 Airbag control module + ("Bremse_5", 50), # From J104 ABS/ESP controller + ("GRA_Neu", 50), # From J??? steering wheel control buttons + ("Kombi_1", 50), # From J285 Instrument cluster + ("Motor_2", 50), # From J623 Engine control module + ("Motor_5", 50), # From J623 Engine control module + ("Lenkhilfe_2", 20), # From J500 Steering Assist with integrated sensors + ("Gate_Komf_1", 10), # From J533 CAN gateway + ] + + if CP.transmissionType == TransmissionType.automatic: + signals += [("Waehlhebelposition__Getriebe_1_", "Getriebe_1", 0)] # Auto trans gear selector position + checks += [("Getriebe_1", 100)] # From J743 Auto transmission control module + elif CP.transmissionType == TransmissionType.manual: + signals += [("Kupplungsschalter", "Motor_1", 0), # Clutch switch + ("GK1_Rueckfahr", "Gate_Komf_1", 0)] # Reverse light from BCM + checks += [("Motor_1", 100)] # From J623 Engine control module + + if CP.networkLocation == NetworkLocation.fwdCamera: + # Extended CAN devices other than the camera are here on CANBUS.pt + signals += PqExtraSignals.fwd_radar_signals + checks += PqExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += PqExtraSignals.bsm_radar_signals + checks += PqExtraSignals.bsm_radar_checks + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt) + + @staticmethod + def get_cam_can_parser_pq(CP): + + signals = [] + checks = [] + + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + # sig_name, sig_address + ("LDW_SW_Warnung_links", "LDW_Status"), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_Status"), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_Status"), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_Status"), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_Status"), # Lane departure, time to line crossing + ] + checks += [ + # sig_address, frequency + ("LDW_Status", 10) # From R242 Driver assistance camera + ] + + if CP.networkLocation == NetworkLocation.gateway: + # Radars are here on CANBUS.cam + signals += PqExtraSignals.fwd_radar_signals + checks += PqExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += PqExtraSignals.bsm_radar_signals + checks += PqExtraSignals.bsm_radar_checks + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam) + + +class MqbExtraSignals: + # Additional signal and message lists for optional or bus-portable controllers + fwd_radar_signals = [ + ("ACC_Wunschgeschw", "ACC_02"), # ACC set speed + ("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release + ("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release + ("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release + ] + fwd_radar_checks = [ + ("ACC_10", 50), # From J428 ACC radar control module + ("ACC_02", 17), # From J428 ACC radar control module + ] + bsm_radar_signals = [ + ("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_01"), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01"), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right + ] + bsm_radar_checks = [ + ("SWA_01", 20), # From J1086 Lane Change Assist + ] + +class PqExtraSignals: + # Additional signal and message lists for optional or bus-portable controllers + fwd_radar_signals = [ + ("ACA_StaACC", "ACC_GRA_Anziege", 0), # ACC drivetrain coordinator status + ("ACA_V_Wunsch", "ACC_GRA_Anziege", 0), # ACC set speed + ] + fwd_radar_checks = [ + ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module + ] + bsm_radar_signals = [ + ("SWA_Infostufe_SWA_li", "SWA_1", 0), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_1", 0), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_1", 0), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_1", 0), # Blind spot object warning, right + ] + bsm_radar_checks = [ + ("SWA_1", 20), # From J1086 Lane Change Assist + ] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py new file mode 100644 index 00000000000000..3ed7a6244d5d6d --- /dev/null +++ b/selfdrive/car/volkswagen/interface.py @@ -0,0 +1,234 @@ +from cereal import car +from panda import Panda +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, \ + gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter + +EventName = car.CarEvent.EventName + + +class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) + + if CP.networkLocation == NetworkLocation.fwdCamera: + self.ext_bus = CANBUS.pt + self.cp_ext = self.cp + else: + self.ext_bus = CANBUS.cam + self.cp_ext = self.cp_cam + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "volkswagen" + ret.radarOffCan = True + + if candidate in PQ_CARS: + # Set global PQ35/PQ46/NMS parameters + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)] + ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1 + + if 0x440 in fingerprint[0]: # Getriebe_1 + ret.transmissionType = TransmissionType.automatic + else: + ret.transmissionType = TransmissionType.manual + + if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1 + ret.networkLocation = NetworkLocation.gateway + else: + ret.networkLocation = NetworkLocation.fwdCamera + + # The PQ port is in dashcam-only mode due to a fixed six-minute maximum timer on HCA steering. An unsupported + # EPS flash update to work around this timer, and enable steering down to zero, is available from: + # https://github.com/pd0wm/pq-flasher + # It is documented in a four-part blog series: + # https://blog.willemmelching.nl/carhacking/2022/01/02/vw-part1/ + # Panda ALLOW_DEBUG firmware required. + ret.dashcamOnly = True + + if experimental_long and ret.networkLocation == NetworkLocation.gateway: + # Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should + # be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required. + ret.experimentalLongitudinalAvailable = True + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL + + else: + # Set global MQB parameters + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] + ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 + + if 0xAD in fingerprint[0]: # Getriebe_11 + ret.transmissionType = TransmissionType.automatic + elif 0x187 in fingerprint[0]: # EV_Gearshift + ret.transmissionType = TransmissionType.direct + else: + ret.transmissionType = TransmissionType.manual + + if any(msg in fingerprint[1] for msg in (0x40, 0x86, 0xB2, 0xFD)): # Airbag_01, LWI_01, ESP_19, ESP_21 + ret.networkLocation = NetworkLocation.gateway + else: + ret.networkLocation = NetworkLocation.fwdCamera + + # Global lateral tuning defaults, can be overridden per-vehicle + + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.4 + ret.steerRatio = 15.6 # Let the params learner figure this out + tire_stiffness_factor = 1.0 # Let the params learner figure this out + ret.lateralTuning.pid.kpBP = [0.] + ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kpV = [0.6] + ret.lateralTuning.pid.kiV = [0.2] + + # Global longitudinal tuning defaults, can be overridden per-vehicle + + ret.pcmCruise = not ret.openpilotLongitudinalControl + ret.longitudinalActuatorDelayUpperBound = 0.5 # s + ret.longitudinalTuning.kpV = [0.1] + ret.longitudinalTuning.kiV = [0.0] + + # Per-chassis tuning values, override tuning defaults here if desired + + if candidate == CAR.ARTEON_MK1: + ret.mass = 1733 + STD_CARGO_KG + ret.wheelbase = 2.84 + + elif candidate == CAR.ATLAS_MK1: + ret.mass = 2011 + STD_CARGO_KG + ret.wheelbase = 2.98 + + elif candidate == CAR.GOLF_MK7: + ret.mass = 1397 + STD_CARGO_KG + ret.wheelbase = 2.62 + + elif candidate == CAR.JETTA_MK7: + ret.mass = 1328 + STD_CARGO_KG + ret.wheelbase = 2.71 + + elif candidate == CAR.PASSAT_MK8: + ret.mass = 1551 + STD_CARGO_KG + ret.wheelbase = 2.79 + + elif candidate == CAR.PASSAT_NMS: + ret.mass = 1503 + STD_CARGO_KG + ret.wheelbase = 2.80 + ret.minEnableSpeed = 20 * CV.KPH_TO_MS # ACC "basic", no FtS + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + elif candidate == CAR.POLO_MK6: + ret.mass = 1230 + STD_CARGO_KG + ret.wheelbase = 2.55 + + elif candidate == CAR.TAOS_MK1: + ret.mass = 1498 + STD_CARGO_KG + ret.wheelbase = 2.69 + + elif candidate == CAR.TCROSS_MK1: + ret.mass = 1150 + STD_CARGO_KG + ret.wheelbase = 2.60 + + elif candidate == CAR.TIGUAN_MK2: + ret.mass = 1715 + STD_CARGO_KG + ret.wheelbase = 2.74 + + elif candidate == CAR.TOURAN_MK2: + ret.mass = 1516 + STD_CARGO_KG + ret.wheelbase = 2.79 + + elif candidate == CAR.TRANSPORTER_T61: + ret.mass = 1926 + STD_CARGO_KG + ret.wheelbase = 3.00 # SWB, LWB is 3.40, TBD how to detect difference + ret.minSteerSpeed = 14.0 + + elif candidate == CAR.TROC_MK1: + ret.mass = 1413 + STD_CARGO_KG + ret.wheelbase = 2.63 + + elif candidate == CAR.AUDI_A3_MK3: + ret.mass = 1335 + STD_CARGO_KG + ret.wheelbase = 2.61 + + elif candidate == CAR.AUDI_Q2_MK1: + ret.mass = 1205 + STD_CARGO_KG + ret.wheelbase = 2.61 + + elif candidate == CAR.AUDI_Q3_MK2: + ret.mass = 1623 + STD_CARGO_KG + ret.wheelbase = 2.68 + + elif candidate == CAR.SEAT_ATECA_MK1: + ret.mass = 1900 + STD_CARGO_KG + ret.wheelbase = 2.64 + + elif candidate == CAR.SEAT_LEON_MK3: + ret.mass = 1227 + STD_CARGO_KG + ret.wheelbase = 2.64 + + elif candidate == CAR.SKODA_KAMIQ_MK1: + ret.mass = 1265 + STD_CARGO_KG + ret.wheelbase = 2.66 + + elif candidate == CAR.SKODA_KAROQ_MK1: + ret.mass = 1278 + STD_CARGO_KG + ret.wheelbase = 2.66 + + elif candidate == CAR.SKODA_KODIAQ_MK1: + ret.mass = 1569 + STD_CARGO_KG + ret.wheelbase = 2.79 + + elif candidate == CAR.SKODA_OCTAVIA_MK3: + ret.mass = 1388 + STD_CARGO_KG + ret.wheelbase = 2.68 + + elif candidate == CAR.SKODA_SCALA_MK1: + ret.mass = 1192 + STD_CARGO_KG + ret.wheelbase = 2.65 + + elif candidate == CAR.SKODA_SUPERB_MK3: + ret.mass = 1505 + STD_CARGO_KG + ret.wheelbase = 2.84 + + else: + raise ValueError(f"unsupported car {candidate}") + + ret.autoResumeSng = ret.minEnableSpeed == -1 + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.45 + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) + + events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], + pcm_enable=not self.CS.CP.openpilotLongitudinalControl) + + # Low speed steer alert hysteresis logic + if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): + self.low_speed_alert = True + elif ret.vEgo > (self.CP.minSteerSpeed + 2.): + self.low_speed_alert = False + if self.low_speed_alert: + events.add(EventName.belowSteerSpeed) + + if self.CS.CP.openpilotLongitudinalControl: + if ret.vEgo < self.CP.minEnableSpeed + 2.: + events.add(EventName.belowEngageSpeed) + if c.enabled and ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.speedTooLow) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS, self.ext_bus) diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py new file mode 100644 index 00000000000000..3819f4f76ffdc0 --- /dev/null +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -0,0 +1,38 @@ +def create_steering_control(packer, bus, apply_steer, lkas_enabled): + values = { + "SET_ME_0X3": 0x3, + "Assist_Torque": abs(apply_steer), + "Assist_Requested": lkas_enabled, + "Assist_VZ": 1 if apply_steer < 0 else 0, + "HCA_Available": 1, + "HCA_Standby": not lkas_enabled, + "HCA_Active": lkas_enabled, + "SET_ME_0XFE": 0xFE, + "SET_ME_0X07": 0x07, + } + return packer.make_can_msg("HCA_01", bus, values) + + +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): + values = ldw_stock_values.copy() + + values.update({ + "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, + "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, + "LDW_Texte": hud_alert, + }) + return packer.make_can_msg("LDW_02", bus, values) + + +def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False): + values = gra_stock_values.copy() + + values.update({ + "COUNTER": counter, + "GRA_Abbrechen": cancel, + "GRA_Tip_Wiederaufnahme": resume, + }) + + return packer.make_can_msg("GRA_ACC_01", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py new file mode 100644 index 00000000000000..e64bb2246e4fcc --- /dev/null +++ b/selfdrive/car/volkswagen/pqcan.py @@ -0,0 +1,84 @@ +def create_steering_control(packer, bus, apply_steer, lkas_enabled): + values = { + "LM_Offset": abs(apply_steer), + "LM_OffSign": 1 if apply_steer < 0 else 0, + "HCA_Status": 5 if (lkas_enabled and apply_steer != 0) else 3, + "Vib_Freq": 16, + } + + return packer.make_can_msg("HCA_1", bus, values) + + +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): + values = ldw_stock_values.copy() + + values.update({ + "LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0, + "LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, + "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, + "LDW_Textbits": hud_alert, + }) + + return packer.make_can_msg("LDW_Status", bus, values) + + +def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False): + values = gra_stock_values.copy() + + values.update({ + "COUNTER": counter, + "GRA_Abbrechen": cancel, + "GRA_Recall": resume, + }) + + return packer.make_can_msg("GRA_Neu", bus, values) + + +def tsk_status_value(main_switch_on, acc_faulted, long_active): + if long_active: + tsk_status = 1 + elif main_switch_on: + tsk_status = 2 + else: + tsk_status = 0 + + return tsk_status + + +def acc_hud_status_value(main_switch_on, acc_faulted, long_active): + if acc_faulted: + hud_status = 6 + elif long_active: + hud_status = 3 + elif main_switch_on: + hud_status = 2 + else: + hud_status = 0 + + return hud_status + + +def create_acc_accel_control(packer, bus, adr_status, accel): + values = { + "ACS_Sta_ADR": adr_status, + "ACS_StSt_Info": adr_status != 1, + "ACS_Typ_ACC": 0, # TODO: this is ACC "basic", find a way to detect FtS support (1) + "ACS_Sollbeschl": accel if adr_status == 1 else 3.01, + "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27, + "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08, + } + + return packer.make_can_msg("ACC_System", bus, values) + + +def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible): + values = { + "ACA_StaACC": acc_status, + "ACA_Zeitluecke": 2, + "ACA_V_Wunsch": set_speed, + "ACA_gemZeitl": 8 if lead_visible else 0, + } + # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke + + return packer.make_can_msg("ACC_GRA_Anziege", bus, values) diff --git a/selfdrive/car/volkswagen/radar_interface.py b/selfdrive/car/volkswagen/radar_interface.py new file mode 100644 index 00000000000000..b2f76511360320 --- /dev/null +++ b/selfdrive/car/volkswagen/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py new file mode 100755 index 00000000000000..05994c0100d669 --- /dev/null +++ b/selfdrive/car/volkswagen/values.py @@ -0,0 +1,1067 @@ +from collections import defaultdict, namedtuple +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from panda.python import uds +from opendbc.can.can_define import CANDefine +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 + +Ecu = car.CarParams.Ecu +NetworkLocation = car.CarParams.NetworkLocation +TransmissionType = car.CarParams.TransmissionType +GearShifter = car.CarState.GearShifter +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) + + +class CarControllerParams: + HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz + ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz + ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz + + ACCEL_MAX = 2.0 # 2.0 m/s max acceleration + ACCEL_MIN = -3.5 # 3.5 m/s max deceleration + + def __init__(self, CP): + # Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec. + # MQB vs PQ maximums are shared, but rate-of-change limited differently + # based on safety requirements driven by lateral accel testing. + self.STEER_MAX = 300 # Max heading control assist torque 3.00 Nm + self.STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily + self.STEER_DRIVER_FACTOR = 1 # from dbc + + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + if CP.carFingerprint in PQ_CARS: + self.LDW_STEP = 5 # LDW_1 message frequency 20Hz + self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm + self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00)) + self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) + + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"] + self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"] + + self.BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_Neu", "GRA_Neu_Setzen", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_Neu", "GRA_Recall", [1]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]), + Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [1]), + ] + + self.LDW_MESSAGES = { + "none": 0, # Nothing to display + "laneAssistUnavail": 1, # "Lane Assist currently not available." + "laneAssistUnavailSysError": 2, # "Lane Assist system error" + "laneAssistUnavailNoSensorView": 3, # "Lane Assist not available. No sensor view." + "laneAssistTakeOver": 4, # "Lane Assist: Please Take Over Steering" + "laneAssistDeactivTrailer": 5, # "Lane Assist: no function with trailer" + } + + else: + self.LDW_STEP = 10 # LDW_02 message frequency 10Hz + self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm + self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) + self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) + + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] + elif CP.transmissionType == TransmissionType.direct: + self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] + self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] + + self.BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]), + Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [1]), + ] + + self.LDW_MESSAGES = { + "none": 0, # Nothing to display + "laneAssistUnavailChime": 1, # "Lane Assist currently not available." with chime + "laneAssistUnavailNoSensorChime": 3, # "Lane Assist not available. No sensor view." with chime + "laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" with urgent beep + "emergencyAssistUrgent": 6, # "Emergency Assist: Please Take Over Steering" with urgent beep + "laneAssistTakeOverChime": 7, # "Lane Assist: Please Take Over Steering" with chime + "laneAssistTakeOver": 8, # "Lane Assist: Please Take Over Steering" silent + "emergencyAssistChangingLanes": 9, # "Emergency Assist: Changing lanes..." with urgent beep + "laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward + } + + +class CANBUS: + pt = 0 + cam = 2 + + +# Check the 7th and 8th characters of the VIN before adding a new CAR. If the +# chassis code is already listed below, don't add a new CAR, just add to the +# FW_VERSIONS for that existing CAR. +# Exception: SEAT Leon and SEAT Ateca share a chassis code + +class CAR: + ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants + ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport + GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants + JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta + PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants + PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift + POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo + TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu + TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants + TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants + TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants + TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California + TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW VW T-Roc and variants + AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants + AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) + AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants + SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca + SEAT_LEON_MK3 = "SEAT LEON 3RD GEN" # Chassis 5F, Mk3 SEAT Leon and variants + SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN" # Chassis NW, Mk1 Skoda Kamiq + SKODA_KAROQ_MK1 = "SKODA KAROQ 1ST GEN" # Chassis NU, Mk1 Skoda Karoq + SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq + SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN" # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq + SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants + SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants + + +PQ_CARS = {CAR.PASSAT_NMS} + + +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None)) +for car_type in PQ_CARS: + DBC[car_type] = dbc_dict("vw_golf_mk4", None) + + +class Footnote(Enum): + KAMIQ = CarFootnote( + "Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.", + Column.MODEL) + PASSAT = CarFootnote( + "Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.", + Column.MODEL) + VW_HARNESS = CarFootnote( + "Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma " + + "store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black " + + "(older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway " + + "inside the dashboard, choose \"VW J533 Development\" from the vehicle drop-down for a suitable harness. " + + "(Some newer models are also observed to not have a J533 connector.)", + Column.MODEL) + VW_VARIANT = CarFootnote( + "Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.)", + Column.MODEL) + + +@dataclass +class VWCarInfo(CarInfo): + package: str = "Driver Assistance" + harness: Enum = Harness.vw + + +CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { + CAR.ARTEON_MK1: [ + VWCarInfo("Volkswagen Arteon 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen Arteon R 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen Arteon eHybrid 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), + VWCarInfo("Volkswagen CC 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"), + ], + CAR.ATLAS_MK1: [ + VWCarInfo("Volkswagen Atlas 2018-23", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Atlas Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Teramont 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Teramont Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Teramont X 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + ], + CAR.GOLF_MK7: [ + VWCarInfo("Volkswagen e-Golf 2014-20"), + VWCarInfo("Volkswagen Golf 2015-20", footnotes=[Footnote.VW_VARIANT]), + VWCarInfo("Volkswagen Golf Alltrack 2015-19"), + VWCarInfo("Volkswagen Golf GTD 2015-20"), + VWCarInfo("Volkswagen Golf GTE 2015-20"), + VWCarInfo("Volkswagen Golf GTI 2015-21"), + VWCarInfo("Volkswagen Golf R 2015-19", footnotes=[Footnote.VW_VARIANT]), + VWCarInfo("Volkswagen Golf SportsVan 2015-20"), + ], + CAR.JETTA_MK7: [ + VWCarInfo("Volkswagen Jetta 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Jetta GLI 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + ], + CAR.PASSAT_MK8: [ + VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.PASSAT, Footnote.VW_VARIANT], harness=Harness.j533), + VWCarInfo("Volkswagen Passat Alltrack 2015-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Passat GTE 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533), + ], + CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22", harness=Harness.j533), + CAR.POLO_MK6: [ + VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + ], + CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), + CAR.TRANSPORTER_T61: [ + VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + VWCarInfo("Volkswagen California 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + ], + CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), + CAR.AUDI_A3_MK3: [ + VWCarInfo("Audi A3 2014-19", "ACC + Lane Assist"), + VWCarInfo("Audi A3 Sportback e-tron 2017-18", "ACC + Lane Assist"), + VWCarInfo("Audi RS3 2018", "ACC + Lane Assist"), + VWCarInfo("Audi S3 2015-17", "ACC + Lane Assist"), + ], + CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018", "ACC + Lane Assist"), + CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2020-21", "ACC + Lane Assist"), + CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"), + CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"), + CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.KAMIQ]), + CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21", footnotes=[Footnote.VW_HARNESS]), + CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2018-19"), + CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020"), + CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-18"), + CAR.SKODA_OCTAVIA_MK3: [ + VWCarInfo("Škoda Octavia 2015, 2018-19"), + VWCarInfo("Škoda Octavia RS 2016"), + ], +} + +# All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars +# with a manual trans won't return transmission firmware, but all other cars will. +# +# The 0xF187 SW part number query should return in the form of N[NX][NX] NNN NNN [X[X]], +# where N=number, X=letter, and the trailing two letters are optional. Performance +# tuners sometimes tamper with that field (e.g. 8V0 9C0 BB0 1 from COBB/EQT). Tampered +# ECU SW part numbers are invalid for vehicle ID and compatibility checks. Try to have +# them repaired by the tuner before including them in openpilot. + +VOLKSWAGEN_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + +VOLKSWAGEN_RX_OFFSET = 0x6a + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [VOLKSWAGEN_VERSION_REQUEST_MULTI], + [VOLKSWAGEN_VERSION_RESPONSE], + whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], + rx_offset=VOLKSWAGEN_RX_OFFSET, + ), + Request( + [VOLKSWAGEN_VERSION_REQUEST_MULTI], + [VOLKSWAGEN_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine, Ecu.transmission], + ), + ], +) + +FW_VERSIONS = { + CAR.ARTEON_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x873G0906259F \xf1\x890004', + b'\xf1\x873G0906259P \xf1\x890001', + b'\xf1\x875NA907115H \xf1\x890002', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158L \xf1\x893611', + b'\xf1\x870GC300011L \xf1\x891401', + b'\xf1\x870GC300040P \xf1\x891401', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121157161111572900', + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900', + b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1', + b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', + b'\xf1\x875Q0907572J \xf1\x890654', + ], + }, + CAR.ATLAS_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8703H906026AA\xf1\x899970', + b'\xf1\x8703H906026AJ\xf1\x890638', + b'\xf1\x8703H906026AT\xf1\x891922', + b'\xf1\x8703H906026BC\xf1\x892664', + b'\xf1\x8703H906026F \xf1\x896696', + b'\xf1\x8703H906026F \xf1\x899970', + b'\xf1\x8703H906026J \xf1\x896026', + b'\xf1\x8703H906026J \xf1\x899971', + b'\xf1\x8703H906026S \xf1\x896693', + b'\xf1\x8703H906026S \xf1\x899970', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158A \xf1\x893387', + b'\xf1\x8709G927158DR\xf1\x893536', + b'\xf1\x8709G927158DR\xf1\x893742', + b'\xf1\x8709G927158FT\xf1\x893835', + b'\xf1\x8709G927158GL\xf1\x893939', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200', + b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900', + b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', + b'\xf1\x872Q0907572R \xf1\x890372', + b'\xf1\x872Q0907572T \xf1\x890383', + b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572J \xf1\x890654', + b'\xf1\x875Q0907572P \xf1\x890682', + ], + }, + CAR.GOLF_MK7: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906016A \xf1\x897697', + b'\xf1\x8704E906016AD\xf1\x895758', + b'\xf1\x8704E906016CE\xf1\x899096', + b'\xf1\x8704E906023AG\xf1\x891726', + b'\xf1\x8704E906023BN\xf1\x894518', + b'\xf1\x8704E906024K \xf1\x896811', + b'\xf1\x8704E906027GR\xf1\x892394', + b'\xf1\x8704E906027HD\xf1\x892603', + b'\xf1\x8704E906027HD\xf1\x893742', + b'\xf1\x8704E906027MA\xf1\x894958', + b'\xf1\x8704L906021DT\xf1\x895520', + b'\xf1\x8704L906021DT\xf1\x898127', + b'\xf1\x8704L906021N \xf1\x895518', + b'\xf1\x8704L906026BP\xf1\x897608', + b'\xf1\x8704L906026NF\xf1\x899528', + b'\xf1\x8704L906056CL\xf1\x893823', + b'\xf1\x8704L906056CR\xf1\x895813', + b'\xf1\x8704L906056HE\xf1\x893758', + b'\xf1\x8704L906056HN\xf1\x896590', + b'\xf1\x870EA906016A \xf1\x898343', + b'\xf1\x870EA906016E \xf1\x894219', + b'\xf1\x870EA906016F \xf1\x894238', + b'\xf1\x870EA906016F \xf1\x895002', + b'\xf1\x870EA906016Q \xf1\x895993', + b'\xf1\x870EA906016S \xf1\x897207', + b'\xf1\x875G0906259 \xf1\x890007', + b'\xf1\x875G0906259J \xf1\x890002', + b'\xf1\x875G0906259L \xf1\x890002', + b'\xf1\x875G0906259N \xf1\x890003', + b'\xf1\x875G0906259Q \xf1\x890002', + b'\xf1\x875G0906259Q \xf1\x892313', + b'\xf1\x875G0906259T \xf1\x890003', + b'\xf1\x878V0906259H \xf1\x890002', + b'\xf1\x878V0906259J \xf1\x890003', + b'\xf1\x878V0906259K \xf1\x890001', + b'\xf1\x878V0906259P \xf1\x890001', + b'\xf1\x878V0906259Q \xf1\x890002', + b'\xf1\x878V0906264F \xf1\x890003', + b'\xf1\x878V0906264L \xf1\x890002', + b'\xf1\x878V0906264M \xf1\x890001', + b'\xf1\x878V09C0BB01 \xf1\x890001', + b'\xf1\x8704E906024K \xf1\x899970', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927749AP\xf1\x892943', + b'\xf1\x8709S927158A \xf1\x893585', + b'\xf1\x870CW300040H \xf1\x890606', + b'\xf1\x870CW300041H \xf1\x891010', + b'\xf1\x870CW300042F \xf1\x891604', + b'\xf1\x870CW300043B \xf1\x891601', + b'\xf1\x870CW300044S \xf1\x894530', + b'\xf1\x870CW300044T \xf1\x895245', + b'\xf1\x870CW300045 \xf1\x894531', + b'\xf1\x870CW300047D \xf1\x895261', + b'\xf1\x870CW300048J \xf1\x890611', + b'\xf1\x870D9300012 \xf1\x894904', + b'\xf1\x870D9300012 \xf1\x894913', + b'\xf1\x870D9300012 \xf1\x894937', + b'\xf1\x870D9300012 \xf1\x895045', + b'\xf1\x870D9300014M \xf1\x895004', + b'\xf1\x870D9300014Q \xf1\x895006', + b'\xf1\x870D9300020Q \xf1\x895201', + b'\xf1\x870D9300020S \xf1\x895201', + b'\xf1\x870D9300040A \xf1\x893613', + b'\xf1\x870D9300040S \xf1\x894311', + b'\xf1\x870D9300041H \xf1\x895220', + b'\xf1\x870D9300041P \xf1\x894507', + b'\xf1\x870DD300045K \xf1\x891120', + b'\xf1\x870DD300046F \xf1\x891601', + b'\xf1\x870GC300012A \xf1\x891403', + b'\xf1\x870GC300014B \xf1\x892401', + b'\xf1\x870GC300014B \xf1\x892405', + b'\xf1\x870GC300020G \xf1\x892401', + b'\xf1\x870GC300020G \xf1\x892403', + b'\xf1\x870GC300020G \xf1\x892404', + b'\xf1\x870GC300020N \xf1\x892804', + b'\xf1\x870GC300043T \xf1\x899999', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\x111413001113120043114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\x111413001113120053114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114317121C111C9113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120043114417121411149113', + b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\x111413001113120053114317121C111C9113', + b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', + b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2252229333463100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142405A2252229333463100', + b'\xf1\x875Q0959655C \xf1\x890361\xf1\x82\x111413001112120004110415121610169112', + b'\xf1\x875Q0959655D \xf1\x890388\xf1\x82\x111413001113120006110417121A101A9113', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13271112111312--071104171825102591131211', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13271212111312--071104171838103891131211', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13341512112212--071104172328102891131211', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13272512111312--07110417182C102C91131211', + b'\xf1\x875Q0959655M \xf1\x890361\xf1\x82\x111413001112120041114115121611169112', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200621143171717111791132111', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1324230011211200061104171724102491132111', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1324230011211200621143171724112491132111', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200061104171717101791132111', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1324230011211200631143171724122491132111', + b'\xf1\x875Q0959655T \xf1\x890825\xf1\x82\x13271200111312--071104171837103791132111', + b'\xf1\x875Q0959655T \xf1\x890830\xf1\x82\x13271100111312--071104171826102691131211', + b'\xf1\x875QD959655 \xf1\x890388\xf1\x82\x111413001113120006110417121D101D9112', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\x0561A01612A0', + b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\x0566A0J612A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A00514A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A0J712A1', + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571A0J714A1', + b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1', + b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A01A18A1', + b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A0JA16A1', + b'\xf1\x873QM909144 \xf1\x895072\xf1\x82\x0571A01714A1', + b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203', + b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00441A1', + b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00608A1', + b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00641A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A00442A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A00642A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A07B05A1', + b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\x0521A00602A0', + b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\x0522A00402A0', + b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\x0521A00502A0', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0511A00403A0', + b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00404A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00604A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A07A02A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A00507A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A07B04A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A20B03A1', + b'\xf1\x875QD909144B \xf1\x891072\xf1\x82\x0521A00507A1', + b'\xf1\x875QM909144A \xf1\x891072\xf1\x82\x0521A20B03A1', + b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A00442A1', + b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A16A1', + b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A18A1', + b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A17A1', + b'\xf1\x875QN909144B \xf1\x895082\xf1\x82\x0571A01A18A1', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A2000400', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\x0101', + b'\xf1\x875Q0907567J \xf1\x890396\xf1\x82\x0101', + b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\x0101', + b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\x0101', + b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\x0101', + b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101', + b'\xf1\x875Q0907572E \xf1\x89X310\xf1\x82\x0101', + b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101', + b'\xf1\x875Q0907572G \xf1\x890571', + b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572J \xf1\x890654', + b'\xf1\x875Q0907572P \xf1\x890682', + b'\xf1\x875Q0907572R \xf1\x890771', + b'\xf1\x875Q0907572S \xf1\x890780', + ], + }, + CAR.JETTA_MK7: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906024AK\xf1\x899937', + b'\xf1\x8704E906024AS\xf1\x899912', + b'\xf1\x8704E906024BC\xf1\x899971', + b'\xf1\x8704E906024BG\xf1\x891057', + b'\xf1\x8704E906024B \xf1\x895594', + b'\xf1\x8704E906024C \xf1\x899970', + b'\xf1\x8704E906024L \xf1\x895595', + b'\xf1\x8704E906024L \xf1\x899970', + b'\xf1\x8704E906027MS\xf1\x896223', + b'\xf1\x875G0906259T \xf1\x890003', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158BQ\xf1\x893545', + b'\xf1\x8709S927158BS\xf1\x893642', + b'\xf1\x8709S927158BS\xf1\x893694', + b'\xf1\x8709S927158CK\xf1\x893770', + b'\xf1\x8709S927158R \xf1\x893552', + b'\xf1\x8709S927158R \xf1\x893587', + b'\xf1\x870GC300020N \xf1\x892803', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AG\xf1\x890336\xf1\x82\02314171231313500314611011630169333463100', + b'\xf1\x875Q0959655AG\xf1\x890338\xf1\x82\x1314171231313500314611011630169333463100', + b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314642011650169333463100', + b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100', + b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100', + b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02319170031313300314240011550159333463100', + b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1314171231313500314643021650169333613100', + b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1314171231313500314642021650169333613100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A10A11A1', + b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A10A01A1', + b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521B00404A1', + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A00642A1', + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A10A01A1', + b'\xf1\x875QN909144B \xf1\x895082\xf1\x82\00571A10A11A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907572N \xf1\x890681', + b'\xf1\x875Q0907572P \xf1\x890682', + b'\xf1\x875Q0907572R \xf1\x890771', + ], + }, + CAR.PASSAT_MK8: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8703N906026E \xf1\x892114', + b'\xf1\x8704E906023AH\xf1\x893379', + b'\xf1\x8704L906026ET\xf1\x891990', + b'\xf1\x8704L906026GA\xf1\x892013', + b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x873G0906264 \xf1\x890004', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300043H \xf1\x891601', + b'\xf1\x870CW300048R \xf1\x890610', + b'\xf1\x870D9300014L \xf1\x895002', + b'\xf1\x870D9300041A \xf1\x894801', + b'\xf1\x870DD300045T \xf1\x891601', + b'\xf1\x870DL300011H \xf1\x895201', + b'\xf1\x870GC300042H \xf1\x891404', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', + b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', + b'\xf1\x873Q0959655BA\xf1\x890195\xf1\x82\r56140056130012516612125111', + b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', + b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900', + b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900', + b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', + b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020600', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572A \xf1\x890130', + b'\xf1\x873Q0907572B \xf1\x890192', + b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x873Q0907572C \xf1\x890196', + b'\xf1\x875Q0907572R \xf1\x890771', + ], + }, + CAR.PASSAT_NMS: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8706K906016C \xf1\x899609', + b'\xf1\x8706K906016G \xf1\x891124', + b'\xf1\x8706K906071BJ\xf1\x894891', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158AB\xf1\x893318', + b'\xf1\x8709G927158BD\xf1\x893121', + b'\xf1\x8709G927158FQ\xf1\x893745', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x87561959655 \xf1\x890210\xf1\x82\02212121111113000102011--121012--101312', + b'\xf1\x87561959655C \xf1\x890508\xf1\x82\02215141111121100314919--153015--304831', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x87561907567A \xf1\x890132', + b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\00152', + ], + }, + CAR.POLO_MK6: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704C906025H \xf1\x895177', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300050D \xf1\x891908', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1248130411110416--04040404784811152H14', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872Q1909144M \xf1\x896041', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572R \xf1\x890372', + ], + }, + CAR.TAOS_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027NJ\xf1\x891445', + b'\xf1\x8705E906013E \xf1\x891624', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709S927158BL\xf1\x893791', + b'\xf1\x8709S927158FF\xf1\x893876', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1311111111333500314646021450149333613100', + b'\xf1\x875Q0959655CE\xf1\x890421\xf1\x82\x1311110011333300314240021350139333613100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060405A1', + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060605A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.TCROSS_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704C906025AK\xf1\x897053', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300050E \xf1\x891903', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\02212130411110411--04041104141311152H14', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872Q1909144M \xf1\x896041', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.TIGUAN_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027NB\xf1\x899504', + b'\xf1\x8704L906026EJ\xf1\x893661', + b'\xf1\x8704L906027G \xf1\x899893', + b'\xf1\x875N0906259 \xf1\x890002', + b'\xf1\x875NA907115E \xf1\x890005', + b'\xf1\x8783A907115B \xf1\x890005', + b'\xf1\x8783A907115G \xf1\x890001', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158DT\xf1\x893698', + b'\xf1\x8709G927158GC\xf1\x893821', + b'\xf1\x8709G927158GD\xf1\x893820', + b'\xf1\x870D9300043 \xf1\x895202', + b'\xf1\x870DL300011N \xf1\x892001', + b'\xf1\x870DL300011N \xf1\x892012', + b'\xf1\x870DL300013A \xf1\x893005', + b'\xf1\x870DL300013G \xf1\x892119', + b'\xf1\x870DL300013G \xf1\x892120', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100', + b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140013750379333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', + b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521A60604A1', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6000600', + b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\00571A60634A1', + b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1', + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1', + b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', + b'\xf1\x872Q0907572J \xf1\x890156', + b'\xf1\x872Q0907572Q \xf1\x890342', + b'\xf1\x872Q0907572R \xf1\x890372', + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.TOURAN_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906026HM\xf1\x893017', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041E \xf1\x891005', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\023363500213533353141324C4732479333313100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572C \xf1\x890195', + ], + }, + CAR.TRANSPORTER_T61: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906057AP\xf1\x891186', + b'\xf1\x8704L906057N \xf1\x890413', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870BT300012G \xf1\x893102', + b'\xf1\x870BT300012E \xf1\x893105', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\02316170411110411--04041704161611152S1411', + b'\xf1\x872Q0959655AF\xf1\x890506\xf1\x82\x1316171111110411--04041711121211152S1413', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572R \xf1\x890372', + ], + }, + CAR.TROC_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906018AT\xf1\x899640', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300051M \xf1\x891925', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1152119333613100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521060405A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.AUDI_A3_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906023AN\xf1\x893695', + b'\xf1\x8704E906023AR\xf1\x893440', + b'\xf1\x8704E906023BL\xf1\x895190', + b'\xf1\x8704E906027CJ\xf1\x897798', + b'\xf1\x8704L997022N \xf1\x899459', + b'\xf1\x875G0906259A \xf1\x890004', + b'\xf1\x875G0906259L \xf1\x890002', + b'\xf1\x875G0906259Q \xf1\x890002', + b'\xf1\x878V0906259F \xf1\x890002', + b'\xf1\x878V0906259K \xf1\x890001', + b'\xf1\x878V0906264B \xf1\x890003', + b'\xf1\x878V0907115B \xf1\x890007', + b'\xf1\x878V0907404A \xf1\x890005', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300044T \xf1\x895245', + b'\xf1\x870CW300048 \xf1\x895201', + b'\xf1\x870D9300012 \xf1\x894912', + b'\xf1\x870D9300012K \xf1\x894513', + b'\xf1\x870D9300013B \xf1\x894931', + b'\xf1\x870D9300041N \xf1\x894512', + b'\xf1\x870D9300043T \xf1\x899699', + b'\xf1\x870DD300046 \xf1\x891604', + b'\xf1\x870DD300046A \xf1\x891602', + b'\xf1\x870DD300046F \xf1\x891602', + b'\xf1\x870DD300046G \xf1\x891601', + b'\xf1\x870DL300012E \xf1\x892012', + b'\xf1\x870GC300013M \xf1\x892402', + b'\xf1\x870GC300042J \xf1\x891402', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AB\xf1\x890388\xf1\x82\0211111001111111206110412111321139114', + b'\xf1\x875Q0959655AM\xf1\x890315\xf1\x82\x1311111111111111311411011231129321212100', + b'\xf1\x875Q0959655AM\xf1\x890318\xf1\x82\x1311111111111112311411011531159321212100', + b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1311110011131100311111011731179321342100', + b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13111112111111--241115141112221291163221', + b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221', + b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112110004110411111421149114', + b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1', + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1', + b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571G0JA14A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0503G0G803A0', + b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516G00804A1', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\00101', + b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101', + b'\xf1\x875Q0907572G \xf1\x890571', + b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572P \xf1\x890682', + ], + }, + CAR.AUDI_Q2_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027JT\xf1\x894145', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041F \xf1\x891006', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655BD\xf1\x890336\xf1\x82\x1311111111111100311211011231129321312111', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571F60511A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + ], + }, + CAR.AUDI_Q3_MK2: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906018N \xf1\x899970', + b'\xf1\x8783A906259 \xf1\x890001', + b'\xf1\x8783A906259 \xf1\x890005', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158CN\xf1\x893608', + b'\xf1\x870GC300046F \xf1\x892701', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655BF\xf1\x890403\xf1\x82\x1321211111211200311121232152219321422111', + b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572R \xf1\x890372', + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.SEAT_ATECA_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027KA\xf1\x893749', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870D9300014S \xf1\x895202', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0161212001211001305121211052900', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\00571N60511A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + ], + }, + CAR.SEAT_LEON_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906021EL\xf1\x897542', + b'\xf1\x8704L906026BP\xf1\x891198', + b'\xf1\x8704L906026BP\xf1\x897608', + b'\xf1\x8705E906018AS\xf1\x899596', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300050J \xf1\x891908', + b'\xf1\x870D9300042M \xf1\x895016', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AC\xf1\x890189\xf1\x82\r11110011110011021511110200', + b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r12110012120012021612110200', + b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\0161312001313001305171311052900', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521N01342A1', + b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511N01805A0', + b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521N05808A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101', + b'\xf1\x875Q0907572H \xf1\x890620', + b'\xf1\x875Q0907572P \xf1\x890682', + ], + }, + CAR.SKODA_KAMIQ_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705C906032M \xf1\x891333', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300020 \xf1\x891906', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\0222221042111042121040404042E2711152H14', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872Q1909144M \xf1\x896041', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.SKODA_KAROQ_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906018P \xf1\x896020', + b'\xf1\x8705L906022BS\xf1\x890913', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041S \xf1\x891615', + b'\xf1\x870GC300014L \xf1\x892802', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BH\xf1\x890712\xf1\x82\0161213001211001101131122012100', + b'\xf1\x873Q0959655DE\xf1\x890731\xf1\x82\x0e1213001211001101131121012J00', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567T6100500', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100700', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572M \xf1\x890233', + b'\xf1\x872Q0907572T \xf1\x890383', + ], + }, + CAR.SKODA_KODIAQ_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027DD\xf1\x893123', + b'\xf1\x8704L906026DE\xf1\x895418', + b'\xf1\x875NA907115E \xf1\x890003', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870D9300043 \xf1\x895202', + b'\xf1\x870DL300012M \xf1\x892107', + b'\xf1\x870DL300012N \xf1\x892110', + b'\xf1\x870DL300013G \xf1\x892119', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\0161213001211001205212111052100', + b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\0161213001211001205212112052100', + b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572Q \xf1\x890342', + b'\xf1\x872Q0907572R \xf1\x890372', + ], + }, + CAR.SKODA_OCTAVIA_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906016ER\xf1\x895823', + b'\xf1\x8704E906027HD\xf1\x893742', + b'\xf1\x8704E906027MH\xf1\x894786', + b'\xf1\x8704L906021DT\xf1\x898127', + b'\xf1\x8704L906026BS\xf1\x891541', + b'\xf1\x875G0906259C \xf1\x890002', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041L \xf1\x891601', + b'\xf1\x870CW300041N \xf1\x891605', + b'\xf1\x870CW300043B \xf1\x891601', + b'\xf1\x870D9300041C \xf1\x894936', + b'\xf1\x870D9300041J \xf1\x894902', + b'\xf1\x870D9300041P \xf1\x894507', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AC\xf1\x890200\xf1\x82\r11120011100010022212110200', + b'\xf1\x873Q0959655AQ\xf1\x890200\xf1\x82\r11120011100010312212113100', + b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200', + b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100', + b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100', + b'\xf1\x875QD959655 \xf1\x890388\xf1\x82\x111101000011110006110411111111119111', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A01513A1', + b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521T00403A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1', + b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1', + b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101', + b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101', + b'\xf1\x875Q0907572J \xf1\x890654', + b'\xf1\x875Q0907572P \xf1\x890682', + b'\xf1\x875Q0907572R \xf1\x890771', + ], + }, + CAR.SKODA_SCALA_MK1: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704C906025AK\xf1\x897053', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300050 \xf1\x891709', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\022111104111104112104040404111111112H14', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x872Q1909144M \xf1\x896041', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572R \xf1\x890372', + ], + }, + CAR.SKODA_SUPERB_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906026FP\xf1\x891196', + b'\xf1\x8704L906026KB\xf1\x894071', + b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x873G0906259B \xf1\x890002', + b'\xf1\x873G0906264A \xf1\x890002', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300042H \xf1\x891601', + b'\xf1\x870D9300011T \xf1\x894801', + b'\xf1\x870D9300012 \xf1\x894940', + b'\xf1\x870GC300043 \xf1\x892301', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111', + b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111', + b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100', + ], + (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203', + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303', + b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700', + b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070600', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572B \xf1\x890192', + b'\xf1\x873Q0907572B \xf1\x890194', + b'\xf1\x873Q0907572C \xf1\x890195', + ], + }, +} diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9e31ac15268bf0..3054b020e1ca82 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,135 +1,664 @@ #!/usr/bin/env python3 +import os import math -from numbers import Number +from typing import SupportsFloat from cereal import car, log +from common.numpy_fast import clip +from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL +from common.profiler import Profiler +from common.params import Params, put_nonblocking import cereal.messaging as messaging -from openpilot.common.constants import CV -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper -from openpilot.common.swaglog import cloudlog - -from opendbc.car.car_helpers import interfaces -from opendbc.car.vehicle_model import VehicleModel -from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature -from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID -from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD -from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque -from openpilot.selfdrive.controls.lib.longcontrol import LongControl -from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose - -State = log.SelfdriveState.OpenpilotState -LaneChangeState = log.LaneChangeState -LaneChangeDirection = log.LaneChangeDirection - +from common.conversions import Conversions as CV +from panda import ALTERNATIVE_EXPERIENCE +from system.swaglog import cloudlog +from system.version import is_tested_branch, get_short_branch +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can +from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET +from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise +from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature +from selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.longcontrol import LongControl +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from selfdrive.controls.lib.events import Events, ET +from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.locationd.calibrationd import Calibration +from system.hardware import HARDWARE +from selfdrive.manager.process_config import managed_processes + +SOFT_DISABLE_TIME = 3 # seconds +LDW_MIN_SPEED = 31 * CV.MPH_TO_MS +LANE_DEPARTURE_THRESHOLD = 0.1 + +REPLAY = "REPLAY" in os.environ +SIMULATION = "SIMULATION" in os.environ +NOSENSOR = "NOSENSOR" in os.environ +IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", + "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", "laikad"} | \ + {k for k, v in managed_processes.items() if not v.enabled} + +ThermalStatus = log.DeviceState.ThermalStatus +State = log.ControlsState.OpenpilotState +PandaType = log.PandaState.PandaType +Desire = log.LateralPlan.Desire +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection +EventName = car.CarEvent.EventName +ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type +SafetyModel = car.CarParams.SafetyModel + +IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) +CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) +ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) +ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) class Controls: - def __init__(self) -> None: - self.params = Params() - cloudlog.info("controlsd is waiting for CarParams") - self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) - cloudlog.info("controlsd got CarParams") - - self.CI = interfaces[self.CP.carFingerprint](self.CP) - - self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState']) - - self.steer_limited_by_safety = False - self.curvature = 0.0 - self.desired_curvature = 0.0 - - self.pose_calibrator = PoseCalibrator() - self.calibrated_pose: Pose | None = None + def __init__(self, sm=None, pm=None, can_sock=None, CI=None): + config_realtime_process(4, Priority.CTRL_HIGH) + + # Ensure the current branch is cached, otherwise the first iteration of controlsd lags + self.branch = get_short_branch("") + + # Setup sockets + self.pm = pm + if self.pm is None: + self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', + 'carControl', 'carEvents', 'carParams']) + + self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] + + self.can_sock = can_sock + if can_sock is None: + can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 + self.can_sock = messaging.sub_sock('can', timeout=can_timeout) + + self.log_sock = messaging.sub_sock('androidLog') + + if CI is None: + # wait for one pandaState and one CAN packet + print("Waiting for CAN messages...") + get_one_can(self.can_sock) + + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) + else: + self.CI, self.CP = CI, CI.CP + + params = Params() + self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) + joystick_packet = ['testJoystick'] if self.joystick_mode else [] + + self.sm = sm + if self.sm is None: + ignore = [] + if SIMULATION: + ignore += ['driverCameraState', 'managerState'] + if params.get_bool('WideCameraOnly'): + ignore += ['roadCameraState'] + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) + + # set alternative experiences from parameters + self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") + self.CP.alternativeExperience = 0 + if not self.disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + + if self.CP.dashcamOnly and params.get_bool("DashcamOverride"): + self.CP.dashcamOnly = False + + # read params + self.is_metric = params.get_bool("IsMetric") + self.is_ldw_enabled = params.get_bool("IsLdwEnabled") + openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") + passive = params.get_bool("Passive") or not openpilot_enabled_toggle + + # detect sound card presence and ensure successful init + sounds_available = HARDWARE.get_sound_card_online() + + car_recognized = self.CP.carName != 'mock' + + controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly + self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly + if self.read_only: + safety_config = car.CarParams.SafetyConfig.new_message() + safety_config.safetyModel = car.CarParams.SafetyModel.noOutput + self.CP.safetyConfigs = [safety_config] + + if is_tested_branch(): + self.CP.experimentalLongitudinalAvailable = False + + # Write CarParams for radard + cp_bytes = self.CP.to_bytes() + params.put("CarParams", cp_bytes) + put_nonblocking("CarParamsCache", cp_bytes) + put_nonblocking("CarParamsPersistent", cp_bytes) + + self.CC = car.CarControl.new_message() + self.CS_prev = car.CarState.new_message() + self.AM = AlertManager() + self.events = Events() self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) + self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL) + self.LaC = LatControlAngle(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'pid': - self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) + self.LaC = LatControlPID(self.CP, self.CI) + elif self.CP.lateralTuning.which() == 'indi': + self.LaC = LatControlINDI(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'torque': - self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) + self.LaC = LatControlTorque(self.CP, self.CI) + + self.initialized = False + self.state = State.disabled + self.enabled = False + self.active = False + self.can_rcv_timeout = False + self.soft_disable_timer = 0 + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph_last = 0 + self.mismatch_counter = 0 + self.cruise_mismatch_counter = 0 + self.can_rcv_timeout_counter = 0 + self.last_blinker_frame = 0 + self.distance_traveled = 0 + self.last_functional_fan_frame = 0 + self.events_prev = [] + self.current_alert_types = [ET.PERMANENT] + self.logged_comm_issue = None + self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} + self.last_actuators = car.CarControl.Actuators.new_message() + self.steer_limited = False + self.desired_curvature = 0.0 + self.desired_curvature_rate = 0.0 - def update(self): - self.sm.update(15) - if self.sm.updated["liveCalibration"]: - self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) - if self.sm.updated["livePose"]: - device_pose = Pose.from_live_pose(self.sm['livePose']) - self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) + # TODO: no longer necessary, aside from process replay + self.sm['liveParameters'].valid = True - def state_control(self): - CS = self.sm['carState'] + self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) - # Update VehicleModel - lp = self.sm['liveParameters'] - x = max(lp.stiffnessFactor, 0.1) - sr = max(lp.steerRatio, 0.1) - self.VM.update_params(x, sr) + if not sounds_available: + self.events.add(EventName.soundsUnavailable, static=True) + if not car_recognized: + self.events.add(EventName.carUnrecognized, static=True) + if len(self.CP.carFw) > 0: + set_offroad_alert("Offroad_CarUnrecognized", True) + else: + set_offroad_alert("Offroad_NoFirmware", True) + elif self.read_only: + self.events.add(EventName.dashcamMode, static=True) + elif self.joystick_mode: + self.events.add(EventName.joystickDebug, static=True) + self.startup_event = None + + # controlsd is driven by can recv, expected at 100Hz + self.rk = Ratekeeper(100, print_delay_threshold=None) + self.prof = Profiler(False) # off by default + + def set_initial_state(self): + if REPLAY: + controls_state = Params().get("ReplayControlsState") + if controls_state is not None: + controls_state = log.ControlsState.from_bytes(controls_state) + self.v_cruise_kph = controls_state.vCruise + + if self.sm['pandaStates'][0].controlsAllowed: + self.state = State.enabled + + def update_events(self, CS): + """Compute carEvents from carState""" + + self.events.clear() + + # Add startup event + if self.startup_event is not None: + self.events.add(self.startup_event) + self.startup_event = None + + # Don't add any more events if not initialized + if not self.initialized: + self.events.add(EventName.controlsInitializing) + return + + # Block resume if cruise never previously enabled + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + if not self.CP.pcmCruise and self.v_cruise_kph == V_CRUISE_INITIAL and resume_pressed: + self.events.add(EventName.resumeBlocked) + + # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 + if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)): + self.events.add(EventName.pedalPressed) + + if CS.gasPressed: + self.events.add(EventName.pedalPressedPreEnable if self.disengage_on_accelerator else + EventName.gasPressedOverride) + + if not self.CP.notCar: + self.events.add_from_msg(self.sm['driverMonitoringState'].events) + + # Add car events, ignore if CAN isn't valid + if CS.canValid: + self.events.add_from_msg(CS.events) + + # Create events for temperature, disk space, and memory + if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: + self.events.add(EventName.overheat) + if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: + # under 7% of space free no enable allowed + self.events.add(EventName.outOfSpace) + # TODO: make tici threshold the same + if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: + self.events.add(EventName.lowMemory) + + # TODO: enable this once loggerd CPU usage is more reasonable + #cpus = list(self.sm['deviceState'].cpuUsagePercent) + #if max(cpus, default=0) > 95 and not SIMULATION: + # self.events.add(EventName.highCpuUsage) + + # Alert if fan isn't spinning for 5 seconds + if self.sm['peripheralState'].pandaType == PandaType.dos: + if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: + if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: + self.events.add(EventName.fanMalfunction) + else: + self.last_functional_fan_frame = self.sm.frame - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) - self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + # Handle calibration status + cal_status = self.sm['liveCalibration'].calStatus + if cal_status != Calibration.CALIBRATED: + if cal_status == Calibration.UNCALIBRATED: + self.events.add(EventName.calibrationIncomplete) + else: + self.events.add(EventName.calibrationInvalid) + + # Handle lane change + if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: + direction = self.sm['lateralPlan'].laneChangeDirection + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ + (CS.rightBlindspot and direction == LaneChangeDirection.right): + self.events.add(EventName.laneChangeBlocked) + else: + if direction == LaneChangeDirection.left: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.preLaneChangeRight) + elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting, + LaneChangeState.laneChangeFinishing): + self.events.add(EventName.laneChange) + + for i, pandaState in enumerate(self.sm['pandaStates']): + # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput + if i < len(self.CP.safetyConfigs): + safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ + pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ + pandaState.alternativeExperience != self.CP.alternativeExperience + else: + safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES + + if safety_mismatch or self.mismatch_counter >= 200: + self.events.add(EventName.controlsMismatch) + + if log.PandaState.FaultType.relayMalfunction in pandaState.faults: + self.events.add(EventName.relayMalfunction) + + # Handle HW and system malfunctions + # Order is very intentional here. Be careful when modifying this. + # All events here should at least have NO_ENTRY and SOFT_DISABLE. + num_events = len(self.events) + + not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} + if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES): + self.events.add(EventName.processNotRunning) + else: + if not SIMULATION and not self.rk.lagging: + if not self.sm.all_alive(self.camera_packets): + self.events.add(EventName.cameraMalfunction) + elif not self.sm.all_freq_ok(self.camera_packets): + self.events.add(EventName.cameraFrameRate) + if self.rk.lagging: + self.events.add(EventName.controlsdLagging) + if len(self.sm['radarState'].radarErrors) or not self.sm.all_checks(['radarState']): + self.events.add(EventName.radarFault) + if not self.sm.valid['pandaStates']: + self.events.add(EventName.usbError) + if CS.canTimeout: + self.events.add(EventName.canBusMissing) + elif not CS.canValid: + self.events.add(EventName.canError) + + # generic catch-all. ideally, a more specific event should be added above instead + has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE)) + no_system_errors = (not has_disable_events) or (len(self.events) == num_events) + if (not self.sm.all_checks() or self.can_rcv_timeout) and no_system_errors: + if not self.sm.all_alive(): + self.events.add(EventName.commIssue) + elif not self.sm.all_freq_ok(): + self.events.add(EventName.commIssueAvgFreq) + else: # invalid or can_rcv_timeout. + self.events.add(EventName.commIssue) + + logs = { + 'invalid': [s for s, valid in self.sm.valid.items() if not valid], + 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], + 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], + 'can_rcv_timeout': self.can_rcv_timeout, + } + if logs != self.logged_comm_issue: + cloudlog.event("commIssue", error=True, **logs) + self.logged_comm_issue = logs + else: + self.logged_comm_issue = None + + if not self.sm['liveParameters'].valid: + self.events.add(EventName.vehicleModelInvalid) + if not self.sm['lateralPlan'].mpcSolutionValid: + self.events.add(EventName.plannerError) + if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR: + if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs + self.events.add(EventName.sensorDataInvalid) + if not self.sm['liveLocationKalman'].posenetOK: + self.events.add(EventName.posenetInvalid) + if not self.sm['liveLocationKalman'].deviceStable: + self.events.add(EventName.deviceFalling) + + if not REPLAY: + # Check for mismatch between openpilot and car's PCM + cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) + self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 + if self.cruise_mismatch_counter > int(6. / DT_CTRL): + self.events.add(EventName.cruiseMismatch) + + # Check for FCW + stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 + model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking + planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled + if planner_fcw or model_fcw: + self.events.add(EventName.fcw) + + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): + try: + msg = m.androidLog.message + if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): + csid = msg.split("CSID:")[-1].split(" ")[0] + evt = CSID_MAP.get(csid, None) + if evt is not None: + self.events.add(evt) + except UnicodeDecodeError: + pass + + # TODO: fix simulator + if not SIMULATION: + if not NOSENSOR: + if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000): + # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes + self.events.add(EventName.noGps) + + if self.sm['modelV2'].frameDropPerc > 20: + self.events.add(EventName.modeldLagging) + if self.sm['liveLocationKalman'].excessiveResets: + self.events.add(EventName.localizerMalfunction) + + # Only allow engagement with brake pressed when stopped behind another stopped car + speeds = self.sm['longitudinalPlan'].speeds + if len(speeds) > 1: + v_future = speeds[-1] + else: + v_future = 100.0 + if CS.brakePressed and v_future >= self.CP.vEgoStarting \ + and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: + self.events.add(EventName.noTarget) + + def data_sample(self): + """Receive data from sockets and update carState""" + + # Update carState from CAN + can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) + CS = self.CI.update(self.CC, can_strs) + + self.sm.update(0) + + if not self.initialized: + all_valid = CS.canValid and self.sm.all_checks() + timed_out = self.sm.frame * DT_CTRL > (6. if REPLAY else 3.5) + if all_valid or timed_out or SIMULATION: + if not self.read_only: + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + + self.initialized = True + self.set_initial_state() + Params().put_bool("ControlsReady", True) + + # Check for CAN timeout + if not can_strs: + self.can_rcv_timeout_counter += 1 + self.can_rcv_timeout = True + else: + self.can_rcv_timeout = False + + # When the panda and controlsd do not agree on controls_allowed + # we want to disengage openpilot. However the status from the panda goes through + # another socket other than the CAN messages and one can arrive earlier than the other. + # Therefore we allow a mismatch for two samples, then we trigger the disengagement. + if not self.enabled: + self.mismatch_counter = 0 + + # All pandas not in silent mode must have controlsAllowed when openpilot is enabled + if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] + if ps.safetyModel not in IGNORED_SAFETY_MODES): + self.mismatch_counter += 1 + + self.distance_traveled += CS.vEgo * DT_CTRL + + return CS + + def state_transition(self, CS): + """Compute conditional state transitions and execute actions on state transitions""" + + self.v_cruise_kph_last = self.v_cruise_kph + + if CS.cruiseState.available: + # if stock cruise is completely disabled, then we can use our own set speed logic + if not self.CP.pcmCruise: + self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, + self.button_timers, self.enabled, self.is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph + else: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + else: + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL + + # decrement the soft disable timer at every step, as it's reset on + # entrance in SOFT_DISABLING state + self.soft_disable_timer = max(0, self.soft_disable_timer - 1) + + self.current_alert_types = [ET.PERMANENT] - # Update Torque Params - if self.CP.lateralTuning.which() == 'torque': - torque_params = self.sm['liveTorqueParameters'] - if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams: - self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, - torque_params.frictionCoefficientFiltered) + # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING + if self.state != State.disabled: + # user and immediate disable always have priority in a non-disabled state + if self.events.any(ET.USER_DISABLE): + self.state = State.disabled + self.current_alert_types.append(ET.USER_DISABLE) + + elif self.events.any(ET.IMMEDIATE_DISABLE): + self.state = State.disabled + self.current_alert_types.append(ET.IMMEDIATE_DISABLE) + + else: + # ENABLED + if self.state == State.enabled: + if self.events.any(ET.SOFT_DISABLE): + self.state = State.softDisabling + self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) + self.current_alert_types.append(ET.SOFT_DISABLE) + + elif self.events.any(ET.OVERRIDE): + self.state = State.overriding + self.current_alert_types.append(ET.OVERRIDE) + + # SOFT DISABLING + elif self.state == State.softDisabling: + if not self.events.any(ET.SOFT_DISABLE): + # no more soft disabling condition, so go back to ENABLED + self.state = State.enabled + + elif self.soft_disable_timer > 0: + self.current_alert_types.append(ET.SOFT_DISABLE) + + elif self.soft_disable_timer <= 0: + self.state = State.disabled + + # PRE ENABLING + elif self.state == State.preEnabled: + if self.events.any(ET.NO_ENTRY): + self.state = State.disabled + self.current_alert_types.append(ET.NO_ENTRY) + elif not self.events.any(ET.PRE_ENABLE): + self.state = State.enabled + else: + self.current_alert_types.append(ET.PRE_ENABLE) + + # OVERRIDING + elif self.state == State.overriding: + if self.events.any(ET.SOFT_DISABLE): + self.state = State.softDisabling + self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) + self.current_alert_types.append(ET.SOFT_DISABLE) + elif not self.events.any(ET.OVERRIDE): + self.state = State.enabled + else: + self.current_alert_types.append(ET.OVERRIDE) + + # DISABLED + elif self.state == State.disabled: + if self.events.any(ET.ENABLE): + if self.events.any(ET.NO_ENTRY): + self.current_alert_types.append(ET.NO_ENTRY) + + else: + if self.events.any(ET.PRE_ENABLE): + self.state = State.preEnabled + elif self.events.any(ET.OVERRIDE): + self.state = State.overriding + else: + self.state = State.enabled + self.current_alert_types.append(ET.ENABLE) + if not self.CP.pcmCruise: + self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) + self.v_cruise_cluster_kph = self.v_cruise_kph + + # Check if openpilot is engaged and actuators are enabled + self.enabled = self.state in ENABLED_STATES + self.active = self.state in ACTIVE_STATES + if self.active: + self.current_alert_types.append(ET.WARNING) + + def state_control(self, CS): + """Given the state, this function returns a CarControl packet""" + + # Update VehicleModel + params = self.sm['liveParameters'] + x = max(params.stiffnessFactor, 0.1) + sr = max(params.steerRatio, 0.1) + self.VM.update_params(x, sr) + lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] - model_v2 = self.sm['modelV2'] CC = car.CarControl.new_message() - CC.enabled = self.sm['selfdriveState'].enabled - + CC.enabled = self.enabled # Check which actuators can be enabled - standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill - CC.latActive = self.sm['selfdriveState'].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.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + CC.longActive = self.active and not self.events.any(ET.OVERRIDE) and self.CP.openpilotLongitudinalControl actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state - # Enable blinkers while lane changing - if model_v2.meta.laneChangeState != LaneChangeState.off: - CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left - CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right + if CS.leftBlinker or CS.rightBlinker: + self.last_blinker_frame = self.sm.frame + + # State specific actions if not CC.latActive: self.LaC.reset() if not CC.longActive: - self.LoC.reset() - - # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) - actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) - - # Steering PID loop and lateral MPC - # Reset desired curvature to current to avoid violating the limits on engage - new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature - self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) - lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS - - actuators.curvature = self.desired_curvature - steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, - curvature_limited, lat_delay) - actuators.torque = float(steer) - actuators.steeringAngleDeg = float(steeringAngleDeg) + self.LoC.reset(v_pid=CS.vEgo) + + if not self.joystick_mode: + # accel PID loop + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) + + # Steering PID loop and lateral MPC + self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, + lat_plan.psis, + lat_plan.curvatures, + lat_plan.curvatureRates) + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params, + self.last_actuators, self.steer_limited, self.desired_curvature, + self.desired_curvature_rate, self.sm['liveLocationKalman']) + else: + lac_log = log.ControlsState.LateralDebugState.new_message() + if self.sm.rcv_frame['testJoystick'] > 0: + if CC.longActive: + actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) + + if CC.latActive: + steer = clip(self.sm['testJoystick'].axes[1], -1, 1) + # max angle is 45 for angle-based cars + actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. + + lac_log.active = self.active + lac_log.steeringAngleDeg = CS.steeringAngleDeg + lac_log.output = actuators.steer + lac_log.saturated = abs(actuators.steer) >= 0.9 + + # Send a "steering required alert" if saturation count has reached the limit + if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: + undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 + turning = abs(lac_log.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.last_actuators.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + self.events.add(EventName.steerSaturated) + elif lac_log.active and not CS.steeringPressed and lac_log.saturated: + dpath_points = lat_plan.dPathPoints + if len(dpath_points): + # Check if we deviated from the path + # TODO use desired vs actual curvature + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + steering_value = actuators.steeringAngleDeg + else: + steering_value = actuators.steer + + left_deviation = steering_value > 0 and dpath_points[0] < -0.20 + right_deviation = steering_value < 0 and dpath_points[0] > 0.20 + + if left_deviation or right_deviation: + self.events.add(EventName.steerSaturated) + # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: attr = getattr(actuators, p) - if not isinstance(attr, Number): + if not isinstance(attr, SupportsFloat): continue if not math.isfinite(attr): @@ -138,90 +667,212 @@ def state_control(self): return CC, lac_log - def publish(self, CC, lac_log): - CS = self.sm['carState'] + def update_button_timers(self, buttonEvents): + # increment timer for buttons still pressed + for k in self.button_timers: + if self.button_timers[k] > 0: + self.button_timers[k] += 1 + + for b in buttonEvents: + if b.type.raw in self.button_timers: + self.button_timers[b.type.raw] = 1 if b.pressed else 0 + + def publish_logs(self, CS, start_time, CC, lac_log): + """Send actuators and hud commands to the car, send controlsstate and MPC logging""" # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller - CC.currentCurvature = self.curvature - if self.calibrated_pose is not None: - CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() - CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() + orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) + if len(orientation_value) > 2: + CC.orientationNED = orientation_value + angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value) + if len(angular_rate_value) > 2: + CC.angularVelocity = angular_rate_value + + CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) + if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: + CC.cruiseControl.cancel = True - CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl - 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 + speeds = self.sm['longitudinalPlan'].speeds + if len(speeds): + CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS) - hudControl.speedVisible = CC.enabled - hudControl.lanesVisible = CC.enabled + hudControl.setSpeed = float(self.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.speedVisible = self.enabled + hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead - hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1 - hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True - if self.sm.valid['driverAssistance']: - hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture - hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture - - if self.sm['selfdriveState'].active: - CO = self.sm['carOutput'] - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ - STEER_ANGLE_SATURATION_THRESHOLD - else: - self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 - # TODO: both controlsState and carControl valids should be set by - # sm.all_checks(), but this creates a circular dependency + recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown + ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ + and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + + model_v2 = self.sm['modelV2'] + desire_prediction = model_v2.meta.desirePrediction + if len(desire_prediction) and ldw_allowed: + right_lane_visible = model_v2.laneLineProbs[2] > 0.5 + left_lane_visible = model_v2.laneLineProbs[1] > 0.5 + l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1] + r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1] + + lane_lines = model_v2.laneLines + l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET)) + + hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) + hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) + + if hudControl.rightLaneDepart or hudControl.leftLaneDepart: + self.events.add(EventName.ldw) + + clear_event_types = set() + if ET.WARNING not in self.current_alert_types: + clear_event_types.add(ET.WARNING) + if self.enabled: + clear_event_types.add(ET.NO_ENTRY) + + alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer]) + self.AM.add_many(self.sm.frame, alerts) + current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types) + if current_alert: + hudControl.visualAlert = current_alert.visual_alert + + if not self.read_only and self.initialized: + # send car controls over can + self.last_actuators, can_sends = self.CI.apply(CC) + self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + CC.actuatorsOutput = self.last_actuators + self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + + force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ + (self.state == State.softDisabling) + + # Curvature & Steering angle + params = self.sm['liveParameters'] + + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) + curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll) # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid - cs = dat.controlsState - - cs.curvature = self.curvature - cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] - cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] - cs.desiredCurvature = self.desired_curvature - cs.longControlState = self.LoC.long_control_state - cs.upAccelCmd = float(self.LoC.pid.p) - cs.uiAccelCmd = float(self.LoC.pid.i) - cs.ufAccelCmd = float(self.LoC.pid.f) - cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or - (self.sm['selfdriveState'].state == State.softDisabling)) + controlsState = dat.controlsState + if current_alert: + controlsState.alertText1 = current_alert.alert_text_1 + controlsState.alertText2 = current_alert.alert_text_2 + controlsState.alertSize = current_alert.alert_size + controlsState.alertStatus = current_alert.alert_status + controlsState.alertBlinkingRate = current_alert.alert_rate + controlsState.alertType = current_alert.alert_type + controlsState.alertSound = current_alert.audible_alert + + controlsState.canMonoTimes = list(CS.canMonoTimes) + controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] + controlsState.enabled = self.enabled + controlsState.active = self.active + controlsState.curvature = curvature + controlsState.desiredCurvature = self.desired_curvature + controlsState.desiredCurvatureRate = self.desired_curvature_rate + controlsState.state = self.state + controlsState.engageable = not self.events.any(ET.NO_ENTRY) + controlsState.longControlState = self.LoC.long_control_state + controlsState.vPid = float(self.LoC.v_pid) + controlsState.vCruise = float(self.v_cruise_kph) + controlsState.vCruiseCluster = float(self.v_cruise_cluster_kph) + controlsState.upAccelCmd = float(self.LoC.pid.p) + controlsState.uiAccelCmd = float(self.LoC.pid.i) + controlsState.ufAccelCmd = float(self.LoC.pid.f) + controlsState.cumLagMs = -self.rk.remaining * 1000. + controlsState.startMonoTime = int(start_time * 1e9) + controlsState.forceDecel = bool(force_decel) + controlsState.canErrorCounter = self.can_rcv_timeout_counter lat_tuning = self.CP.lateralTuning.which() - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - cs.lateralControlState.angleState = lac_log + if self.joystick_mode: + controlsState.lateralControlState.debugState = lac_log + elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: + controlsState.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': - cs.lateralControlState.pidState = lac_log + controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': - cs.lateralControlState.torqueState = lac_log + controlsState.lateralControlState.torqueState = lac_log + elif lat_tuning == 'indi': + controlsState.lateralControlState.indiState = lac_log self.pm.send('controlsState', dat) + # carState + car_events = self.events.to_msg() + cs_send = messaging.new_message('carState') + cs_send.valid = CS.canValid + cs_send.carState = CS + cs_send.carState.events = car_events + self.pm.send('carState', cs_send) + + # carEvents - logged every second or on change + if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): + ce_send = messaging.new_message('carEvents', len(self.events)) + ce_send.carEvents = car_events + self.pm.send('carEvents', ce_send) + self.events_prev = self.events.names.copy() + + # carParams - logged every 50 seconds (> 1 per segment) + if (self.sm.frame % int(50. / DT_CTRL) == 0): + cp_send = messaging.new_message('carParams') + cp_send.carParams = self.CP + self.pm.send('carParams', cp_send) + # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) - def run(self): - rk = Ratekeeper(100, print_delay_threshold=None) - while True: - self.update() - CC, lac_log = self.state_control() - self.publish(CC, lac_log) - rk.monitor_time() + # copy CarControl to pass to CarInterface on the next iteration + self.CC = CC + + def step(self): + start_time = sec_since_boot() + self.prof.checkpoint("Ratekeeper", ignore=True) + + # Sample data from sockets and get a carState + CS = self.data_sample() + cloudlog.timestamp("Data sampled") + self.prof.checkpoint("Sample") + + self.update_events(CS) + cloudlog.timestamp("Events updated") + + if not self.read_only and self.initialized: + # Update control state + self.state_transition(CS) + self.prof.checkpoint("State transition") + # Compute actuators (runs PID loops and lateral MPC) + CC, lac_log = self.state_control(CS) + + self.prof.checkpoint("State Control") + + # Publish data + self.publish_logs(CS, start_time, CC, lac_log) + self.prof.checkpoint("Sent") + + self.update_button_timers(CS.buttonEvents) + self.CS_prev = CS + + def controlsd_thread(self): + while True: + self.step() + self.rk.monitor_time() + self.prof.display() -def main(): - config_realtime_process(4, Priority.CTRL_HIGH) - controls = Controls() - controls.run() +def main(sm=None, pm=None, logcan=None): + controls = Controls(sm, pm, logcan) + controls.controlsd_thread() if __name__ == "__main__": diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py new file mode 100644 index 00000000000000..cb878483de473b --- /dev/null +++ b/selfdrive/controls/lib/alertmanager.py @@ -0,0 +1,64 @@ +import copy +import os +import json +from collections import defaultdict +from dataclasses import dataclass +from typing import List, Dict, Optional + +from common.basedir import BASEDIR +from common.params import Params +from selfdrive.controls.lib.events import Alert + + +with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: + OFFROAD_ALERTS = json.load(f) + + +def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = None) -> None: + if show_alert: + a = OFFROAD_ALERTS[alert] + if extra_text is not None: + a = copy.copy(OFFROAD_ALERTS[alert]) + a['text'] += extra_text + Params().put(alert, json.dumps(a)) + else: + Params().remove(alert) + + +@dataclass +class AlertEntry: + alert: Optional[Alert] = None + start_frame: int = -1 + end_frame: int = -1 + + def active(self, frame: int) -> bool: + return frame <= self.end_frame + +class AlertManager: + def __init__(self): + self.alerts: Dict[str, AlertEntry] = defaultdict(AlertEntry) + + def add_many(self, frame: int, alerts: List[Alert]) -> None: + for alert in alerts: + entry = self.alerts[alert.alert_type] + entry.alert = alert + if not entry.active(frame): + entry.start_frame = frame + min_end_frame = entry.start_frame + alert.duration + entry.end_frame = max(frame + 1, min_end_frame) + + def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]: + current_alert = AlertEntry() + for v in self.alerts.values(): + if not v.alert: + continue + + if v.alert.event_type in clear_event_types: + v.end_frame = -1 + + # sort by priority first and then by start_frame + greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame) + if v.active(frame) and greater: + current_alert = v + + return current_alert.alert diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json new file mode 100644 index 00000000000000..2f85ea917ae6b4 --- /dev/null +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -0,0 +1,52 @@ +{ + "Offroad_TemperatureTooHigh": { + "text": "Device temperature too high. System won't start.", + "severity": 1 + }, + "Offroad_ConnectivityNeededPrompt": { + "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in ", + "severity": 0, + "_comment": "Append the number of days at the end of the text" + }, + "Offroad_ConnectivityNeeded": { + "text": "Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates.", + "severity": 1 + }, + "Offroad_UpdateFailed": { + "text": "Unable to download updates\n", + "severity": 1, + "_comment": "Append the command and error to the text." + }, + "Offroad_InvalidTime": { + "text": "Invalid date and time settings, system won't start. Connect to internet to set time.", + "severity": 1 + }, + "Offroad_IsTakingSnapshot": { + "text": "Taking camera snapshots. System won't start until finished.", + "severity": 0 + }, + "Offroad_NeosUpdate": { + "text": "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.", + "severity": 0 + }, + "Offroad_UnofficialHardware": { + "text": "Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, contact support@comma.ai.", + "severity": 1 + }, + "Offroad_StorageMissing": { + "text": "NVMe drive not mounted.", + "severity": 1 + }, + "Offroad_BadNvme": { + "text": "Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.", + "severity": 1 + }, + "Offroad_CarUnrecognized": { + "text": "openpilot 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 + }, + "Offroad_NoFirmware": { + "text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.", + "severity": 0 + } +} diff --git a/selfdrive/controls/lib/cluster/.gitignore b/selfdrive/controls/lib/cluster/.gitignore new file mode 100644 index 00000000000000..9daeafb9864cf4 --- /dev/null +++ b/selfdrive/controls/lib/cluster/.gitignore @@ -0,0 +1 @@ +test diff --git a/selfdrive/controls/lib/cluster/LICENSE b/selfdrive/controls/lib/cluster/LICENSE new file mode 100644 index 00000000000000..ab8b4db7d73569 --- /dev/null +++ b/selfdrive/controls/lib/cluster/LICENSE @@ -0,0 +1,13 @@ +Copyright: + * fastcluster_dm.cpp & fastcluster_R_dm.cpp: + © 2011 Daniel Müllner + * fastcluster.(h|cpp) & demo.cpp & plotresult.r: + © 2018 Christoph Dalitz +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/selfdrive/controls/lib/cluster/README b/selfdrive/controls/lib/cluster/README new file mode 100644 index 00000000000000..acb18bc72b71d4 --- /dev/null +++ b/selfdrive/controls/lib/cluster/README @@ -0,0 +1,79 @@ +C++ interface to fast hierarchical clustering algorithms +======================================================== + +This is a simplified C++ interface to fast implementations of hierarchical +clustering by Daniel Müllner. The original library with interfaces to R +and Python is described in: + +Daniel Müllner: "fastcluster: Fast Hierarchical, Agglomerative Clustering +Routines for R and Python." Journal of Statistical Software 53 (2013), +no. 9, pp. 1–18, http://www.jstatsoft.org/v53/i09/ + + +Usage of the library +-------------------- + +For using the library, the following source files are needed: + +fastcluster_dm.cpp, fastcluster_R_dm.cpp + original code by Daniel Müllner + these are included by fastcluster.cpp via #include, and therefore + need not be compiled to object code + +fastcluster.[h|cpp] + simplified C++ interface + fastcluster.cpp is the only file that must be compiled + +The library provides the clustering function *hclust_fast* for +creating the dendrogram information in an encoding as used by the +R function *hclust*. For a description of the parameters, see fastcluster.h. +Its parameter *method* can be one of + +HCLUST_METHOD_SINGLE + single link with the minimum spanning tree algorithm (Rohlf, 1973) + +HHCLUST_METHOD_COMPLETE + complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984) + +HCLUST_METHOD_AVERAGE + complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984) + +HCLUST_METHOD_MEDIAN + median link with the generic algorithm (Müllner, 2011) + +For splitting the dendrogram into clusters, the two functions *cutree_k* +and *cutree_cdist* are provided. + +Note that output parameters must be allocated beforehand, e.g. + int* merge = new int[2*(npoints-1)]; +For a complete usage example, see lines 135-142 of demo.cpp. + + +Demonstration program +--------------------- + +A simple demo is implemented in demo.cpp, which can be compiled and run with + + make + ./hclust-demo -m complete lines.csv + +It creates two clusters of line segments such that the segment angle between +line segments of different clusters have a maximum (cosine) dissimilarity. +For visualizing the result, plotresult.r can be used as follows +(requires R to be installed): + + ./hclust-demo -m complete lines.csv | Rscript plotresult.r + + +Authors & Copyright +------------------- + +Daniel Müllner, 2011, +Christoph Dalitz, 2018, + + +License +------- + +This code is provided under a BSD-style license. +See the file LICENSE for details. diff --git a/selfdrive/controls/lib/cluster/SConscript b/selfdrive/controls/lib/cluster/SConscript new file mode 100644 index 00000000000000..97eb4300d4da66 --- /dev/null +++ b/selfdrive/controls/lib/cluster/SConscript @@ -0,0 +1,8 @@ +Import('env') + +fc = env.SharedLibrary("fastcluster", "fastcluster.cpp") + +# TODO: how do I gate on test +#env.Program("test", ["test.cpp"], LIBS=[fc]) +#valgrind --leak-check=full ./test + diff --git a/system/ui/lib/__init__.py b/selfdrive/controls/lib/cluster/__init__.py similarity index 100% rename from system/ui/lib/__init__.py rename to selfdrive/controls/lib/cluster/__init__.py diff --git a/selfdrive/controls/lib/cluster/fastcluster.cpp b/selfdrive/controls/lib/cluster/fastcluster.cpp new file mode 100644 index 00000000000000..d2b557d6f5be20 --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster.cpp @@ -0,0 +1,218 @@ +// +// C++ standalone verion of fastcluster by Daniel Müllner +// +// Copyright: Christoph Dalitz, 2018 +// Daniel Müllner, 2011 +// License: BSD style license +// (see the file LICENSE for details) +// + + +#include +#include +#include + + +extern "C" { +#include "fastcluster.h" +} + +// Code by Daniel Müllner +// workaround to make it usable as a standalone version (without R) +bool fc_isnan(double x) { return false; } +#include "fastcluster_dm.cpp" +#include "fastcluster_R_dm.cpp" + +extern "C" { +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the cluster result is split into nclust clusters. +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// nclust = number of clusters +// Output arguments: +// labels = allocated integer array of size n for result +// + void cutree_k(int n, const int* merge, int nclust, int* labels) { + + int k,m1,m2,j,l; + + if (nclust > n || nclust < 2) { + for (j=0; j last_merge(n, 0); + for (k=1; k<=(n-nclust); k++) { + // (m1,m2) = merge[k,] + m1 = merge[k-1]; + m2 = merge[n-1+k-1]; + if (m1 < 0 && m2 < 0) { // both single observables + last_merge[-m1-1] = last_merge[-m2-1] = k; + } + else if (m1 < 0 || m2 < 0) { // one is a cluster + if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2; + // merging single observable and cluster + for(l = 0; l < n; l++) + if (last_merge[l] == m1) + last_merge[l] = k; + last_merge[j-1] = k; + } + else { // both cluster + for(l=0; l < n; l++) { + if( last_merge[l] == m1 || last_merge[l] == m2 ) + last_merge[l] = k; + } + } + } + + // assign cluster labels + int label = 0; + std::vector z(n,-1); + for (j=0; j= cdist + // + // Input arguments: + // n = number of observables + // merge = clustering result in R format + // height = cluster distance at each merge step + // cdist = cutoff cluster distance + // Output arguments: + // labels = allocated integer array of size n for result + // + void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) { + + int k; + + for (k=0; k<(n-1); k++) { + if (height[k] >= cdist) { + break; + } + } + cutree_k(n, merge, n-k, labels); + } + + + // + // Hierarchical clustering with one of Daniel Muellner's fast algorithms + // + // Input arguments: + // n = number of observables + // distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing + // the upper triangle (without diagonal elements) of the distance + // matrix, e.g. for n=4: + // d00 d01 d02 d03 + // d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23 + // d20 d21 d22 d23 + // d30 d31 d32 d33 + // method = cluster metric (see enum method_code) + // Output arguments: + // merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result. + // Result follows R hclust convention: + // - observabe indices start with one + // - merge[i][] contains the merged nodes in step i + // - merge[i][j] is negative when the node is an atom + // height = allocated (n-1) array with distances at each merge step + // Return code: + // 0 = ok + // 1 = invalid method + // + int hclust_fast(int n, double* distmat, int method, int* merge, double* height) { + + // call appropriate culstering function + cluster_result Z2(n-1); + if (method == HCLUST_METHOD_SINGLE) { + // single link + MST_linkage_core(n, distmat, Z2); + } + else if (method == HCLUST_METHOD_COMPLETE) { + // complete link + NN_chain_core(n, distmat, NULL, Z2); + } + else if (method == HCLUST_METHOD_AVERAGE) { + // best average distance + double* members = new double[n]; + for (int i=0; i(n, distmat, members, Z2); + delete[] members; + } + else if (method == HCLUST_METHOD_MEDIAN) { + // best median distance (beware: O(n^3)) + generic_linkage(n, distmat, NULL, Z2); + } + else if (method == HCLUST_METHOD_CENTROID) { + // best centroid distance (beware: O(n^3)) + double* members = new double[n]; + for (int i=0; i(n, distmat, members, Z2); + delete[] members; + } + else { + return 1; + } + + int* order = new int[n]; + if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) { + generate_R_dendrogram(merge, height, order, Z2, n); + } else { + generate_R_dendrogram(merge, height, order, Z2, n); + } + delete[] order; // only needed for visualization + + return 0; + } + + + // Build condensed distance matrix + // Input arguments: + // n = number of observables + // m = dimension of observable + // Output arguments: + // out = allocated integer array of size n * (n - 1) / 2 for result + void hclust_pdist(int n, int m, double* pts, double* out) { + int ii = 0; + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { + // Compute euclidian distance + double d = 0; + for (int k = 0; k < m; k ++) { + double error = pts[i * m + k] - pts[j * m + k]; + d += (error * error); + } + out[ii] = d;//sqrt(d); + ii++; + } + } + } + + void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) { + double* pdist = new double[n * (n - 1) / 2]; + int* merge = new int[2 * (n - 1)]; + double* height = new double[n - 1]; + + hclust_pdist(n, m, pts, pdist); + hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height); + cutree_cdist(n, merge, height, dist, idx); + + delete[] pdist; + delete[] merge; + delete[] height; + } +} diff --git a/selfdrive/controls/lib/cluster/fastcluster.h b/selfdrive/controls/lib/cluster/fastcluster.h new file mode 100644 index 00000000000000..56c63b6a5e291a --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster.h @@ -0,0 +1,77 @@ +// +// C++ standalone verion of fastcluster by Daniel Muellner +// +// Copyright: Daniel Muellner, 2011 +// Christoph Dalitz, 2018 +// License: BSD style license +// (see the file LICENSE for details) +// + +#ifndef fastclustercpp_H +#define fastclustercpp_H + +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the cluster result is split into nclust clusters. +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// nclust = number of clusters +// Output arguments: +// labels = allocated integer array of size n for result +// +void cutree_k(int n, const int* merge, int nclust, int* labels); + +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the hierarchical clsutering is stopped at cluster distance cdist +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// height = cluster distance at each merge step +// cdist = cutoff cluster distance +// Output arguments: +// labels = allocated integer array of size n for result +// +void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels); + +// +// Hierarchical clustering with one of Daniel Muellner's fast algorithms +// +// Input arguments: +// n = number of observables +// distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing +// the upper triangle (without diagonal elements) of the distance +// matrix, e.g. for n=4: +// d00 d01 d02 d03 +// d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23 +// d20 d21 d22 d23 +// d30 d31 d32 d33 +// method = cluster metric (see enum method_code) +// Output arguments: +// merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result. +// Result follows R hclust convention: +// - observabe indices start with one +// - merge[i][] contains the merged nodes in step i +// - merge[i][j] is negative when the node is an atom +// height = allocated (n-1) array with distances at each merge step +// Return code: +// 0 = ok +// 1 = invalid method +// +int hclust_fast(int n, double* distmat, int method, int* merge, double* height); +enum hclust_fast_methods { + HCLUST_METHOD_SINGLE = 0, + HCLUST_METHOD_COMPLETE = 1, + HCLUST_METHOD_AVERAGE = 2, + HCLUST_METHOD_MEDIAN = 3, + HCLUST_METHOD_CENTROID = 5, +}; + +void hclust_pdist(int n, int m, double* pts, double* out); +void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx); + + +#endif diff --git a/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp b/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp new file mode 100644 index 00000000000000..cbe126c15e33cc --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp @@ -0,0 +1,115 @@ +// +// Excerpt from fastcluster_R.cpp +// +// Copyright: Daniel Müllner, 2011 +// + +struct pos_node { + t_index pos; + int node; +}; + +void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) { + /* Parameters: + N : number of data points + merge : (N-1)×2 array which specifies the node indices which are + merged in each step of the clustering procedure. + Negative entries -1...-N point to singleton nodes, while + positive entries 1...(N-1) point to nodes which are themselves + parents of other nodes. + node_size : array of node sizes - makes it easier + order : output array of size N + + Runtime: Θ(N) + */ + auto_array_ptr queue(N/2); + + int parent; + int child; + t_index pos = 0; + + queue[0].pos = 0; + queue[0].node = N-2; + t_index idx = 1; + + do { + --idx; + pos = queue[idx].pos; + parent = queue[idx].node; + + // First child + child = merge[parent]; + if (child<0) { // singleton node, write this into the 'order' array. + order[pos] = -child; + ++pos; + } + else { /* compound node: put it on top of the queue and decompose it + in a later iteration. */ + queue[idx].pos = pos; + queue[idx].node = child-1; // convert index-1 based to index-0 based + ++idx; + pos += node_size[child-1]; + } + // Second child + child = merge[parent+N-1]; + if (child<0) { + order[pos] = -child; + } + else { + queue[idx].pos = pos; + queue[idx].node = child-1; + ++idx; + } + } while (idx>0); +} + +#define size_(r_) ( ((r_ +void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) { + // The array "nodes" is a union-find data structure for the cluster + // identites (only needed for unsorted cluster_result input). + union_find nodes(sorted ? 0 : N); + if (!sorted) { + std::stable_sort(Z2[0], Z2[N-1]); + } + + t_index node1, node2; + auto_array_ptr node_size(N-1); + + for (t_index i=0; inode1; + node2 = Z2[i]->node2; + } + else { + node1 = nodes.Find(Z2[i]->node1); + node2 = nodes.Find(Z2[i]->node2); + // Merge the nodes in the union-find data structure by making them + // children of a new node. + nodes.Union(node1, node2); + } + // Sort the nodes in the output array. + if (node1>node2) { + t_index tmp = node1; + node1 = node2; + node2 = tmp; + } + /* Conversion between labeling conventions. + Input: singleton nodes 0,...,N-1 + compound nodes N,...,2N-2 + Output: singleton nodes -1,...,-N + compound nodes 1,...,N + */ + merge[i] = (node1(node1)-1 + : static_cast(node1)-N+1; + merge[i+N-1] = (node2(node2)-1 + : static_cast(node2)-N+1; + height[i] = Z2[i]->dist; + node_size[i] = size_(node1) + size_(node2); + } + + order_nodes(N, merge, node_size, order); +} diff --git a/selfdrive/controls/lib/cluster/fastcluster_dm.cpp b/selfdrive/controls/lib/cluster/fastcluster_dm.cpp new file mode 100644 index 00000000000000..ee6670c2042420 --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster_dm.cpp @@ -0,0 +1,1794 @@ +/* + fastcluster: Fast hierarchical clustering routines for R and Python + + Copyright © 2011 Daniel Müllner + + + This library implements various fast algorithms for hierarchical, + agglomerative clustering methods: + + (1) Algorithms for the "stored matrix approach": the input is the array of + pairwise dissimilarities. + + MST_linkage_core: single linkage clustering with the "minimum spanning + tree algorithm (Rohlfs) + + NN_chain_core: nearest-neighbor-chain algorithm, suitable for single, + complete, average, weighted and Ward linkage (Murtagh) + + generic_linkage: generic algorithm, suitable for all distance update + formulas (Müllner) + + (2) Algorithms for the "stored data approach": the input are points in a + vector space. + + MST_linkage_core_vector: single linkage clustering for vector data + + generic_linkage_vector: generic algorithm for vector data, suitable for + the Ward, centroid and median methods. + + generic_linkage_vector_alternative: alternative scheme for updating the + nearest neighbors. This method seems faster than "generic_linkage_vector" + for the centroid and median methods but slower for the Ward method. + + All these implementation treat infinity values correctly. They throw an + exception if a NaN distance value occurs. +*/ + +// Older versions of Microsoft Visual Studio do not have the fenv header. +#ifdef _MSC_VER +#if (_MSC_VER == 1500 || _MSC_VER == 1600) +#define NO_INCLUDE_FENV +#endif +#endif +// NaN detection via fenv might not work on systems with software +// floating-point emulation (bug report for Debian armel). +#ifdef __SOFTFP__ +#define NO_INCLUDE_FENV +#endif +#ifdef NO_INCLUDE_FENV +#pragma message("Do not use fenv header.") +#else +//#pragma message("Use fenv header. If there is a warning about unknown #pragma STDC FENV_ACCESS, this can be ignored.") +//#pragma STDC FENV_ACCESS on +#include +#endif + +#include // for std::pow, std::sqrt +#include // for std::ptrdiff_t +#include // for std::numeric_limits<...>::infinity() +#include // for std::fill_n +#include // for std::runtime_error +#include // for std::string + +#include // also for DBL_MAX, DBL_MIN +#ifndef DBL_MANT_DIG +#error The constant DBL_MANT_DIG could not be defined. +#endif +#define T_FLOAT_MANT_DIG DBL_MANT_DIG + +#ifndef LONG_MAX +#include +#endif +#ifndef LONG_MAX +#error The constant LONG_MAX could not be defined. +#endif +#ifndef INT_MAX +#error The constant INT_MAX could not be defined. +#endif + +#ifndef INT32_MAX +#ifdef _MSC_VER +#if _MSC_VER >= 1600 +#define __STDC_LIMIT_MACROS +#include +#else +typedef __int32 int_fast32_t; +typedef __int64 int64_t; +#endif +#else +#define __STDC_LIMIT_MACROS +#include +#endif +#endif + +#define FILL_N std::fill_n +#ifdef _MSC_VER +#if _MSC_VER < 1600 +#undef FILL_N +#define FILL_N stdext::unchecked_fill_n +#endif +#endif + +// Suppress warnings about (potentially) uninitialized variables. +#ifdef _MSC_VER + #pragma warning (disable:4700) +#endif + +#ifndef HAVE_DIAGNOSTIC +#if __GNUC__ > 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ >= 6)) +#define HAVE_DIAGNOSTIC 1 +#endif +#endif + +#ifndef HAVE_VISIBILITY +#if __GNUC__ >= 4 +#define HAVE_VISIBILITY 1 +#endif +#endif + +/* Since the public interface is given by the Python respectively R interface, + * we do not want other symbols than the interface initalization routines to be + * visible in the shared object file. The "visibility" switch is a GCC concept. + * Hiding symbols keeps the relocation table small and decreases startup time. + * See http://gcc.gnu.org/wiki/Visibility + */ +#if HAVE_VISIBILITY +#pragma GCC visibility push(hidden) +#endif + +typedef int_fast32_t t_index; +#ifndef INT32_MAX +#define MAX_INDEX 0x7fffffffL +#else +#define MAX_INDEX INT32_MAX +#endif +#if (LONG_MAX < MAX_INDEX) +#error The integer format "t_index" must not have a greater range than "long int". +#endif +#if (INT_MAX > MAX_INDEX) +#error The integer format "int" must not have a greater range than "t_index". +#endif +typedef double t_float; + +/* Method codes. + + These codes must agree with the METHODS array in fastcluster.R and the + dictionary mthidx in fastcluster.py. +*/ +enum method_codes { + // non-Euclidean methods + METHOD_METR_SINGLE = 0, + METHOD_METR_COMPLETE = 1, + METHOD_METR_AVERAGE = 2, + METHOD_METR_WEIGHTED = 3, + METHOD_METR_WARD = 4, + METHOD_METR_WARD_D = METHOD_METR_WARD, + METHOD_METR_CENTROID = 5, + METHOD_METR_MEDIAN = 6, + METHOD_METR_WARD_D2 = 7, + + MIN_METHOD_CODE = 0, + MAX_METHOD_CODE = 7 +}; + +enum method_codes_vector { + // Euclidean methods + METHOD_VECTOR_SINGLE = 0, + METHOD_VECTOR_WARD = 1, + METHOD_VECTOR_CENTROID = 2, + METHOD_VECTOR_MEDIAN = 3, + + MIN_METHOD_VECTOR_CODE = 0, + MAX_METHOD_VECTOR_CODE = 3 +}; + +// self-destructing array pointer +template +class auto_array_ptr{ +private: + type * ptr; + auto_array_ptr(auto_array_ptr const &); // non construction-copyable + auto_array_ptr& operator=(auto_array_ptr const &); // non copyable +public: + auto_array_ptr() + : ptr(NULL) + { } + template + auto_array_ptr(index const size) + : ptr(new type[size]) + { } + template + auto_array_ptr(index const size, value const val) + : ptr(new type[size]) + { + FILL_N(ptr, size, val); + } + ~auto_array_ptr() { + delete [] ptr; } + void free() { + delete [] ptr; + ptr = NULL; + } + template + void init(index const size) { + ptr = new type [size]; + } + template + void init(index const size, value const val) { + init(size); + FILL_N(ptr, size, val); + } + inline operator type *() const { return ptr; } +}; + +struct node { + t_index node1, node2; + t_float dist; +}; + +inline bool operator< (const node a, const node b) { + return (a.dist < b.dist); +} + +class cluster_result { +private: + auto_array_ptr Z; + t_index pos; + +public: + cluster_result(const t_index size) + : Z(size) + , pos(0) + {} + + void append(const t_index node1, const t_index node2, const t_float dist) { + Z[pos].node1 = node1; + Z[pos].node2 = node2; + Z[pos].dist = dist; + ++pos; + } + + node * operator[] (const t_index idx) const { return Z + idx; } + + /* Define several methods to postprocess the distances. All these functions + are monotone, so they do not change the sorted order of distances. */ + + void sqrt() const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = std::sqrt(ZZ->dist); + } + } + + void sqrt(const t_float) const { // ignore the argument + sqrt(); + } + + void sqrtdouble(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = std::sqrt(2*ZZ->dist); + } + } + + #ifdef R_pow + #define my_pow R_pow + #else + #define my_pow std::pow + #endif + + void power(const t_float p) const { + t_float const q = 1/p; + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = my_pow(ZZ->dist,q); + } + } + + void plusone(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist += 1; + } + } + + void divide(const t_float denom) const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist /= denom; + } + } +}; + +class doubly_linked_list { + /* + Class for a doubly linked list. Initially, the list is the integer range + [0, size]. We provide a forward iterator and a method to delete an index + from the list. + + Typical use: for (i=L.start; L succ; + +private: + auto_array_ptr pred; + // Not necessarily private, we just do not need it in this instance. + +public: + doubly_linked_list(const t_index size) + // Initialize to the given size. + : start(0) + , succ(size+1) + , pred(size+1) + { + for (t_index i=0; i(2*N-3-(r_))*(r_)>>1)+(c_)-1] ) +// Z is an ((N-1)x4)-array +#define Z_(_r, _c) (Z[(_r)*4 + (_c)]) + +/* + Lookup function for a union-find data structure. + + The function finds the root of idx by going iteratively through all + parent elements until a root is found. An element i is a root if + nodes[i] is zero. To make subsequent searches faster, the entry for + idx and all its parents is updated with the root element. + */ +class union_find { +private: + auto_array_ptr parent; + t_index nextparent; + +public: + union_find(const t_index size) + : parent(size>0 ? 2*size-1 : 0, 0) + , nextparent(size) + { } + + t_index Find (t_index idx) const { + if (parent[idx] != 0 ) { // a → b + t_index p = idx; + idx = parent[idx]; + if (parent[idx] != 0 ) { // a → b → c + do { + idx = parent[idx]; + } while (parent[idx] != 0); + do { + t_index tmp = parent[p]; + parent[p] = idx; + p = tmp; + } while (parent[p] != idx); + } + } + return idx; + } + + void Union (const t_index node1, const t_index node2) { + parent[node1] = parent[node2] = nextparent++; + } +}; + +class nan_error{}; +#ifdef FE_INVALID +class fenv_error{}; +#endif + +static void MST_linkage_core(const t_index N, const t_float * const D, + cluster_result & Z2) { +/* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +/* Functions for the update of the dissimilarity array */ + +inline static void f_single( t_float * const b, const t_float a ) { + if (*b > a) *b = a; +} +inline static void f_complete( t_float * const b, const t_float a ) { + if (*b < a) *b = a; +} +inline static void f_average( t_float * const b, const t_float a, const t_float s, const t_float t) { + *b = s*a + t*(*b); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_weighted( t_float * const b, const t_float a) { + *b = (a+*b)*.5; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_ward( t_float * const b, const t_float a, const t_float c, const t_float s, const t_float t, const t_float v) { + *b = ( (v+s)*a - v*c + (v+t)*(*b) ) / (s+t+v); + //*b = a+(*b)-(t*a+s*(*b)+v*c)/(s+t+v); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_centroid( t_float * const b, const t_float a, const t_float stc, const t_float s, const t_float t) { + *b = s*a - stc + t*(*b); + #ifndef FE_INVALID + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_median( t_float * const b, const t_float a, const t_float c_4) { + *b = (a+(*b))*.5 - c_4; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} + +template +static void NN_chain_core(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { +/* + N: integer + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + This is the NN-chain algorithm, described on page 86 in the following book: + + Fionn Murtagh, Multidimensional Clustering Algorithms, + Vienna, Würzburg: Physica-Verlag, 1985. +*/ + t_index i; + + auto_array_ptr NN_chain(N); + t_index NN_chain_tip = 0; + + t_index idx1, idx2; + + t_float size1, size2; + doubly_linked_list active_nodes(N); + + t_float min; + + for (t_float const * DD=D; DD!=D+(static_cast(N)*(N-1)>>1); + ++DD) { +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*DD)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + } + + #ifdef FE_INVALID + if (feclearexcept(FE_INVALID)) throw fenv_error(); + #endif + + for (t_index j=0; jidx2) { + t_index tmp = idx1; + idx1 = idx2; + idx2 = tmp; + } + + if (method==METHOD_METR_AVERAGE || + method==METHOD_METR_WARD) { + size1 = static_cast(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + + // Remove the smaller index from the valid indices (active_nodes). + active_nodes.remove(idx1); + + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (i=active_nodes.start; i(members[i]); + for (i=active_nodes.start; i(members[i]) ); + // Update the distance matrix in the range (idx1, idx2). + for (; i(members[i]) ); + // Update the distance matrix in the range (idx2, N). + for (i=active_nodes.succ[idx2]; i(members[i]) ); + break; + + default: + throw std::runtime_error(std::string("Invalid method.")); + } + } + #ifdef FE_INVALID + if (fetestexcept(FE_INVALID)) throw fenv_error(); + #endif +} + +class binary_min_heap { + /* + Class for a binary min-heap. The data resides in an array A. The elements of + A are not changed but two lists I and R of indices are generated which point + to elements of A and backwards. + + The heap tree structure is + + H[2*i+1] H[2*i+2] + \ / + \ / + ≤ ≤ + \ / + \ / + H[i] + + where the children must be less or equal than their parent. Thus, H[0] + contains the minimum. The lists I and R are made such that H[i] = A[I[i]] + and R[I[i]] = i. + + This implementation is not designed to handle NaN values. + */ +private: + t_float * const A; + t_index size; + auto_array_ptr I; + auto_array_ptr R; + + // no default constructor + binary_min_heap(); + // noncopyable + binary_min_heap(binary_min_heap const &); + binary_min_heap & operator=(binary_min_heap const &); + +public: + binary_min_heap(t_float * const A_, const t_index size_) + : A(A_), size(size_), I(size), R(size) + { // Allocate memory and initialize the lists I and R to the identity. This + // does not make it a heap. Call heapify afterwards! + for (t_index i=0; i>1); idx>0; ) { + --idx; + update_geq_(idx); + } + } + + inline t_index argmin() const { + // Return the minimal element. + return I[0]; + } + + void heap_pop() { + // Remove the minimal element from the heap. + --size; + I[0] = I[size]; + R[I[0]] = 0; + update_geq_(0); + } + + void remove(t_index idx) { + // Remove an element from the heap. + --size; + R[I[size]] = R[idx]; + I[R[idx]] = I[size]; + if ( H(size)<=A[idx] ) { + update_leq_(R[idx]); + } + else { + update_geq_(R[idx]); + } + } + + void replace ( const t_index idxold, const t_index idxnew, + const t_float val) { + R[idxnew] = R[idxold]; + I[R[idxnew]] = idxnew; + if (val<=A[idxold]) + update_leq(idxnew, val); + else + update_geq(idxnew, val); + } + + void update ( const t_index idx, const t_float val ) const { + // Update the element A[i] with val and re-arrange the indices to preserve + // the heap condition. + if (val<=A[idx]) + update_leq(idx, val); + else + update_geq(idx, val); + } + + void update_leq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not more than the old value. + A[idx] = val; + update_leq_(R[idx]); + } + + void update_geq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not less than the old value. + A[idx] = val; + update_geq_(R[idx]); + } + +private: + void update_leq_ (t_index i) const { + t_index j; + for ( ; (i>0) && ( H(i)>1) ); i=j) + heap_swap(i,j); + } + + void update_geq_ (t_index i) const { + t_index j; + for ( ; (j=2*i+1)=H(i) ) { + ++j; + if ( j>=size || H(j)>=H(i) ) break; + } + else if ( j+1 +static void generic_linkage(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { + /* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + */ + + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float size1, size2; // and their cardinalities + + t_float min; // minimum and row index for nearest-neighbor search + t_index idx; + + for (i=0; ii} D(i,j) for i in range(N-1) + t_float const * DD = D; + for (i=0; i::infinity(); + for (idx=j=i+1; ji} D(i,j) + + Normally, we have equality. However, this minimum may become invalid due + to the updates in the distance matrix. The rules are: + + 1) If mindist[i] is equal to D(i, n_nghbr[i]), this is the correct + minimum and n_nghbr[i] is a nearest neighbor. + + 2) If mindist[i] is smaller than D(i, n_nghbr[i]), this might not be the + correct minimum. The minimum needs to be recomputed. + + 3) mindist[i] is never bigger than the true minimum. Hence, we never + miss the true minimum if we take the smallest mindist entry, + re-compute the value if necessary (thus maybe increasing it) and + looking for the now smallest mindist entry until a valid minimal + entry is found. This step is done in the lines below. + + The update process for D below takes care that these rules are + fulfilled. This makes sure that the minima in the rows D(i,i+1:)of D are + re-calculated when necessary but re-calculation is avoided whenever + possible. + + The re-calculation of the minima makes the worst-case runtime of this + algorithm cubic in N. We avoid this whenever possible, and in most cases + the runtime appears to be quadratic. + */ + idx1 = nn_distances.argmin(); + if (method != METHOD_METR_SINGLE) { + while ( mindist[idx1] < D_(idx1, n_nghbr[idx1]) ) { + // Recompute the minimum mindist[idx1] and n_nghbr[idx1]. + n_nghbr[idx1] = j = active_nodes.succ[idx1]; // exists, maximally N-1 + min = D_(idx1,j); + for (j=active_nodes.succ[j]; j(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + Z2.append(node1, node2, mindist[idx1]); + + // Remove idx1 from the list of active indices (active_nodes). + active_nodes.remove(idx1); + // Index idx2 now represents the new (merged) node with label N+i. + row_repr[idx2] = N+i; + + // Update the distance matrix + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (j=active_nodes.start; j(members[j]) ); + if (n_nghbr[j] == idx1) + n_nghbr[j] = idx2; + } + // Update the distance matrix in the range (idx1, idx2). + for (; j(members[j]) ); + if (D_(j, idx2) < mindist[j]) { + nn_distances.update_leq(j, D_(j, idx2)); + n_nghbr[j] = idx2; + } + } + // Update the distance matrix in the range (idx2, N). + if (idx2(members[j]) ); + min = D_(idx2,j); + for (j=active_nodes.succ[j]; j(members[j]) ); + if (D_(idx2,j) < min) { + min = D_(idx2,j); + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + break; + + case METHOD_METR_CENTROID: { + /* + Centroid linkage. + + Shorter and longer distances can occur, not bigger than max(d1,d2) + but maybe smaller than min(d1,d2). + */ + // Update the distance matrix in the range [start, idx1). + t_float s = size1/(size1+size2); + t_float t = size2/(size1+size2); + t_float stc = s*t*mindist[idx1]; + for (j=active_nodes.start; j +static void MST_linkage_core_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { +/* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +template +static void generic_linkage_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float min; // minimum and row index for nearest-neighbor search + + for (i=0; ii} D(i,j) for i in range(N-1) + for (i=0; i::infinity(); + t_index idx; + for (idx=j=i+1; j(i,j); + } + if (tmp(idx1,j); + for (j=active_nodes.succ[j]; j(idx1,j); + if (tmp(j, idx2); + if (tmp < mindist[j]) { + nn_distances.update_leq(j, tmp); + n_nghbr[j] = idx2; + } + else if (n_nghbr[j] == idx2) + n_nghbr[j] = idx1; // invalidate + } + // Find the nearest neighbor for idx2. + if (idx2(idx2,j); + for (j=active_nodes.succ[j]; j(idx2, j); + if (tmp < min) { + min = tmp; + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + } + } +} + +template +static void generic_linkage_vector_alternative(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j=0; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(2*N-2); // array of nearest neighbors + auto_array_ptr mindist(2*N-2); // distances to the nearest neighbors + + doubly_linked_list active_nodes(N+N_1); + binary_min_heap nn_distances(&*mindist, N_1, 2*N-2, 1); // minimum heap + // structure for the distance to the nearest neighbor of each point + + t_float min; // minimum for nearest-neighbor searches + + // Initialize the minimal distances: + // Find the nearest neighbor of each point. + // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1) + for (i=1; i::infinity(); + t_index idx; + for (idx=j=0; j(i,j); + } + if (tmp + +extern "C" { +#include "fastcluster.h" +} + + +int main(int argc, const char* argv[]) { + const int n = 11; + const int m = 3; + double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019, + 91.61999817, -0.31999999, -2.75, + 31.38000031, 0.40000001, -0.2, + 89.57999725, -8.07999992, -18.04999924, + 53.42000122, 0.63999999, -0.175, + 31.38000031, 0.47999999, -0.2, + 36.33999939, 0.16, -0.2, + 53.33999939, 0.95999998, -0.175, + 59.26000137, -9.76000023, -5.44999981, + 33.93999977, 0.40000001, -0.22499999, + 106.74000092, -5.76000023, -18.04999924}; + + int * idx = new int[n]; + int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6}; + + cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx); + + for (int i = 0; i < n; i++) { + assert(idx[i] == correct_idx[i]); + } + + delete[] idx; + delete[] correct_idx; + delete[] pts; +} diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index ee4567f1e988e3..d41d6780e3e7db 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,31 +1,31 @@ from cereal import log -from openpilot.common.constants import CV -from openpilot.common.realtime import DT_MDL +from common.conversions import Conversions as CV +from common.realtime import DT_MDL -LaneChangeState = log.LaneChangeState -LaneChangeDirection = log.LaneChangeDirection +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection -LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS +LANE_CHANGE_SPEED_MIN = 15 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. DESIRES = { LaneChangeDirection.none: { - LaneChangeState.off: log.Desire.none, - LaneChangeState.preLaneChange: log.Desire.none, - LaneChangeState.laneChangeStarting: log.Desire.none, - LaneChangeState.laneChangeFinishing: log.Desire.none, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, }, LaneChangeDirection.left: { - LaneChangeState.off: log.Desire.none, - LaneChangeState.preLaneChange: log.Desire.none, - LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { - LaneChangeState.off: log.Desire.none, - LaneChangeState.preLaneChange: log.Desire.none, - LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight, + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, }, } @@ -38,11 +38,7 @@ def __init__(self): self.lane_change_ll_prob = 1.0 self.keep_pulse_timer = 0.0 self.prev_one_blinker = False - self.desire = log.Desire.none - - @staticmethod - def get_lane_change_direction(CS): - return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right + self.desire = log.LateralPlan.Desire.none def update(self, carstate, lateral_active, lane_change_prob): v_ego = carstate.vEgo @@ -57,13 +53,12 @@ def update(self, carstate, lateral_active, lane_change_prob): 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: - # Update lane change direction - self.lane_change_direction = self.get_lane_change_direction(carstate) + # Set lane change direction + self.lane_change_direction = LaneChangeDirection.left if \ + carstate.leftBlinker else LaneChangeDirection.right torque_applied = carstate.steeringPressed and \ ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or @@ -74,7 +69,6 @@ def update(self, carstate, lateral_active, lane_change_prob): if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting @@ -115,5 +109,5 @@ def update(self, carstate, lateral_active, lane_change_prob): self.keep_pulse_timer += DT_MDL if self.keep_pulse_timer > 1.0: self.keep_pulse_timer = 0.0 - elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): - self.desire = log.Desire.none + elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): + self.desire = log.LateralPlan.Desire.none diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index bf6dd04f603a2f..ffa83738344825 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,65 +1,134 @@ -import numpy as np -from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY -from openpilot.common.realtime import DT_CTRL, DT_MDL +import math -MIN_SPEED = 1.0 +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import clip, interp +from common.realtime import DT_MDL +from selfdrive.modeld.constants import T_IDXS + +# WARNING: this value was determined based on the model's training distribution, +# model predictions above this speed can be unpredictable +V_CRUISE_MAX = 145 # kph +V_CRUISE_MIN = 8 # kph +V_CRUISE_ENABLE_MIN = 40 # kph +V_CRUISE_INITIAL = 255 # kph + +LAT_MPC_N = 16 +LON_MPC_N = 32 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 -# This is a turn radius smaller than most cars can achieve -MAX_CURVATURE = 0.2 -MAX_VEL_ERR = 5.0 # m/s # EU guidelines -MAX_LATERAL_JERK = 5.0 # m/s^3 -MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 - - -def clamp(val, min_val, max_val): - clamped_val = float(np.clip(val, min_val, max_val)) - return clamped_val, clamped_val != val - -def smooth_value(val, prev_val, tau, dt=DT_MDL): - alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 - return alpha * val + (1 - alpha) * prev_val - -def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]: - # This function respects ISO lateral jerk and acceleration limits + a max curvature - v_ego = max(v_ego, MIN_SPEED) - max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - new_curvature = np.clip(new_curvature, - prev_curvature - max_curvature_rate * DT_CTRL, - prev_curvature + max_curvature_rate * DT_CTRL) - - roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY - max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation - min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation - new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) - - new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) - return float(new_curvature), limited_accel or limited_max_curv - - -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): - if len(speeds) == len(t_idxs): - v_now = speeds[0] - a_now = accels[0] - v_target = np.interp(action_t, t_idxs, speeds) - a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) +MAX_LATERAL_JERK = 5.0 + +ButtonType = car.CarState.ButtonEvent.Type +CRUISE_LONG_PRESS = 50 +CRUISE_NEAREST_FUNC = { + ButtonType.accelCruise: math.ceil, + ButtonType.decelCruise: math.floor, +} +CRUISE_INTERVAL_SIGN = { + ButtonType.accelCruise: +1, + ButtonType.decelCruise: -1, +} + + +class MPC_COST_LAT: + PATH = 1.0 + HEADING = 1.0 + STEER_RATE = 1.0 + + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + + +def rate_limit(new_value, last_value, dw_step, up_step): + return clip(new_value, last_value + dw_step, last_value + up_step) + + +def update_v_cruise(v_cruise_kph, v_ego, gas_pressed, buttonEvents, button_timers, enabled, metric): + # handle button presses. TODO: this should be in state_control, but a decelCruise press + # would have the effect of both enabling and changing speed is checked after the state transition + if not enabled: + return v_cruise_kph + + long_press = False + button_type = None + + # should be CV.MPH_TO_KPH, but this causes rounding errors + v_cruise_delta = 1. if metric else 1.6 + + for b in buttonEvents: + if b.type.raw in button_timers and not b.pressed: + if button_timers[b.type.raw] > CRUISE_LONG_PRESS: + return v_cruise_kph # end long press + button_type = b.type.raw + break else: - v_target = 0.0 - v_target_1sec = 0.0 - a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) - return a_target, should_stop - -def curv_from_psis(psi_target, psi_rate, vego, action_t): - vego = np.clip(vego, MIN_SPEED, np.inf) - curv_from_psi = psi_target / (vego * action_t) - return 2*curv_from_psi - psi_rate / vego - -def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t): - psi_target = np.interp(action_t, t_idxs, yaws) - psi_rate = yaw_rates[0] - return curv_from_psis(psi_target, psi_rate, vego, action_t) + for k in button_timers.keys(): + if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0: + button_type = k + long_press = True + break + + if button_type: + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval + v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta + else: + v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + + # If set is pressed while overriding, clip cruise speed to minimum of vEgo + if gas_pressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): + v_cruise_kph = max(v_cruise_kph, v_ego * CV.MS_TO_KPH) + + v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + + return v_cruise_kph + + +def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): + for b in buttonEvents: + # 250kph or above probably means we never had a set speed + if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and v_cruise_last < 250: + return v_cruise_last + + return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + + +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): + if len(psis) != CONTROL_N: + psis = [0.0]*CONTROL_N + curvatures = [0.0]*CONTROL_N + curvature_rates = [0.0]*CONTROL_N + v_ego = max(v_ego, 0.1) + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + + # MPC can plan to turn the wheel and turn back before t_delay. This means + # in high delay cases some corrections never even get commanded. So just use + # psi to calculate a simple linearization of desired curvature + current_curvature_desired = curvatures[0] + psi = interp(delay, T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + desired_curvature_rate = curvature_rates[0] + max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + safe_desired_curvature_rate = clip(desired_curvature_rate, + -max_curvature_rate, + max_curvature_rate) + safe_desired_curvature = clip(desired_curvature, + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) + + return safe_desired_curvature, safe_desired_curvature_rate diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py new file mode 100644 index 00000000000000..5139ead84bf195 --- /dev/null +++ b/selfdrive/controls/lib/events.py @@ -0,0 +1,943 @@ +import math +import os +from enum import IntEnum +from typing import Dict, Union, Callable, List, Optional + +from cereal import log, car +import cereal.messaging as messaging +from common.conversions import Conversions as CV +from common.realtime import DT_CTRL +from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +from system.version import get_short_branch + +AlertSize = log.ControlsState.AlertSize +AlertStatus = log.ControlsState.AlertStatus +VisualAlert = car.CarControl.HUDControl.VisualAlert +AudibleAlert = car.CarControl.HUDControl.AudibleAlert +EventName = car.CarEvent.EventName + + +# Alert priorities +class Priority(IntEnum): + LOWEST = 0 + LOWER = 1 + LOW = 2 + MID = 3 + HIGH = 4 + HIGHEST = 5 + + +# Event types +class ET: + ENABLE = 'enable' + PRE_ENABLE = 'preEnable' + OVERRIDE = 'override' + NO_ENTRY = 'noEntry' + WARNING = 'warning' + USER_DISABLE = 'userDisable' + SOFT_DISABLE = 'softDisable' + IMMEDIATE_DISABLE = 'immediateDisable' + PERMANENT = 'permanent' + + +# get event name from enum +EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} + + +class Events: + def __init__(self): + self.events: List[int] = [] + self.static_events: List[int] = [] + self.events_prev = dict.fromkeys(EVENTS.keys(), 0) + + @property + def names(self) -> List[int]: + return self.events + + def __len__(self) -> int: + return len(self.events) + + def add(self, event_name: int, static: bool=False) -> None: + if static: + self.static_events.append(event_name) + self.events.append(event_name) + + def clear(self) -> None: + self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()} + self.events = self.static_events.copy() + + def any(self, event_type: str) -> bool: + return any(event_type in EVENTS.get(e, {}) for e in self.events) + + def create_alerts(self, event_types: List[str], callback_args=None): + if callback_args is None: + callback_args = [] + + ret = [] + for e in self.events: + types = EVENTS[e].keys() + for et in event_types: + if et in types: + alert = EVENTS[e][et] + if not isinstance(alert, Alert): + alert = alert(*callback_args) + + if DT_CTRL * (self.events_prev[e] + 1) >= alert.creation_delay: + alert.alert_type = f"{EVENT_NAME[e]}/{et}" + alert.event_type = et + ret.append(alert) + return ret + + def add_from_msg(self, events): + for e in events: + self.events.append(e.name.raw) + + def to_msg(self): + ret = [] + for event_name in self.events: + event = car.CarEvent.new_message() + event.name = event_name + for event_type in EVENTS.get(event_name, {}): + setattr(event, event_type, True) + ret.append(event) + return ret + + +class Alert: + def __init__(self, + alert_text_1: str, + alert_text_2: str, + alert_status: log.ControlsState.AlertStatus, + alert_size: log.ControlsState.AlertSize, + priority: Priority, + visual_alert: car.CarControl.HUDControl.VisualAlert, + audible_alert: car.CarControl.HUDControl.AudibleAlert, + duration: float, + alert_rate: float = 0., + creation_delay: float = 0.): + + self.alert_text_1 = alert_text_1 + self.alert_text_2 = alert_text_2 + self.alert_status = alert_status + self.alert_size = alert_size + self.priority = priority + self.visual_alert = visual_alert + self.audible_alert = audible_alert + + self.duration = int(duration / DT_CTRL) + + self.alert_rate = alert_rate + self.creation_delay = creation_delay + + self.alert_type = "" + self.event_type: Optional[str] = None + + def __str__(self) -> str: + return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" + + def __gt__(self, alert2) -> bool: + if not isinstance(alert2, Alert): + return False + return self.priority > alert2.priority + + +class NoEntryAlert(Alert): + def __init__(self, alert_text_2: str, + alert_text_1: str = "openpilot Unavailable", + visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none): + super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, + AlertSize.mid, Priority.LOW, visual_alert, + AudibleAlert.refuse, 3.) + + +class SoftDisableAlert(Alert): + def __init__(self, alert_text_2: str): + super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, + AlertStatus.userPrompt, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, + AudibleAlert.warningSoft, 2.), + + +# less harsh version of SoftDisable, where the condition is user-triggered +class UserSoftDisableAlert(SoftDisableAlert): + def __init__(self, alert_text_2: str): + super().__init__(alert_text_2), + self.alert_text_1 = "openpilot will disengage" + + +class ImmediateDisableAlert(Alert): + def __init__(self, alert_text_2: str): + super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, + AudibleAlert.warningImmediate, 4.), + + +class EngagementAlert(Alert): + def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert): + super().__init__("", "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, + audible_alert, .2), + + +class NormalPermanentAlert(Alert): + def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.): + super().__init__(alert_text_1, alert_text_2, + AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small, + priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay), + + +class StartupAlert(Alert): + def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal): + super().__init__(alert_text_1, alert_text_2, + alert_status, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 10.), + + +# ********** helper functions ********** +def get_display_speed(speed_ms: float, metric: bool) -> str: + speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))) + unit = 'km/h' if metric else 'mph' + return f"{speed} {unit}" + + +# ********** alert callback functions ********** + +AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int], Alert] + + +def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return SoftDisableAlert(alert_text_2) + return func + +def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return UserSoftDisableAlert(alert_text_2) + return func + +def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + branch = get_short_branch("") # Ensure get_short_branch is cached to avoid lags on startup + if "REPLAY" in os.environ: + branch = "replay" + + return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt) + +def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}") + + +def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + return Alert( + f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) + + +def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + return Alert( + "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, + f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + + +def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + gps_integrated = sm['peripheralState'].pandaType in (log.PandaState.PandaType.uno, log.PandaState.PandaType.dos) + return Alert( + "Poor GPS reception", + "Hardware malfunctioning if sky is visible" if gps_integrated else "Check GPS antenna placement", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.) + +# *** debug alerts *** + +def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + full_perc = round(100. - sm['deviceState'].freeSpacePercent) + return NormalPermanentAlert("Out of Storage", f"{full_perc}% full") + + +def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan + err = CS.vEgo - mdl + msg = f"Speed Error: {err:.1f} m/s" + return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid") + + +def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] + msg = ', '.join(not_running) + return NoEntryAlert(msg, alert_text_1="Process Not Running") + + +def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])] + msg = ', '.join(bs[:4]) # can't fit too many on one line + return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes") + + +def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') + bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] + return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) + + +def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + rpy = sm['liveCalibration'].rpyCalib + yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) + pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) + angles = f"Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°" + return NormalPermanentAlert("Calibration Invalid", angles) + + +def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + cpu = max(sm['deviceState'].cpuTempC, default=0.) + gpu = max(sm['deviceState'].gpuTempC, default=0.) + temp = max((cpu, gpu, sm['deviceState'].memoryTempC)) + return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C") + + +def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used") + + +def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + x = max(sm['deviceState'].cpuUsagePercent, default=0.) + return NormalPermanentAlert("High CPU Usage", f"{x}% used") + + +def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped") + + +def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + text = "Cruise Mode Disabled" + if CP.carName == "honda": + text = "Main Switch Off" + return NoEntryAlert(text) + + +def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + axes = sm['testJoystick'].axes + gb, steer = list(axes)[:2] if len(axes) else (0., 0.) + vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" + return NormalPermanentAlert("Joystick Mode", vals) + + + +EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { + # ********** events with no alerts ********** + + EventName.stockFcw: {}, + + # ********** events only containing alerts displayed in all states ********** + + EventName.joystickDebug: { + ET.WARNING: joystick_alert, + ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), + }, + + EventName.controlsInitializing: { + ET.NO_ENTRY: NoEntryAlert("System Initializing"), + }, + + EventName.startup: { + ET.PERMANENT: StartupAlert("Be ready to take over at any time") + }, + + EventName.startupMaster: { + ET.PERMANENT: startup_master_alert, + }, + + # Car is recognized, but marked as dashcam only + EventName.startupNoControl: { + ET.PERMANENT: StartupAlert("Dashcam mode"), + }, + + # Car is not recognized + EventName.startupNoCar: { + ET.PERMANENT: StartupAlert("Dashcam mode for unsupported car"), + }, + + EventName.startupNoFw: { + ET.PERMANENT: StartupAlert("Car Unrecognized", + "Check comma power connections", + alert_status=AlertStatus.userPrompt), + }, + + EventName.dashcamMode: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + priority=Priority.LOWEST), + }, + + EventName.invalidLkasSetting: { + ET.PERMANENT: NormalPermanentAlert("Stock LKAS is on", + "Turn off stock LKAS to engage"), + }, + + EventName.cruiseMismatch: { + #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), + }, + + # openpilot doesn't recognize the car. This switches openpilot into a + # read-only mode. This can be solved by adding your fingerprint. + # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information + EventName.carUnrecognized: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + "Car Unrecognized", + priority=Priority.LOWEST), + }, + + EventName.stockAeb: { + ET.PERMANENT: Alert( + "BRAKE!", + "Stock AEB: Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), + ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"), + }, + + EventName.fcw: { + ET.PERMANENT: Alert( + "BRAKE!", + "Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.), + }, + + EventName.ldw: { + ET.PERMANENT: Alert( + "Lane Departure Detected", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.), + }, + + # ********** events only containing alerts that display while engaged ********** + + # openpilot tries to learn certain parameters about your car by observing + # how the car behaves to steering inputs from both human and openpilot driving. + # This includes: + # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle + # - tire stiffness: how much grip your tires have + # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight + # This alert is thrown when any of these values exceed a sanity check. This can be caused by + # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub + EventName.vehicleModelInvalid: { + ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"), + ET.SOFT_DISABLE: soft_disable_alert("Vehicle Parameter Identification Failed"), + }, + + EventName.steerTempUnavailableSilent: { + ET.WARNING: Alert( + "Steering Temporarily Unavailable", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), + }, + + EventName.preDriverDistracted: { + ET.WARNING: Alert( + "Pay Attention", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.promptDriverDistracted: { + ET.WARNING: Alert( + "Pay Attention", + "Driver Distracted", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), + }, + + EventName.driverDistracted: { + ET.WARNING: Alert( + "DISENGAGE IMMEDIATELY", + "Driver Distracted", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), + }, + + EventName.preDriverUnresponsive: { + ET.WARNING: Alert( + "Touch Steering Wheel: No Face Detected", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1, alert_rate=0.75), + }, + + EventName.promptDriverUnresponsive: { + ET.WARNING: Alert( + "Touch Steering Wheel", + "Driver Unresponsive", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), + }, + + EventName.driverUnresponsive: { + ET.WARNING: Alert( + "DISENGAGE IMMEDIATELY", + "Driver Unresponsive", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), + }, + + EventName.manualRestart: { + ET.WARNING: Alert( + "TAKE CONTROL", + "Resume Driving Manually", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + + EventName.resumeRequired: { + ET.WARNING: Alert( + "STOPPED", + "Press Resume to Go", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + + EventName.belowSteerSpeed: { + ET.WARNING: below_steer_speed_alert, + }, + + EventName.preLaneChangeLeft: { + ET.WARNING: Alert( + "Steer Left to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + + EventName.preLaneChangeRight: { + ET.WARNING: Alert( + "Steer Right to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + + EventName.laneChangeBlocked: { + ET.WARNING: Alert( + "Car Detected in Blindspot", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), + }, + + EventName.laneChange: { + ET.WARNING: Alert( + "Changing Lanes", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.steerSaturated: { + ET.WARNING: Alert( + "Take Control", + "Turn Exceeds Steering Limit", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 1.), + }, + + # Thrown when the fan is driven at >50% but is not rotating + EventName.fanMalfunction: { + ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"), + }, + + # Camera is not outputting frames + EventName.cameraMalfunction: { + ET.PERMANENT: camera_malfunction_alert, + ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"), + ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"), + }, + # Camera framerate too low + EventName.cameraFrameRate: { + ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"), + ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"), + ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"), + }, + + # Unused + EventName.gpsMalfunction: { + ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Likely Hardware Issue"), + }, + + # When the GPS position and localizer diverge the localizer is reset to the + # current GPS position. This alert is thrown when the localizer is reset + # more often than expected. + EventName.localizerMalfunction: { + # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"), + }, + + # ********** events that affect controls state transitions ********** + + EventName.pcmEnable: { + ET.ENABLE: EngagementAlert(AudibleAlert.engage), + }, + + EventName.buttonEnable: { + ET.ENABLE: EngagementAlert(AudibleAlert.engage), + }, + + EventName.pcmDisable: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + }, + + EventName.buttonCancel: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + }, + + EventName.brakeHold: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"), + }, + + EventName.parkBrake: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"), + }, + + EventName.pedalPressed: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Pedal Pressed", + visual_alert=VisualAlert.brakePressed), + }, + + EventName.pedalPressedPreEnable: { + ET.PRE_ENABLE: Alert( + "Release Pedal to Engage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), + }, + + EventName.gasPressedOverride: { + ET.OVERRIDE: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.wrongCarMode: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: wrong_car_mode_alert, + }, + + EventName.resumeBlocked: { + ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"), + }, + + EventName.wrongCruiseMode: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"), + }, + + EventName.steerTempUnavailable: { + ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"), + ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), + }, + + EventName.outOfSpace: { + ET.PERMANENT: out_of_space_alert, + ET.NO_ENTRY: NoEntryAlert("Out of Storage"), + }, + + EventName.belowEngageSpeed: { + ET.NO_ENTRY: below_engage_speed_alert, + }, + + EventName.sensorDataInvalid: { + ET.PERMANENT: Alert( + "Sensor Data Invalid", + "Ensure device is mounted securely", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.), + ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"), + ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), + }, + + EventName.noGps: { + ET.PERMANENT: no_gps_alert, + }, + + EventName.soundsUnavailable: { + ET.PERMANENT: NormalPermanentAlert("Speaker not found", "Reboot your Device"), + ET.NO_ENTRY: NoEntryAlert("Speaker not found"), + }, + + EventName.tooDistracted: { + ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), + }, + + EventName.overheat: { + ET.PERMANENT: overheat_alert, + ET.SOFT_DISABLE: soft_disable_alert("System Overheated"), + ET.NO_ENTRY: NoEntryAlert("System Overheated"), + }, + + EventName.wrongGear: { + ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"), + ET.NO_ENTRY: NoEntryAlert("Gear not D"), + }, + + # This alert is thrown when the calibration angles are outside of the acceptable range. + # For example if the device is pointed too much to the left or the right. + # Usually this can only be solved by removing the mount from the windshield completely, + # and attaching while making sure the device is pointed straight forward and is level. + # See https://comma.ai/setup for more information + EventName.calibrationInvalid: { + ET.PERMANENT: calibration_invalid_alert, + ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"), + ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), + }, + + EventName.calibrationIncomplete: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"), + ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), + }, + + EventName.doorOpen: { + ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), + ET.NO_ENTRY: NoEntryAlert("Door Open"), + }, + + EventName.seatbeltNotLatched: { + ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"), + ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), + }, + + EventName.espDisabled: { + ET.SOFT_DISABLE: soft_disable_alert("ESP Off"), + ET.NO_ENTRY: NoEntryAlert("ESP Off"), + }, + + EventName.lowBattery: { + ET.SOFT_DISABLE: soft_disable_alert("Low Battery"), + ET.NO_ENTRY: NoEntryAlert("Low Battery"), + }, + + # Different openpilot services communicate between each other at a certain + # interval. If communication does not follow the regular schedule this alert + # is thrown. This can mean a service crashed, did not broadcast a message for + # ten times the regular interval, or the average interval is more than 10% too high. + EventName.commIssue: { + ET.SOFT_DISABLE: soft_disable_alert("Communication Issue between Processes"), + ET.NO_ENTRY: comm_issue_alert, + }, + EventName.commIssueAvgFreq: { + ET.SOFT_DISABLE: soft_disable_alert("Low Communication Rate between Processes"), + ET.NO_ENTRY: NoEntryAlert("Low Communication Rate between Processes"), + }, + + EventName.controlsdLagging: { + ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"), + ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"), + }, + + # Thrown when manager detects a service exited unexpectedly while driving + EventName.processNotRunning: { + ET.NO_ENTRY: process_not_running_alert, + ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"), + }, + + EventName.radarFault: { + ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"), + ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), + }, + + # Every frame from the camera should be processed by the model. If modeld + # is not processing frames fast enough they have to be dropped. This alert is + # thrown when over 20% of frames are dropped. + EventName.modeldLagging: { + ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"), + ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"), + ET.PERMANENT: modeld_lagging_alert, + }, + + # Besides predicting the path, lane lines and lead car data the model also + # predicts the current velocity and rotation speed of the car. If the model is + # very uncertain about the current velocity while the car is moving, this + # usually means the model has trouble understanding the scene. This is used + # as a heuristic to warn the driver. + EventName.posenetInvalid: { + ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"), + ET.NO_ENTRY: posenet_invalid_alert, + }, + + # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we + # alert the driver the device might have fallen from the windshield. + EventName.deviceFalling: { + ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"), + ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), + }, + + EventName.lowMemory: { + ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"), + ET.PERMANENT: low_memory_alert, + ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), + }, + + EventName.highCpuUsage: { + #ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"), + #ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"), + ET.NO_ENTRY: high_cpu_usage_alert, + }, + + EventName.accFaulted: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Faulted"), + ET.PERMANENT: NormalPermanentAlert("Cruise Faulted", ""), + ET.NO_ENTRY: NoEntryAlert("Cruise Faulted"), + }, + + EventName.controlsMismatch: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), + ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), + }, + + EventName.roadCameraError: { + ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road", + duration=1., + creation_delay=30.), + }, + + EventName.wideRoadCameraError: { + ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road Fisheye", + duration=1., + creation_delay=30.), + }, + + EventName.driverCameraError: { + ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Driver", + duration=1., + creation_delay=30.), + }, + + # Sometimes the USB stack on the device can get into a bad state + # causing the connection to the panda to be lost + EventName.usbError: { + ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"), + ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""), + ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), + }, + + # This alert can be thrown for the following reasons: + # - No CAN data received at all + # - CAN data is received, but some message are not received at the right frequency + # If you're not writing a new car port, this is usually cause by faulty wiring + EventName.canError: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error"), + ET.PERMANENT: Alert( + "CAN Error: Check Connections", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + ET.NO_ENTRY: NoEntryAlert("CAN Error: Check Connections"), + }, + + EventName.canBusMissing: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"), + ET.PERMANENT: Alert( + "CAN Bus Disconnected: Likely Faulty Cable", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"), + }, + + EventName.steerUnavailable: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"), + ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"), + }, + + EventName.brakeUnavailable: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), + ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), + }, + + EventName.reverseGear: { + ET.PERMANENT: Alert( + "Reverse\nGear", + "", + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), + ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"), + ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), + }, + + # On cars that use stock ACC the car can decide to cancel ACC for various reasons. + # When this happens we can no long control the car so the user needs to be warned immediately. + EventName.cruiseDisabled: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"), + }, + + # For planning the trajectory Model Predictive Control (MPC) is used. This is + # an optimization algorithm that is not guaranteed to find a feasible solution. + # If no solution is found or the solution has a very high cost this alert is thrown. + EventName.plannerError: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Planner Solution Error"), + ET.NO_ENTRY: NoEntryAlert("Planner Solution Error"), + }, + + # When the relay in the harness box opens the CAN bus between the LKAS camera + # and the rest of the car is separated. When messages from the LKAS camera + # are received on the car side this usually means the relay hasn't opened correctly + # and this alert is thrown. + EventName.relayMalfunction: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"), + ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"), + ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"), + }, + + EventName.noTarget: { + ET.IMMEDIATE_DISABLE: Alert( + "openpilot Canceled", + "No close lead car", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), + ET.NO_ENTRY: NoEntryAlert("No Close Lead Car"), + }, + + EventName.speedTooLow: { + ET.IMMEDIATE_DISABLE: Alert( + "openpilot Canceled", + "Speed too low", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), + }, + + # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable. + EventName.speedTooHigh: { + ET.WARNING: Alert( + "Speed Too High", + "Model uncertain at this speed", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.), + ET.NO_ENTRY: NoEntryAlert("Slow down to engage"), + }, + + EventName.lowSpeedLockout: { + ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), + }, + + EventName.lkasDisabled: { + ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"), + ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), + }, + +} diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d69796738f35f1..78b59fda591f29 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,29 +1,31 @@ -import numpy as np from abc import abstractmethod, ABC +from common.numpy_fast import clip +from common.realtime import DT_CTRL + +MIN_STEER_SPEED = 0.3 + class LatControl(ABC): - def __init__(self, CP, CI, dt): - self.dt = dt + def __init__(self, CP, CI): + self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer - self.sat_time = 0. - self.sat_check_min_speed = 10. + self.sat_count = 0. # we define the steer torque scale as [-1.0...1.0] self.steer_max = 1.0 @abstractmethod - def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float): + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pass def reset(self): - self.sat_time = 0. + self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): - # Saturated only if control output is not being limited by car torque/angle rate limits - if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: - self.sat_time += self.dt + def _check_saturation(self, saturated, CS, steer_limited): + if saturated and CS.vEgo > 10. and not steer_limited and not CS.steeringPressed: + self.sat_count += self.sat_count_rate else: - self.sat_time -= self.dt - self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit) - return self.sat_time > (self.sat_limit - 1e-3) + self.sat_count -= self.sat_count_rate + self.sat_count = clip(self.sat_count, 0.0, self.sat_limit) + return self.sat_count > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 808c9a659aa5fd..0e5be4a97705ae 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,22 +1,16 @@ import math from cereal import log -from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED -# TODO This is speed dependent STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def __init__(self, CP, CI, dt): - super().__init__(CP, CI, dt) - self.sat_check_min_speed = 5. - self.use_steer_limited_by_safety = CP.brand == "tesla" - - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() - if not active: + if CS.vEgo < MIN_STEER_SPEED or not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) else: @@ -24,14 +18,8 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - if self.use_steer_limited_by_safety: - # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation - angle_control_saturated = steer_limited_by_safety - else: - # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota) - # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved - angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) + angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD + angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py new file mode 100644 index 00000000000000..2bc3cef76bf7b2 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -0,0 +1,120 @@ +import math +import numpy as np + +from cereal import log +from common.filter_simple import FirstOrderFilter +from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED + + +class LatControlINDI(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.angle_steers_des = 0. + + A = np.array([[1.0, DT_CTRL, 0.0], + [0.0, 1.0, DT_CTRL], + [0.0, 0.0, 1.0]]) + C = np.array([[1.0, 0.0, 0.0], + [0.0, 1.0, 0.0]]) + + # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) + # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) + + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # K = np.transpose(K) + K = np.array([[7.30262179e-01, 2.07003658e-04], + [7.29394177e+00, 1.39159419e-02], + [1.71022442e+01, 3.38495381e-02]]) + + self.speed = 0. + + self.K = K + self.A_K = A - np.dot(K, C) + self.x = np.array([[0.], [0.], [0.]]) + + self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) + self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) + self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) + self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) + + self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL) + self.reset() + + @property + def RC(self): + return interp(self.speed, self._RC[0], self._RC[1]) + + @property + def G(self): + return interp(self.speed, self._G[0], self._G[1]) + + @property + def outer_loop_gain(self): + return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1]) + + @property + def inner_loop_gain(self): + return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) + + def reset(self): + super().reset() + self.steer_filter.x = 0. + self.speed = 0. + + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): + self.speed = CS.vEgo + # Update Kalman filter + y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) + self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) + + indi_log = log.ControlsState.LateralINDIState.new_message() + indi_log.steeringAngleDeg = math.degrees(self.x[0]) + indi_log.steeringRateDeg = math.degrees(self.x[1]) + indi_log.steeringAccelDeg = math.degrees(self.x[2]) + + steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll) + steers_des += math.radians(params.angleOffsetDeg) + indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) + + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature + rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) + indi_log.steeringRateDesiredDeg = math.degrees(rate_des) + + if CS.vEgo < MIN_STEER_SPEED or not active: + indi_log.active = False + self.steer_filter.x = 0.0 + output_steer = 0 + else: + # Expected actuator value + self.steer_filter.update_alpha(self.RC) + self.steer_filter.update(last_actuators.steer) + + # Compute acceleration error + rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des + accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) + accel_error = accel_sp - self.x[2] + + # Compute change in actuator + g_inv = 1. / self.G + delta_u = g_inv * accel_error + + # If steering pressed, only allow wind down + if CS.steeringPressed and (delta_u * last_actuators.steer > 0): + delta_u = 0 + + output_steer = self.steer_filter.x + delta_u + + output_steer = clip(output_steer, -self.steer_max, self.steer_max) + + indi_log.active = True + indi_log.rateSetPoint = float(rate_sp) + indi_log.accelSetPoint = float(accel_sp) + indi_log.accelError = float(accel_error) + indi_log.delayedOutput = float(self.steer_filter.x) + indi_log.delta = float(delta_u) + indi_log.output = float(output_steer) + indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited) + + return float(output_steer), float(steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 14ab9f21b5580b..6bd678073e70aa 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,20 +1,23 @@ import math from cereal import log -from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.common.pid import PIDController +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController class LatControlPID(LatControl): - def __init__(self, CP, CI, dt): - super().__init__(CP, CI, dt) + def __init__(self, CP, CI): + super().__init__(CP, CI) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - pos_limit=self.steer_max, neg_limit=-self.steer_max) - self.ff_factor = CP.lateralTuning.pid.kf + k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def reset(self): + super().reset() + self.pid.reset() + + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -25,25 +28,21 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = error - if not active: - output_torque = 0.0 + if CS.vEgo < MIN_STEER_SPEED or not active: + output_steer = 0.0 pid_log.active = False - + self.pid.reset() else: # offset does not contribute to resistive torque - ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - - output_torque = self.pid.update(error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + output_steer = self.pid.update(error, override=CS.steeringPressed, + feedforward=steer_feedforward, speed=CS.vEgo) pid_log.active = True - pid_log.p = float(self.pid.p) - pid_log.i = float(self.pid.i) - pid_log.f = float(self.pid.f) - pid_log.output = float(output_torque) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.f = self.pid.f + pid_log.output = output_steer + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited) - return output_torque, angle_steers_des, pid_log + return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 903700d4b3ce06..c4604d90e1614d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,13 +1,11 @@ import math -import numpy as np -from collections import deque from cereal import log -from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction -from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.common.pid import PIDController +from common.numpy_fast import interp +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController +from selfdrive.controls.lib.drive_helpers import apply_deadzone +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -15,94 +13,74 @@ # wheel slip, or to speed. # This controller applies torque to achieve desired lateral -# accelerations. To compensate for the low speed effects the -# proportional gain is increased at low speeds by the PID controller. -# Additionally, there is friction in the steering wheel that needs -# to be overcome to move it at all, this is compensated for too. +# accelerations. To compensate for the low speed effects we +# use a LOW_SPEED_FACTOR in the error. Additionally, there is +# friction in the steering wheel that needs to be overcome to +# move it at all, this is compensated for too. -KP = 0.8 -KI = 0.15 -INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] -KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] +FRICTION_THRESHOLD = 0.2 -LP_FILTER_CUTOFF_HZ = 1.2 -JERK_LOOKAHEAD_SECONDS = 0.19 -JERK_GAIN = 0.3 -LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 1 class LatControlTorque(LatControl): - def __init__(self, CP, CI, dt): - super().__init__(CP, CI, dt) - self.torque_params = CP.lateralTuning.torque.as_builder() - self.torque_from_lateral_accel = CI.torque_from_lateral_accel() - self.lateral_accel_from_torque = CI.lateral_accel_from_torque() - self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt) - self.update_limits() - self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) - self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) - self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) - self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) - - def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): - self.torque_params.latAccelFactor = latAccelFactor - self.torque_params.latAccelOffset = latAccelOffset - self.torque_params.friction = friction - self.update_limits() - - def update_limits(self): - self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), - self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, + k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.get_steer_feedforward = CI.get_steer_feedforward_function() + self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle + self.friction = CP.lateralTuning.torque.friction + self.kf = CP.lateralTuning.torque.kf + self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg + + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() - pid_log.version = VERSION - measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - measurement = measured_curvature * CS.vEgo ** 2 - future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.lat_accel_request_buffer.append(future_desired_lateral_accel) - - 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)) - lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len)) - expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] - setpoint = expected_lateral_accel - error = setpoint - measurement - - lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2)) - raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt) - desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) - gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation - ff = gravity_adjusted_future_lateral_accel - # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll - ff -= self.torque_params.latAccelOffset - ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) - - if not active: + if CS.vEgo < MIN_STEER_SPEED or not active: output_torque = 0.0 pid_log.active = False else: - # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error) - - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) - output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) + if self.use_steering_angle: + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + else: + actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + curvature_deadzone = 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 + actual_lateral_accel = actual_curvature * CS.vEgo ** 2 + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 + + + low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200]) + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + measurement = actual_lateral_accel + low_speed_factor * actual_curvature + error = setpoint - measurement + pid_log.error = error + + ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY + # convert friction into lateral accel units for feedforward + friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction]) + ff += friction_compensation / self.kf + freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 + output_torque = self.pid.update(error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) pid_log.active = True - pid_log.p = float(self.pid.p) - pid_log.i = float(self.pid.i) - pid_log.d = float(self.pid.d) - pid_log.f = float(self.pid.f) - pid_log.output = float(-output_torque) # TODO: log lat accel? - pid_log.actualLateralAccel = float(measurement) - pid_log.desiredLateralAccel = float(setpoint) - pid_log.desiredLateralJerk = float(desired_lateral_jerk) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.d = self.pid.d + pid_log.f = self.pid.f + pid_log.output = -output_torque + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) + pid_log.actualLateralAccel = actual_lateral_accel + pid_log.desiredLateralAccel = desired_lateral_accel # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript index c9ebf892079976..df1e2a2a1ae6a6 100644 --- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version') +Import('env', 'envCython', 'arch', 'common') gen = "c_generated_code" @@ -32,59 +32,51 @@ generated_files = [ f'{gen}/Makefile', f'{gen}/main_lat.c', - f'{gen}/main_sim_lat.c', f'{gen}/acados_solver_lat.h', - f'{gen}/acados_sim_solver_lat.h', - f'{gen}/acados_sim_solver_lat.c', f'{gen}/acados_solver.pxd', f'{gen}/lat_model/lat_expl_vde_adj.c', f'{gen}/lat_model/lat_model.h', - f'{gen}/lat_constraints/lat_constraints.h', - f'{gen}/lat_cost/lat_cost.h', + f'{gen}/lat_cost/lat_cost_y_fun.h', + f'{gen}/lat_cost/lat_cost_y_e_fun.h', + f'{gen}/lat_cost/lat_cost_y_0_fun.h', ] + build_files acados_dir = '#third_party/acados' -acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' +acados_templates_dir = '#pyextra/acados_template/c_templates_tera' source_list = ['lat_mpc.py', - '#selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', + f'{acados_dir}/x86_64/lib/libacados.so', + f'{acados_dir}/larch64/lib/libacados.so', f'{acados_templates_dir}/acados_solver.in.c', ] lenv = env.Clone() -acados_rel_path = Dir(gen).rel_path(Dir(f"#third_party/acados/{arch}/lib")) -lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')] lenv.Clean(generated_files, Dir(gen)) -generated_lat = lenv.Command(generated_files, - source_list, - f"cd {Dir('.').abspath} && python3 lat_mpc.py") -lenv.Depends(generated_lat, [msgq_python, common_python]) +lenv.Command(generated_files, + source_list, + f"cd {Dir('.').abspath} && python3 lat_mpc.py") lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CCFLAGS"].append("-Wno-unused") if arch != "Darwin": lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags") -else: - lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_lat.dylib") - lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}") lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat", build_files, LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) # generate cython stuff -acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx") -acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd") +acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx") +acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd") libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd') libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c') lenv2 = envCython.Clone() -lenv2["LIBPATH"] += [lib_solver[0].dir.abspath] -lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')] +lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()] lenv2.Command(libacados_ocp_solver_c, [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd], f'cython' + \ @@ -92,6 +84,5 @@ lenv2.Command(libacados_ocp_solver_c, f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \ f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \ f' {acados_ocp_solver_pyx.get_labspath()}') -lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat']) +lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c]) lenv2.Depends(lib_cython, lib_solver) -lenv2.Depends(libacados_ocp_solver_c, np_version) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index ad60861088bec4..c0e7358160fd0c 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -1,28 +1,25 @@ #!/usr/bin/env python3 import os -import time import numpy as np from casadi import SX, vertcat, sin, cos -# WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import ModelConstants + +from common.realtime import sec_since_boot +from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N +from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code - from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: - from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython + from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json") X_DIM = 4 P_DIM = 2 -COST_E_DIM = 3 -COST_DIM = COST_E_DIM + 2 -SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' -N = 32 def gen_lat_model(): model = AcadosModel() @@ -32,8 +29,8 @@ def gen_lat_model(): x_ego = SX.sym('x_ego') y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') - psi_rate_ego = SX.sym('psi_rate_ego') - model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego) + curv_ego = SX.sym('curv_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) # parameters v_ego = SX.sym('v_ego') @@ -41,22 +38,22 @@ def gen_lat_model(): model.p = vertcat(v_ego, rotation_radius) # controls - psi_accel_ego = SX.sym('psi_accel_ego') - model.u = vertcat(psi_accel_ego) + curv_rate = SX.sym('curv_rate') + model.u = vertcat(curv_rate) # xdot x_ego_dot = SX.sym('x_ego_dot') y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') - psi_rate_ego_dot = SX.sym('psi_rate_ego_dot') + curv_ego_dot = SX.sym('curv_ego_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot) + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) # dynamics model - f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego, - v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego, - psi_rate_ego, - psi_accel_ego) + f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), + v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), + v_ego * curv_ego, + curv_rate) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -66,7 +63,7 @@ def gen_lat_ocp(): ocp = AcadosOcp() ocp.model = gen_lat_model() - Tf = np.array(ModelConstants.T_IDXS)[N] + Tf = np.array(T_IDXS)[N] # set dimensions ocp.dims.N = N @@ -75,35 +72,26 @@ def gen_lat_ocp(): ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' - Q = np.diag(np.zeros(COST_E_DIM)) - QR = np.diag(np.zeros(COST_DIM)) + Q = np.diag([0.0, 0.0]) + QR = np.diag([0.0, 0.0, 0.0]) ocp.cost.W = QR ocp.cost.W_e = Q - y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3] - psi_rate_ego_dot = ocp.model.u[0] + y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] + curv_rate = ocp.model.u[0] v_ego = ocp.model.p[0] ocp.parameter_values = np.zeros((P_DIM, )) - ocp.cost.yref = np.zeros((COST_DIM, )) - ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - # Add offset to smooth out low speed control - # TODO unclear if this right solution long term - v_ego_offset = v_ego + SPEED_OFFSET - # TODO there are two costs on psi_rate_ego_dot, one - # is correlated to jerk the other to steering wheel movement - # the steering wheel movement cost is added to prevent excessive - # wheel movements + ocp.cost.yref = np.zeros((3, )) + ocp.cost.yref_e = np.zeros((2, )) + # TODO hacky weights to keep behavior the same ocp.model.cost_y_expr = vertcat(y_ego, - v_ego_offset * psi_ego, - v_ego_offset * psi_rate_ego, - v_ego_offset * psi_rate_ego_dot, - psi_rate_ego_dot / (v_ego + 0.1)) + ((v_ego +5.0) * psi_ego), + ((v_ego + 5.0) * 4.0 * curv_rate)) ocp.model.cost_y_expr_e = vertcat(y_ego, - v_ego_offset * psi_ego, - v_ego_offset * psi_rate_ego) + ((v_ego +5.0) * psi_ego)) # set constraints ocp.constraints.constr_type = 'BGH' @@ -122,28 +110,24 @@ def gen_lat_ocp(): # set prediction horizon ocp.solver_options.tf = Tf - ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1] + ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1] ocp.code_export_directory = EXPORT_DIR return ocp -class LateralMpc: - def __init__(self, x0=None): - if x0 is None: - x0 = np.zeros(X_DIM) +class LateralMpc(): + def __init__(self, x0=np.zeros(X_DIM)): self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset(x0) - def reset(self, x0=None): - if x0 is None: - x0 = np.zeros(X_DIM) + def reset(self, x0=np.zeros(X_DIM)): self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N, 1)) - self.yref = np.zeros((N+1, COST_DIM)) + self.yref = np.zeros((N+1, 3)) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) + self.solver.cost_set(N, "yref", self.yref[N][:2]) # Somehow needed for stable init for i in range(N+1): @@ -156,35 +140,32 @@ def reset(self, x0=None): self.solve_time = 0.0 self.cost = 0 - def set_weights(self, path_weight, heading_weight, - lat_accel_weight, lat_jerk_weight, - steering_rate_weight): - W = np.asfortranarray(np.diag([path_weight, heading_weight, - lat_accel_weight, lat_jerk_weight, - steering_rate_weight])) + def set_weights(self, path_weight, heading_weight, steer_rate_weight): + W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight])) for i in range(N): self.solver.cost_set(i, 'W', W) - self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM]) + #TODO hacky weights to keep behavior the same + self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) - def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts): + def run(self, x0, p, y_pts, heading_pts, curv_rate_pts): x0_cp = np.copy(x0) p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts - v_ego = p_cp[0, 0] + v_ego = p_cp[0] # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) - self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) + self.yref[:,1] = heading_pts*(v_ego+5.0) + self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0 for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.set(i, "p", p_cp[i]) - self.solver.set(N, "p", p_cp[N]) - self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) + self.solver.set(i, "p", p_cp) + self.solver.set(N, "p", p_cp) + self.solver.cost_set(N, "yref", self.yref[N][:2]) - t = time.monotonic() + t = sec_since_boot() self.solution_status = self.solver.solve() - self.solve_time = time.monotonic() - t + self.solve_time = sec_since_boot() - t for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py new file mode 100644 index 00000000000000..3470754bc6bbb6 --- /dev/null +++ b/selfdrive/controls/lib/lateral_planner.py @@ -0,0 +1,120 @@ +import numpy as np +from common.realtime import sec_since_boot, DT_MDL +from common.numpy_fast import interp +from system.swaglog import cloudlog +from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc +from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N +from selfdrive.controls.lib.desire_helper import DesireHelper +import cereal.messaging as messaging +from cereal import log + +TRAJECTORY_SIZE = 33 +CAMERA_OFFSET = 0.04 + +class LateralPlanner: + def __init__(self, CP): + self.DH = DesireHelper() + + # Vehicle model parameters used to calculate lateral movement of car + self.factor1 = CP.wheelbase - CP.centerToFront + self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear) + self.last_cloudlog_t = 0 + self.solution_invalid_cnt = 0 + + self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) + self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3)) + self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) + self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,)) + self.t_idxs = np.arange(TRAJECTORY_SIZE) + self.y_pts = np.zeros(TRAJECTORY_SIZE) + + self.lat_mpc = LateralMpc() + self.reset_mpc(np.zeros(4)) + + def reset_mpc(self, x0=np.zeros(4)): + self.x0 = x0 + self.lat_mpc.reset(x0=self.x0) + + def update(self, sm): + v_ego = sm['carState'].vEgo + measured_curvature = sm['controlsState'].curvature + + # Parse model predictions + md = sm['modelV2'] + if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: + self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) + self.t_idxs = np.array(md.position.t) + self.plan_yaw = np.array(md.orientation.z) + if len(md.position.xStd) == TRAJECTORY_SIZE: + self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) + + # Lane change logic + desire_state = md.meta.desireState + if len(desire_state): + self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight] + lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob + self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) + + d_path_xyz = self.path_xyz + # Heading cost is useful at low speed, otherwise end of plan can be off-heading + heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) + + y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) + heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) + curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate) + self.y_pts = y_pts + + assert len(y_pts) == LAT_MPC_N + 1 + assert len(heading_pts) == LAT_MPC_N + 1 + assert len(curv_rate_pts) == LAT_MPC_N + 1 + lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) + p = np.array([v_ego, lateral_factor]) + self.lat_mpc.run(self.x0, + p, + y_pts, + heading_pts, + curv_rate_pts) + # init state for next + # mpc.u_sol is the desired curvature rate given x0 curv state. + # with x0[3] = measured_curvature, this would be the actual desired rate. + # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. + self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) + + # Check for infeasible MPC solution + mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any() + t = sec_since_boot() + if mpc_nans or self.lat_mpc.solution_status != 0: + self.reset_mpc() + self.x0[3] = measured_curvature + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Lateral mpc - nan: True") + + if self.lat_mpc.cost > 20000. or mpc_nans: + self.solution_invalid_cnt += 1 + else: + self.solution_invalid_cnt = 0 + + def publish(self, sm, pm): + plan_solution_valid = self.solution_invalid_cnt < 2 + plan_send = messaging.new_message('lateralPlan') + plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) + + lateralPlan = plan_send.lateralPlan + lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] + lateralPlan.dPathPoints = self.y_pts.tolist() + lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() + lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist() + lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] + + lateralPlan.mpcSolutionValid = bool(plan_solution_valid) + lateralPlan.solverExecutionTime = self.lat_mpc.solve_time + + lateralPlan.desire = self.DH.desire + lateralPlan.useLaneLines = False + lateralPlan.laneChangeState = self.DH.lane_change_state + lateralPlan.laneChangeDirection = self.DH.lane_change_direction + + pm.send('lateralPlan', plan_send) diff --git a/selfdrive/controls/lib/ldw.py b/selfdrive/controls/lib/ldw.py deleted file mode 100644 index 78a6d6cf6eee49..00000000000000 --- a/selfdrive/controls/lib/ldw.py +++ /dev/null @@ -1,41 +0,0 @@ -from cereal import log -from openpilot.common.realtime import DT_CTRL -from openpilot.common.constants import CV - - -CAMERA_OFFSET = 0.04 -LDW_MIN_SPEED = 31 * CV.MPH_TO_MS -LANE_DEPARTURE_THRESHOLD = 0.1 - -class LaneDepartureWarning: - def __init__(self): - self.left = False - self.right = False - self.last_blinker_frame = 0 - - def update(self, frame, modelV2, CS, CC): - if CS.leftBlinker or CS.rightBlinker: - self.last_blinker_frame = frame - - recent_blinker = (frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown - ldw_allowed = CS.vEgo > LDW_MIN_SPEED and not recent_blinker and not CC.latActive - - desire_prediction = modelV2.meta.desirePrediction - if len(desire_prediction) and ldw_allowed: - right_lane_visible = modelV2.laneLineProbs[2] > 0.5 - left_lane_visible = modelV2.laneLineProbs[1] > 0.5 - l_lane_change_prob = desire_prediction[log.Desire.laneChangeLeft] - r_lane_change_prob = desire_prediction[log.Desire.laneChangeRight] - - lane_lines = modelV2.laneLines - l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET)) - r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET)) - - self.left = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) - self.right = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) - else: - self.left, self.right = False, False - - @property - def warning(self) -> bool: - return bool(self.left or self.right) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 62dbc842c54ef2..f72995d4141e3e 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,19 +1,28 @@ -import numpy as np from cereal import car -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.common.pid import PIDController -from openpilot.selfdrive.modeld.constants import ModelConstants - -CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL +from selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone +from selfdrive.controls.lib.pid import PIDController +from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState - -def long_control_state_trans(CP, active, long_control_state, v_ego, - should_stop, brake_pressed, cruise_standstill): - stopping_condition = should_stop - starting_condition = (not should_stop and +# As per ISO 15622:2018 for all speeds +ACCEL_MIN_ISO = -3.5 # m/s^2 +ACCEL_MAX_ISO = 2.0 # m/s^2 + +def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, + v_target_1sec, brake_pressed, cruise_standstill): + accelerating = v_target_1sec > v_target + planned_stop = (v_target < CP.vEgoStopping and + v_target_1sec < CP.vEgoStopping and + not accelerating) + stay_stopped = (v_ego < CP.vEgoStopping and + (brake_pressed or cruise_standstill)) + stopping_condition = planned_stop or stay_stopped + + starting_condition = (v_target_1sec > CP.vEgoStarting and + accelerating and not cruise_standstill and not brake_pressed) started_condition = v_ego > CP.vEgoStarting @@ -23,13 +32,11 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, else: if long_control_state == LongCtrlState.off: - if not starting_condition: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.pid: + if stopping_condition: long_control_state = LongCtrlState.stopping - else: - if starting_condition and CP.startingState: - long_control_state = LongCtrlState.starting - else: - long_control_state = LongCtrlState.pid elif long_control_state == LongCtrlState.stopping: if starting_condition and CP.startingState: @@ -37,52 +44,95 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, elif starting_condition: long_control_state = LongCtrlState.pid - elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]: + elif long_control_state == LongCtrlState.starting: if stopping_condition: long_control_state = LongCtrlState.stopping elif started_condition: long_control_state = LongCtrlState.pid + + + + + return long_control_state + class LongControl: def __init__(self, CP): self.CP = CP - self.long_control_state = LongCtrlState.off + self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - rate=1 / DT_CTRL) + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + self.v_pid = 0.0 self.last_output_accel = 0.0 - def reset(self): + def reset(self, v_pid): + """Reset PID controller and change setpoint""" self.pid.reset() + self.v_pid = v_pid - def update(self, active, CS, a_target, should_stop, accel_limits): + def update(self, active, CS, long_plan, accel_limits, t_since_plan): """Update longitudinal control. This updates the state machine and runs a PID loop""" + # Interp control trajectory + speeds = long_plan.speeds + if len(speeds) == CONTROL_N: + v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) + + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now + + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now + + v_target = min(v_target_lower, v_target_upper) + a_target = min(a_target_lower, a_target_upper) + + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds) + else: + v_target = 0.0 + v_target_now = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] + output_accel = self.last_output_accel self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, - should_stop, CS.brakePressed, + v_target, v_target_1sec, CS.brakePressed, CS.cruiseState.standstill) + if self.long_control_state == LongCtrlState.off: - self.reset() + self.reset(CS.vEgo) output_accel = 0. elif self.long_control_state == LongCtrlState.stopping: - output_accel = self.last_output_accel if output_accel > self.CP.stopAccel: - output_accel = min(output_accel, 0.0) output_accel -= self.CP.stoppingDecelRate * DT_CTRL - self.reset() + self.reset(CS.vEgo) elif self.long_control_state == LongCtrlState.starting: output_accel = self.CP.startAccel - self.reset() + self.reset(CS.vEgo) + + elif self.long_control_state == LongCtrlState.pid: + self.v_pid = v_target_now + + # Toyota starts braking more when it thinks you want to stop + # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration + # TODO too complex, needs to be simplified and tested on toyotas + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) + freeze_integrator = prevent_overshoot + + error = self.v_pid - CS.vEgo + error_deadzone = apply_deadzone(error, deadzone) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, + feedforward=a_target, + freeze_integrator=freeze_integrator) - else: # LongCtrlState.pid - error = a_target - CS.aEgo - output_accel = self.pid.update(error, speed=CS.vEgo, - feedforward=a_target) + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 7a6c02a538d728..5a9e69c29781e8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version') +Import('env', 'envCython', 'arch', 'common') gen = "c_generated_code" @@ -38,58 +38,52 @@ generated_files = [ f'{gen}/Makefile', f'{gen}/main_long.c', - f'{gen}/main_sim_long.c', f'{gen}/acados_solver_long.h', - f'{gen}/acados_sim_solver_long.h', - f'{gen}/acados_sim_solver_long.c', f'{gen}/acados_solver.pxd', f'{gen}/long_model/long_expl_vde_adj.c', f'{gen}/long_model/long_model.h', - f'{gen}/long_constraints/long_constraints.h', - f'{gen}/long_cost/long_cost.h', + f'{gen}/long_constraints/long_h_constraint.h', + f'{gen}/long_cost/long_cost_y_fun.h', + f'{gen}/long_cost/long_cost_y_e_fun.h', + f'{gen}/long_cost/long_cost_y_0_fun.h', ] + build_files acados_dir = '#third_party/acados' -acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' +acados_templates_dir = '#pyextra/acados_template/c_templates_tera' source_list = ['long_mpc.py', - '#selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', + f'{acados_dir}/x86_64/lib/libacados.so', + f'{acados_dir}/larch64/lib/libacados.so', f'{acados_templates_dir}/acados_solver.in.c', ] lenv = env.Clone() -acados_rel_path = Dir(gen).rel_path(Dir(f"#third_party/acados/{arch}/lib")) -lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')] lenv.Clean(generated_files, Dir(gen)) -generated_long = lenv.Command(generated_files, - source_list, - f"cd {Dir('.').abspath} && python3 long_mpc.py") -lenv.Depends(generated_long, [msgq_python, common_python]) + +lenv.Command(generated_files, + source_list, + f"cd {Dir('.').abspath} && python3 long_mpc.py") lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CCFLAGS"].append("-Wno-unused") if arch != "Darwin": lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags") -else: - lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_long.dylib") - lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}") lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long", build_files, LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) # generate cython stuff -acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx") -acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd") +acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx") +acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd") libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd') libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c') lenv2 = envCython.Clone() -lenv2["LIBPATH"] += [lib_solver[0].dir.abspath] -lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')] +lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()] lenv2.Command(libacados_ocp_solver_c, [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd], f'cython' + \ @@ -97,6 +91,5 @@ lenv2.Command(libacados_ocp_solver_c, f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \ f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \ f' {acados_ocp_solver_pyx.get_labspath()}') -lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long']) +lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c]) lenv2.Depends(lib_cython, lib_solver) -lenv2.Depends(libacados_ocp_solver_c, np_version) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py old mode 100755 new mode 100644 index 9408132c5b8025..ea9b0683a4c76c --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -1,19 +1,17 @@ #!/usr/bin/env python3 import os -import time import numpy as np -from cereal import log -from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX -from openpilot.common.realtime import DT_MDL -from openpilot.common.swaglog import cloudlog -# WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import index_function -from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU + +from common.realtime import sec_since_boot +from common.numpy_fast import clip +from system.swaglog import cloudlog +from selfdrive.modeld.constants import index_function +from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code - from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: - from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython + from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error from casadi import SX, vertcat @@ -35,14 +33,15 @@ X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 5. +J_EGO_COST = 5.0 A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. -CRASH_DISTANCE = .25 +CRASH_DISTANCE = .5 LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' + # Fewer timestamps don't hurt performance and lead to # much better convergence of the MPC with low iterations N = 12 @@ -50,47 +49,31 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)] T_IDXS = np.array(T_IDXS_LST) -FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) +MIN_ACCEL = -3.5 +MAX_ACCEL = 2.0 +T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -CRUISE_MIN_ACCEL = -1.2 -CRUISE_MAX_ACCEL = 1.6 -MIN_X_LEAD_FACTOR = 0.5 - -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 - else: - raise NotImplementedError("Longitudinal personality not supported") - - -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 - else: - raise NotImplementedError("Longitudinal personality not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) -def get_safe_obstacle_distance(v_ego, t_follow): +def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE +def desired_follow_distance(v_ego, v_lead): + return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) + + def gen_long_model(): model = AcadosModel() model.name = MODEL_NAME - # states - x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego') + # set up states & controls + x_ego = SX.sym('x_ego') + v_ego = SX.sym('v_ego') + a_ego = SX.sym('a_ego') model.x = vertcat(x_ego, v_ego, a_ego) # controls @@ -118,6 +101,7 @@ def gen_long_model(): model.f_expl_expr = f_expl return model + def gen_long_ocp(): ocp = AcadosOcp() ocp.model = gen_long_model() @@ -175,8 +159,7 @@ def gen_long_ocp(): x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR]) - + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -213,31 +196,29 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, dt=DT_MDL): - self.dt = dt + def __init__(self, mode='acc'): + self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] def reset(self): + # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.solver.reset() - - self.x_sol = np.zeros((N+1, X_DIM)) - self.u_sol = np.zeros((N, 1)) + # self.solver.options_set('print_level', 2) self.v_solution = np.zeros(N+1) self.a_solution = np.zeros(N+1) - self.j_solution = np.zeros(N) self.prev_a = np.array(self.a_solution) + self.j_solution = np.zeros(N) self.yref = np.zeros((N+1, COST_DIM)) - for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - + self.x_sol = np.zeros((N+1, X_DIM)) + self.u_sol = np.zeros((N,1)) self.params = np.zeros((N+1, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) - self.last_cloudlog_t = 0 self.status = False self.crash_cnt = 0.0 @@ -266,11 +247,19 @@ def set_cost_weights(self, cost_weights, constraint_cost_weights): for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + def set_weights(self, prev_accel_constraint=True): + if self.mode == 'acc': + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + elif self.mode == 'blended': + cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] + elif self.mode == 'e2e': + cost_weights = [0., 0.2, 0.25, 1., 0.0, .1] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) def set_cur_state(self, v, a): @@ -278,7 +267,7 @@ def set_cur_state(self, v, a): self.x0[1] = v self.x0[2] = a if abs(v_prev - v) > 2.: # probably only helps if v < v_prev - for i in range(N+1): + for i in range(0, N+1): self.solver.set(i, 'x', self.x0) @staticmethod @@ -305,15 +294,18 @@ def process_lead(self, lead): # MPC will not converge if immediate crash is expected # Clip lead distance to what is still possible to brake for - min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2) - x_lead = np.clip(x_lead, min_x_lead, 1e8) - v_lead = np.clip(v_lead, 0.0, 1e8) - a_lead = np.clip(a_lead, -10., 5.) + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2) + x_lead = clip(x_lead, min_x_lead, 1e8) + v_lead = clip(v_lead, 0.0, 1e8) + a_lead = clip(a_lead, -10., 5.) lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def set_accel_limits(self, min_a, max_a): + self.cruise_min_a = min_a + self.cruise_max_a = max_a + + def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -326,37 +318,99 @@ def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.s lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) - # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) + # Update in ACC mode or ACC/e2e blend + if self.mode == 'acc': + self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a + self.params[:,1] = self.cruise_max_a + self.params[:,5] = LEAD_DANGER_FACTOR + + # Fake an obstacle for cruise, this ensures smooth acceleration to set speed + # when the leads are no factor. + v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) + v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05) + v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), + v_lower, + v_upper) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + self.source = SOURCES[np.argmin(x_obstacles[0])] + + # These are not used in ACC mode + x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 + + elif self.mode == 'blended': + self.params[:,0] = MIN_ACCEL + self.params[:,1] = MAX_ACCEL + self.params[:,5] = 1.0 + + x_obstacles = np.column_stack([lead_0_obstacle, + lead_1_obstacle]) + cruise_target = T_IDXS * v_cruise + x[0] + xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) + x = np.cumsum(np.insert(xforward, 0, x[0])) + + x_and_cruise = np.column_stack([x, cruise_target]) + x = np.min(x_and_cruise, axis=1) + + self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise' - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) - self.source = SOURCES[np.argmin(x_obstacles[0])] + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') - self.yref[:,:] = 0.0 + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a + self.yref[:,5] = j for i in range(N): self.solver.set(i, "yref", self.yref[i]) self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) - self.params[:,0] = ACCEL_MIN - self.params[:,1] = ACCEL_MAX self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) - self.params[:,4] = t_follow - self.params[:,5] = LEAD_DANGER_FACTOR + self.params[:,4] = T_FOLLOW self.run() - if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and + if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and radarstate.leadOne.modelProb > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 + # Check if it got within lead comfort range + # TODO This should be done cleaner + if self.mode == 'blended': + if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0): + self.source = 'lead0' + if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], T_FOLLOW))- self.x_sol[:,0] < 0.0) and \ + (lead_1_obstacle[0] - lead_0_obstacle[0]): + self.source = 'lead1' + + + + def update_with_xva(self, x, v, a): + self.params[:,0] = -10. + self.params[:,1] = 10. + self.params[:,2] = 1e5 + self.params[:,4] = T_FOLLOW + self.params[:,5] = LEAD_DANGER_FACTOR + + # v, and a are in local frame, but x is wrt the x[0] position + # In >90degree turns, x goes to 0 (and may even be -ve) + # So, we use integral(v) + x[0] to obtain the forward-distance + xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) + x = np.cumsum(np.insert(xforward, 0, x[0])) + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a + for i in range(N): + self.solver.cost_set(i, "yref", self.yref[i]) + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) + self.params[:,3] = np.copy(self.prev_a) + self.run() + def run(self): + # t0 = sec_since_boot() + # reset = 0 for i in range(N+1): self.solver.set(i, 'p', self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) @@ -368,6 +422,12 @@ def run(self): self.time_linearization = float(self.solver.get_stats('time_lin')[0]) self.time_integrator = float(self.solver.get_stats('time_sim')[0]) + # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific + # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}") + # res = self.solver.get_residuals() + # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}") + # self.solver.print_statistics() + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): @@ -377,16 +437,19 @@ def run(self): self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] - self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution) + self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) - t = time.monotonic() + t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}") self.reset() + # reset = 1 + # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(sec_since_boot() - t0):.2e} qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}") if __name__ == "__main__": ocp = gen_long_ocp() AcadosOcpSolver.generate(ocp, json_file=JSON_FILE) + # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c5c03eba18028e..b9b16e34fcefa4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,44 +1,44 @@ #!/usr/bin/env python3 import math import numpy as np +from common.numpy_fast import interp import cereal.messaging as messaging -from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX -from openpilot.common.constants import CV -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, SOURCES -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan -from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET -from openpilot.common.swaglog import cloudlog - -A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] -A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] -CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] -ALLOW_THROTTLE_THRESHOLD = 0.4 -MIN_ALLOW_THROTTLE_SPEED = 2.5 +from common.conversions import Conversions as CV +from common.filter_simple import FirstOrderFilter +from common.params import Params +from common.realtime import DT_MDL +from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.longcontrol import LongCtrlState +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N +from system.swaglog import cloudlog + +LON_MPC_STEP = 0.2 # first step is 0.2s +AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted +A_CRUISE_MIN = -1.2 +A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6] +A_CRUISE_MAX_BP = [0., 15., 25., 40.] # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] + def get_max_accel(v_ego): - return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) + return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) -def get_coast_accel(pitch): - return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration this should avoid accelerating when losing the target in turns """ + # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel # The lookup table for turns should also be updated if we do this - a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) @@ -46,135 +46,112 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: - def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): + def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc(dt=dt) + self.params = Params() + self.param_read_counter = 0 + + self.mpc = LongitudinalMpc() + self.read_param() + self.fcw = False - self.dt = dt - self.allow_throttle = True self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) - self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] - self.output_a_target = 0.0 - self.output_should_stop = False + self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) + self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) - - @staticmethod - def parse_model(model_msg): - if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) - j = np.zeros(len(T_IDXS_MPC)) + self.solverExecutionTime = 0.0 + + def read_param(self): + e2e = self.params.get_bool('EndToEndLong') and self.CP.openpilotLongitudinalControl + self.mpc.mode = 'blended' if e2e else 'acc' + + def parse_model(self, model_msg): + if (len(model_msg.position.x) == 33 and + len(model_msg.velocity.x) == 33 and + len(model_msg.acceleration.x) == 33): + x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) + v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) + a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) + # Uniform interp so gradient is less noisy + a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x) + j_sparse = np.gradient(a_sparse, self.t_uniform) + j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse) else: x = np.zeros(len(T_IDXS_MPC)) v = np.zeros(len(T_IDXS_MPC)) a = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC)) - if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: - throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] - else: - throttle_prob = 1.0 - return x, v, a, j, throttle_prob + return x, v, a, j def update(self, sm): - if len(sm['carControl'].orientationNED) == 3: - accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) - else: - accel_coast = ACCEL_MAX + if self.param_read_counter % 50 == 0: + self.read_param() + self.param_read_counter += 1 v_ego = sm['carState'].vEgo - v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) + v_cruise_kph = sm['controlsState'].vCruise + v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS - v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed - reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled - # PCM cruise speed may be updated a few cycles later, check if initialized - reset_state = reset_state or not v_cruise_initialized + reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) - if reset_state: self.v_desired_filter.x = v_ego - # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) + self.a_desired = 0.0 # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - _, _, _, _, throttle_prob = self.parse_model(sm['modelV2']) - # Don't clip at low speeds since throttle_prob doesn't account for creep - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - - if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_clip[0]) - 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) + accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: - v_cruise = 0.0 - - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) + # if required so, force a smooth deceleration + accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) + accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) + # clip limits, cannot init MPC outside of bounds + accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) + accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + + self.mpc.set_weights(prev_accel_constraint) + self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + x, v, a, j = self.parse_model(sm['modelV2']) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j) - self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) - self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) - self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) + self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) + self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill + # TODO write fcw in e2e_long mode + self.fcw = self.mpc.mode == 'acc' and self.mpc.crash_cnt > 5 if self.fcw: cloudlog.info("FCW triggered") # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) - self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - - action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - - if sm['selfdriveState'].experimentalMode: - output_a_target = min(output_a_target_e2e, output_a_target_mpc) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - if output_a_target < output_a_target_mpc: - self.mpc.source = SOURCES[3] - else: - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc - - for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) - self.prev_accel_clip = accel_clip + self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) + self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') - plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'radarState']) + plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] - longitudinalPlan.solverExecutionTime = self.mpc.solve_time longitudinalPlan.speeds = self.v_desired_trajectory.tolist() longitudinalPlan.accels = self.a_desired_trajectory.tolist() @@ -184,9 +161,6 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - longitudinalPlan.aTarget = float(self.output_a_target) - longitudinalPlan.shouldStop = bool(self.output_should_stop) - longitudinalPlan.allowBrake = True - longitudinalPlan.allowThrottle = bool(self.allow_throttle) + longitudinalPlan.solverExecutionTime = self.mpc.solve_time pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py new file mode 100644 index 00000000000000..965158131b2073 --- /dev/null +++ b/selfdrive/controls/lib/pid.py @@ -0,0 +1,75 @@ +import numpy as np +from numbers import Number + +from common.numpy_fast import clip, interp + + +class PIDController(): + def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + self._k_p = k_p + self._k_i = k_i + self._k_d = k_d + self.k_f = k_f # feedforward gain + if isinstance(self._k_p, Number): + self._k_p = [[0], [self._k_p]] + if isinstance(self._k_i, Number): + self._k_i = [[0], [self._k_i]] + if isinstance(self._k_d, Number): + self._k_d = [[0], [self._k_d]] + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.speed = 0.0 + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + @property + def k_d(self): + return interp(self.speed, self._k_d[0], self._k_d[1]) + + @property + def error_integral(self): + return self.i/self.k_i + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.d = 0.0 + self.f = 0.0 + self.control = 0 + + def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): + self.speed = speed + + self.p = float(error) * self.k_p + self.f = feedforward * self.k_f + self.d = error_rate * self.k_d + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + i + self.d + self.f + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + + control = self.p + self.i + self.d + self.f + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py new file mode 100644 index 00000000000000..0e2b96668f6518 --- /dev/null +++ b/selfdrive/controls/lib/radar_helpers.py @@ -0,0 +1,158 @@ +from common.numpy_fast import mean +from common.kalman.simple_kalman import KF1D + + +# the longer lead decels, the more likely it will keep decelerating +# TODO is this a good default? +_LEAD_ACCEL_TAU = 1.5 + +# radar tracks +SPEED, ACCEL = 0, 1 # Kalman filter states enum + +# stationary qualification parameters +v_ego_stationary = 4. # no stationary object flag below this speed + +RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car +RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame + +class Track(): + def __init__(self, v_lead, kalman_params): + self.cnt = 0 + self.aLeadTau = _LEAD_ACCEL_TAU + self.K_A = kalman_params.A + self.K_C = kalman_params.C + self.K_K = kalman_params.K + self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) + + def update(self, d_rel, y_rel, v_rel, v_lead, measured): + # relative values, copy + self.dRel = d_rel # LONG_DIST + self.yRel = y_rel # -LAT_DIST + self.vRel = v_rel # REL_SPEED + self.vLead = v_lead + self.measured = measured # measured or estimate + + # computed velocity and accelerations + if self.cnt > 0: + self.kf.update(self.vLead) + + self.vLeadK = float(self.kf.x[SPEED][0]) + self.aLeadK = float(self.kf.x[ACCEL][0]) + + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau = _LEAD_ACCEL_TAU + else: + self.aLeadTau *= 0.9 + + self.cnt += 1 + + def get_key_for_cluster(self): + # Weigh y higher since radar is inaccurate in this dimension + return [self.dRel, self.yRel*2, self.vRel] + + def reset_a_lead(self, aLeadK, aLeadTau): + self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) + self.aLeadK = aLeadK + self.aLeadTau = aLeadTau + + +class Cluster(): + def __init__(self): + self.tracks = set() + + def add(self, t): + # add the first track + self.tracks.add(t) + + # TODO: make generic + @property + def dRel(self): + return mean([t.dRel for t in self.tracks]) + + @property + def yRel(self): + return mean([t.yRel for t in self.tracks]) + + @property + def vRel(self): + return mean([t.vRel for t in self.tracks]) + + @property + def aRel(self): + return mean([t.aRel for t in self.tracks]) + + @property + def vLead(self): + return mean([t.vLead for t in self.tracks]) + + @property + def dPath(self): + return mean([t.dPath for t in self.tracks]) + + @property + def vLat(self): + return mean([t.vLat for t in self.tracks]) + + @property + def vLeadK(self): + return mean([t.vLeadK for t in self.tracks]) + + @property + def aLeadK(self): + if all(t.cnt <= 1 for t in self.tracks): + return 0. + else: + return mean([t.aLeadK for t in self.tracks if t.cnt > 1]) + + @property + def aLeadTau(self): + if all(t.cnt <= 1 for t in self.tracks): + return _LEAD_ACCEL_TAU + else: + return mean([t.aLeadTau for t in self.tracks if t.cnt > 1]) + + @property + def measured(self): + return any(t.measured for t in self.tracks) + + def get_RadarState(self, model_prob=0.0): + return { + "dRel": float(self.dRel), + "yRel": float(self.yRel), + "vRel": float(self.vRel), + "vLead": float(self.vLead), + "vLeadK": float(self.vLeadK), + "aLeadK": float(self.aLeadK), + "status": True, + "fcw": self.is_potential_fcw(model_prob), + "modelProb": model_prob, + "radar": True, + "aLeadTau": float(self.aLeadTau) + } + + def get_RadarState_from_vision(self, lead_msg, v_ego): + return { + "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.y[0]), + "vRel": float(lead_msg.v[0] - v_ego), + "vLead": float(lead_msg.v[0]), + "vLeadK": float(lead_msg.v[0]), + "aLeadK": float(0), + "aLeadTau": _LEAD_ACCEL_TAU, + "fcw": False, + "modelProb": float(lead_msg.prob), + "radar": False, + "status": True + } + + def __str__(self): + ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" + return ret + + def potential_low_speed_lead(self, v_ego): + # stop for stuff in front of you and low speed, even without model confirmation + return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and self.dRel < 25 + + def is_potential_fcw(self, model_prob): + return model_prob > .9 diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py new file mode 100755 index 00000000000000..6c1b6fc4a2e3bc --- /dev/null +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import random +import unittest + +from selfdrive.controls.lib.events import Alert, EVENTS +from selfdrive.controls.lib.alertmanager import AlertManager + + +class TestAlertManager(unittest.TestCase): + + def test_duration(self): + """ + Enforce that an alert lasts for max(alert duration, duration the alert is added) + """ + for duration in range(1, 100): + alert = None + while not isinstance(alert, Alert): + event = random.choice([e for e in EVENTS.values() if len(e)]) + alert = random.choice(list(event.values())) + + alert.duration = duration + + # check two cases: + # - alert is added to AM for <= the alert's duration + # - alert is added to AM for > alert's duration + for greater in (True, False): + if greater: + add_duration = duration + random.randint(1, 10) + else: + add_duration = random.randint(1, duration) + show_duration = max(duration, add_duration) + + AM = AlertManager() + for frame in range(duration+10): + if frame < add_duration: + AM.add_many(frame, [alert, ]) + current_alert = AM.process_alerts(frame, {}) + + shown = current_alert is not None + should_show = frame <= show_duration + self.assertEqual(shown, should_show, msg=f"{frame=} {add_duration=} {duration=}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py new file mode 100755 index 00000000000000..f15ab2fa5662bf --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import unittest + +from parameterized import parameterized + +from cereal import car, log +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.honda.values import CAR as HONDA +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.nissan.values import CAR as NISSAN +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.vehicle_model import VehicleModel + + +class TestLatControl(unittest.TestCase): + + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + def test_saturation(self, car_name, controller): + CarInterface, CarController, CarState = interfaces[car_name] + CP = CarInterface.get_params(car_name) + CI = CarInterface(CP, CarController, CarState) + VM = VehicleModel(CP) + + controller = controller(CP, CI) + + CS = car.CarState.new_message() + CS.vEgo = 30 + + last_actuators = car.CarControl.Actuators.new_message() + + params = log.LiveParametersData.new_message() + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, True, 1, 0) + + self.assertTrue(lac_log.saturated) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/lib/tests/test_vehicle_model.py b/selfdrive/controls/lib/tests/test_vehicle_model.py new file mode 100755 index 00000000000000..3e08cb0aa08d30 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_vehicle_model.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +import math +import unittest + +import numpy as np +from control import StateSpace + +from selfdrive.car.honda.interface import CarInterface +from selfdrive.car.honda.values import CAR +from selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices + + +class TestVehicleModel(unittest.TestCase): + def setUp(self): + CP = CarInterface.get_params(CAR.CIVIC) + self.VM = VehicleModel(CP) + + def test_round_trip_yaw_rate(self): + # TODO: fix VM to work at zero speed + for u in np.linspace(1, 30, num=10): + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + yr = self.VM.yaw_rate(sa, u, roll) + new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll) + + self.assertAlmostEqual(sa, new_sa) + + def test_dyn_ss_sol_against_yaw_rate(self): + """Verify that the yaw_rate helper function matches the results + from the state space model.""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + + # Compute yaw rate based on state space model + _, yr1 = dyn_ss_sol(sa, u, roll, self.VM) + + # Compute yaw rate using direct computations + yr2 = self.VM.yaw_rate(sa, u, roll) + self.assertAlmostEqual(float(yr1), yr2) + + def test_syn_ss_sol_simulate(self): + """Verifies that dyn_ss_sol matches a simulation""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + A, B = create_dyn_state_matrices(u, self.VM) + + # Convert to discrete time system + ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2))) + ss = ss.sample(0.01) + + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + inp = np.array([[sa], [roll]]) + + # Simulate for 1 second + x1 = np.zeros((2, 1)) + for _ in range(100): + x1 = ss.A @ x1 + ss.B @ inp + + # Compute steady state solution directly + x2 = dyn_ss_sol(sa, u, roll, self.VM) + + np.testing.assert_almost_equal(x1, x2, decimal=3) + + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py new file mode 100755 index 00000000000000..0750384918dc31 --- /dev/null +++ b/selfdrive/controls/lib/vehicle_model.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +""" +Dynamic bicycle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" + +The state is x = [v, r]^T +with v lateral speed [m/s], and r rotational speed [rad/s] + +The input u is the steering angle [rad], and roll [rad] + +The system is defined by +x_dot = A*x + B*u + +A depends on longitudinal speed, u [m/s], and vehicle parameters CP +""" +from typing import Tuple + +import numpy as np +from numpy.linalg import solve + +from cereal import car + +ACCELERATION_DUE_TO_GRAVITY = 9.8 + + +class VehicleModel: + def __init__(self, CP: car.CarParams): + """ + Args: + CP: Car Parameters + """ + # for math readability, convert long names car params into short names + self.m: float = CP.mass + self.j: float = CP.rotationalInertia + self.l: float = CP.wheelbase + self.aF: float = CP.centerToFront + self.aR: float = CP.wheelbase - CP.centerToFront + self.chi: float = CP.steerRatioRear + + self.cF_orig: float = CP.tireStiffnessFront + self.cR_orig: float = CP.tireStiffnessRear + self.update_params(1.0, CP.steerRatio) + + def update_params(self, stiffness_factor: float, steer_ratio: float) -> None: + """Update the vehicle model with a new stiffness factor and steer ratio""" + self.cF: float = stiffness_factor * self.cF_orig + self.cR: float = stiffness_factor * self.cR_orig + self.sR: float = steer_ratio + + def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray: + """Returns the steady state solution. + + If the speed is too low we can't use the dynamic model (tire slip is undefined), + we then have to use the kinematic model + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + 2x1 matrix with steady state solution (lateral speed, rotational speed) + """ + if u > 0.1: + return dyn_ss_sol(sa, u, roll, self) + else: + return kin_ss_sol(sa, u, self) + + def calc_curvature(self, sa: float, u: float, roll: float) -> float: + """Returns the curvature. Multiplied by the speed this will give the yaw rate. + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Curvature factor [1/m] + """ + return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u) + + def curvature_factor(self, u: float) -> float: + """Returns the curvature factor. + Multiplied by wheel angle (not steering wheel angle) this will give the curvature. + + Args: + u: Speed [m/s] + + Returns: + Curvature factor [1/m] + """ + sf = calc_slip_factor(self) + return (1. - self.chi) / (1. - sf * u**2) / self.l + + def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float: + """Calculates the required steering wheel angle for a given curvature + + Args: + curv: Desired curvature [1/m] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Steering wheel angle [rad] + """ + + return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u) + + def roll_compensation(self, roll: float, u: float) -> float: + """Calculates the roll-compensation to curvature + + Args: + roll: Road Roll [rad] + u: Speed [m/s] + + Returns: + Roll compensation curvature [rad] + """ + sf = calc_slip_factor(self) + + if abs(sf) < 1e-6: + return 0 + else: + return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2) + + def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float: + """Calculates the required steering wheel angle for a given yaw_rate + + Args: + yaw_rate: Desired yaw rate [rad/s] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Steering wheel angle [rad] + """ + curv = yaw_rate / u + return self.get_steer_from_curvature(curv, u, roll) + + def yaw_rate(self, sa: float, u: float, roll: float) -> float: + """Calculate yaw rate + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Yaw rate [rad/s] + """ + return self.calc_curvature(sa, u, roll) * u + + +def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray: + """Calculate the steady state solution at low speeds + At low speeds the tire slip is undefined, so a kinematic + model is used. + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + K = np.zeros((2, 1)) + K[0, 0] = VM.aR / VM.sR / VM.l * u + K[1, 0] = 1. / VM.sR / VM.l * u + return K * sa + + +def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, np.ndarray]: + """Returns the A and B matrix for the dynamics system + + Args: + u: Vehicle speed [m/s] + VM: Vehicle model + + Returns: + A tuple with the 2x2 A matrix, and 2x2 B matrix + + Parameters in the vehicle model: + cF: Tire stiffness Front [N/rad] + cR: Tire stiffness Front [N/rad] + aF: Distance from CG to front wheels [m] + aR: Distance from CG to rear wheels [m] + m: Mass [kg] + j: Rotational inertia [kg m^2] + sR: Steering ratio [-] + chi: Steer ratio rear [-] + """ + A = np.zeros((2, 2)) + B = np.zeros((2, 2)) + A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u) + A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u + A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u) + A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u) + + # Steering input + B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR + B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR + + # Roll input + B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY + + return A, B + + +def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray: + """Calculate the steady state solution when x_dot = 0, + Ax + Bu = 0 => x = -A^{-1} B u + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + A, B = create_dyn_state_matrices(u, VM) + inp = np.array([[sa], [roll]]) + return -solve(A, B) @ inp # type: ignore + + +def calc_slip_factor(VM: VehicleModel) -> float: + """The slip factor is a measure of how the curvature changes with speed + it's positive for Oversteering vehicle, negative (usual case) otherwise. + """ + return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index bec7eede0b56c9..93d0c80daccb88 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,39 +1,43 @@ #!/usr/bin/env python3 from cereal import car -from openpilot.common.params import Params -from openpilot.common.realtime import Priority, config_realtime_process -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning -from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from common.params import Params +from common.realtime import Priority, config_realtime_process +from system.swaglog import cloudlog +from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging -def main(): +def plannerd_thread(sm=None, pm=None): config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") params = Params() - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - cloudlog.info("plannerd got CarParams: %s", CP.brand) + CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) + cloudlog.info("plannerd got CarParams: %s", CP.carName) - ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], - poll='modelV2') + lateral_planner = LateralPlanner(CP) + + if sm is None: + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], + poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) + + if pm is None: + pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan']) while True: sm.update() + if sm.updated['modelV2']: + lateral_planner.update(sm) + lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) - msg = messaging.new_message('driverAssistance') - msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters']) - msg.driverAssistance.leftLaneDeparture = ldw.left - msg.driverAssistance.rightLaneDeparture = ldw.right - pm.send('driverAssistance', msg) + +def main(sm=None, pm=None): + plannerd_thread(sm, pm) if __name__ == "__main__": diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 98fce1cb26e7b5..3d958139d6413c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,33 +1,20 @@ #!/usr/bin/env python3 +import importlib import math -import numpy as np -from collections import deque -from typing import Any +from collections import defaultdict, deque -import capnp -from cereal import messaging, log, car -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process -from openpilot.common.swaglog import cloudlog -from openpilot.common.simple_kalman import KF1D +import cereal.messaging as messaging +from cereal import car +from common.numpy_fast import interp +from common.params import Params +from common.realtime import Ratekeeper, Priority, config_realtime_process +from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid +from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA +from system.swaglog import cloudlog -# Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 - -# radar tracks -SPEED, ACCEL = 0, 1 # Kalman filter states enum - -# stationary qualification parameters -V_EGO_STATIONARY = 4. # no stationary object flag below this speed - -RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car -RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame - - -class KalmanParams: - def __init__(self, dt: float): +class KalmanParams(): + def __init__(self, dt): # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" @@ -36,7 +23,7 @@ def __init__(self, dt: float): #Q = np.matrix([[10., 0.0], [0.0, 100.]]) #R = 1e3 #K = np.matrix([[ 0.05705578], [ 0.03073241]]) - dts = [i * 0.01 for i in range(1, 21)] + dts = [dt * 0.01 for dt in range(1, 21)] K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, @@ -45,169 +32,88 @@ def __init__(self, dt: float): 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, 0.26393339, 0.26278425] - self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]] - - -class Track: - def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams): - self.identifier = identifier - self.cnt = 0 - self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL) - self.K_A = kalman_params.A - self.K_C = kalman_params.C - self.K_K = kalman_params.K - self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) - - def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): - # relative values, copy - self.dRel = d_rel # LONG_DIST - self.yRel = y_rel # -LAT_DIST - self.vRel = v_rel # REL_SPEED - self.vLead = v_lead - self.measured = measured # measured or estimate - - # computed velocity and accelerations - if self.cnt > 0: - self.kf.update(self.vLead) - - self.vLeadK = float(self.kf.x[SPEED][0]) - self.aLeadK = float(self.kf.x[ACCEL][0]) - - # Learn if constant acceleration - if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU - else: - self.aLeadTau.update(0.0) - - self.cnt += 1 - - def get_RadarState(self, model_prob: float = 0.0): - return { - "dRel": float(self.dRel), - "yRel": float(self.yRel), - "vRel": float(self.vRel), - "vLead": float(self.vLead), - "vLeadK": float(self.vLeadK), - "aLeadK": float(self.aLeadK), - "aLeadTau": float(self.aLeadTau.x), - "status": True, - "fcw": self.is_potential_fcw(model_prob), - "modelProb": model_prob, - "radar": True, - "radarTrackId": self.identifier, - } - - def potential_low_speed_lead(self, v_ego: float): - # stop for stuff in front of you and low speed, even without model confirmation - # Radar points closer than 0.75, are almost always glitches on toyota radars - return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25) - - def is_potential_fcw(self, model_prob: float): - return model_prob > .9 - - def __str__(self): - ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" - return ret - - -def laplacian_pdf(x: float, mu: float, b: float): + self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] + + +def laplacian_cdf(x, mu, b): b = max(b, 1e-4) return math.exp(-abs(x-mu)/b) -def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): +def match_vision_to_cluster(v_ego, lead, clusters): + # match vision point to best statistical cluster match offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - # This isn't exactly right, but it's a good heuristic + # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v - track = max(tracks.values(), key=prob) + cluster = max(clusters, key=prob) # if no 'sane' match is found return -1 # stationary radar points can be false positives - dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3) + dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) if dist_sane and vel_sane: - return track + return cluster else: return None -def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego - return { - "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), - "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), - "aLeadTau": 0.3, - "fcw": False, - "modelProb": float(lead_msg.prob), - "status": True, - "radar": False, - "radarTrackId": -1, - } - - -def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]: +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > .5: - track = match_vision_to_track(v_ego, lead_msg, tracks) + if len(clusters) > 0 and ready and lead_msg.prob > .5: + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) else: - track = None + cluster = None lead_dict = {'status': False} - if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) - elif (track is None) and ready and (lead_msg.prob > .5): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + if cluster is not None: + lead_dict = cluster.get_RadarState(lead_msg.prob) + elif (cluster is None) and ready and (lead_msg.prob > .5): + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) if low_speed_override: - low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] - if len(low_speed_tracks) > 0: - closest_track = min(low_speed_tracks, key=lambda c: c.dRel) + low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] + if len(low_speed_clusters) > 0: + closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) - # Only choose new track if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): - lead_dict = closest_track.get_RadarState() + # Only choose new cluster if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): + lead_dict = closest_cluster.get_RadarState() return lead_dict -class RadarD: - def __init__(self, delay: float = 0.0): - self.current_time = 0.0 - - self.tracks: dict[int, Track] = {} - self.kalman_params = KalmanParams(DT_MDL) +class RadarD(): + def __init__(self, radar_ts, delay=0): + self.current_time = 0 - self.v_ego = 0.0 - self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) - self.last_v_ego_frame = -1 + self.tracks = defaultdict(dict) + self.kalman_params = KalmanParams(radar_ts) - self.radar_state: capnp._DynamicStructBuilder | None = None - self.radar_state_valid = False + # v_ego + self.v_ego = 0. + self.v_ego_hist = deque([0], maxlen=delay+1) self.ready = False - def update(self, sm: messaging.SubMaster, rr: car.RadarData): - self.ready = sm.seen['modelV2'] + def update(self, sm, rr): self.current_time = 1e-9*max(sm.logMonoTime.values()) - if sm.recv_frame['carState'] != self.last_v_ego_frame: + if sm.updated['carState']: self.v_ego = sm['carState'].vEgo self.v_ego_hist.append(self.v_ego) - self.last_v_ego_frame = sm.recv_frame['carState'] + if sm.updated['modelV2']: + self.ready = True - ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points} + ar_pts = {} + for pt in rr.points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): @@ -223,54 +129,111 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): # create the track if it doesn't exist or it's a new track if ids not in self.tracks: - self.tracks[ids] = Track(ids, v_lead, self.kalman_params) + self.tracks[ids] = Track(v_lead, self.kalman_params) self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) - # *** publish radarState *** - self.radar_state_valid = sm.all_checks() - self.radar_state = log.RadarState.new_message() - self.radar_state.mdMonoTime = sm.logMonoTime['modelV2'] - self.radar_state.radarErrors = rr.errors - self.radar_state.carStateMonoTime = sm.logMonoTime['carState'] - - if len(sm['modelV2'].velocity.x): - model_v_ego = sm['modelV2'].velocity.x[0] + idens = list(sorted(self.tracks.keys())) + track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] + + # If we have multiple points, cluster them + if len(track_pts) > 1: + cluster_idxs = cluster_points_centroid(track_pts, 2.5) + clusters = [None] * (max(cluster_idxs) + 1) + + for idx in range(len(track_pts)): + cluster_i = cluster_idxs[idx] + if clusters[cluster_i] is None: + clusters[cluster_i] = Cluster() + clusters[cluster_i].add(self.tracks[idens[idx]]) + elif len(track_pts) == 1: + # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 + cluster_idxs = [0] + clusters = [Cluster()] + clusters[0].add(self.tracks[idens[0]]) else: - model_v_ego = self.v_ego - leads_v3 = sm['modelV2'].leadsV3 - if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + clusters = [] + + # if a new point, reset accel to the rest of the cluster + for idx in range(len(track_pts)): + if self.tracks[idens[idx]].cnt <= 1: + aLeadK = clusters[cluster_idxs[idx]].aLeadK + aLeadTau = clusters[cluster_idxs[idx]].aLeadTau + self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) - def publish(self, pm: messaging.PubMaster): - assert self.radar_state is not None + # *** publish radarState *** + dat = messaging.new_message('radarState') + dat.valid = sm.all_checks() and len(rr.errors) == 0 + radarState = dat.radarState + radarState.mdMonoTime = sm.logMonoTime['modelV2'] + radarState.canMonoTimes = list(rr.canMonoTimes) + radarState.radarErrors = list(rr.errors) + radarState.carStateMonoTime = sm.logMonoTime['carState'] - radar_msg = messaging.new_message("radarState") - radar_msg.valid = self.radar_state_valid - radar_msg.radarState = self.radar_state - pm.send("radarState", radar_msg) + leads_v3 = sm['modelV2'].leadsV3 + if len(leads_v3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + return dat # fuses camera and radar data for best lead detection -def main() -> None: +def radard_thread(sm=None, pm=None, can_sock=None): config_realtime_process(5, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") - CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams) + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("radard got CarParams") + # import the radar from the fingerprint + cloudlog.info("radard is importing %s", CP.carName) + RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface + # *** setup messaging - sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') - pm = messaging.PubMaster(['radarState']) + if can_sock is None: + can_sock = messaging.sub_sock('can') + if sm is None: + sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing + if pm is None: + pm = messaging.PubMaster(['radarState', 'liveTracks']) - RD = RadarD(CP.radarDelay) + RI = RadarInterface(CP) + + rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) + RD = RadarD(CP.radarTimeStep, RI.delay) while 1: - sm.update() + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) + + if rr is None: + continue + + sm.update(0) + + dat = RD.update(sm, rr) + dat.radarState.cumLagMs = -rk.remaining*1000. + + pm.send('radarState', dat) + + # *** publish tracks for UI debugging (keep last) *** + tracks = RD.tracks + dat = messaging.new_message('liveTracks', len(tracks)) + + for cnt, ids in enumerate(sorted(tracks.keys())): + dat.liveTracks[cnt] = { + "trackId": ids, + "dRel": float(tracks[ids].dRel), + "yRel": float(tracks[ids].yRel), + "vRel": float(tracks[ids].vRel), + } + pm.send('liveTracks', dat) + + rk.monitor_time() + - RD.update(sm, sm['liveTracks']) - RD.publish(pm) +def main(sm=None, pm=None, can_sock=None): + radard_thread(sm, pm, can_sock) if __name__ == "__main__": diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py new file mode 100755 index 00000000000000..9ed7eee1228826 --- /dev/null +++ b/selfdrive/controls/tests/test_alerts.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +import json +import os +import unittest +import random +from PIL import Image, ImageDraw, ImageFont + +from cereal import log, car +from common.basedir import BASEDIR +from common.params import Params +from selfdrive.controls.lib.events import Alert, EVENTS, ET +from selfdrive.controls.lib.alertmanager import set_offroad_alert +from selfdrive.test.process_replay.process_replay import FakeSubMaster, CONFIGS + +AlertSize = log.ControlsState.AlertSize + +OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json") + +# TODO: add callback alerts +ALERTS = [] +for event_types in EVENTS.values(): + for alert in event_types.values(): + ALERTS.append(alert) + + +class TestAlerts(unittest.TestCase): + + @classmethod + def setUpClass(cls): + with open(OFFROAD_ALERTS_PATH) as f: + cls.offroad_alerts = json.loads(f.read()) + + # Create fake objects for callback + cls.CS = car.CarState.new_message() + cls.CP = car.CarParams.new_message() + cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0] + cls.sm = FakeSubMaster(cfg.pub_sub.keys()) + + def test_events_defined(self): + # Ensure all events in capnp schema are defined in events.py + events = car.CarEvent.EventName.schema.enumerants + + for name, e in events.items(): + if not name.endswith("DEPRECATED"): + fail_msg = "%s @%d not in EVENTS" % (name, e) + self.assertTrue(e in EVENTS.keys(), msg=fail_msg) + + # ensure alert text doesn't exceed allowed width + def test_alert_text_length(self): + font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts") + regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") + bold_font_path = os.path.join(font_path, "Inter-Bold.ttf") + semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") + + max_text_width = 2160 - 300 # full screen width is usable, minus sidebar + draw = ImageDraw.Draw(Image.new('RGB', (0, 0))) + + fonts = { + AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)], + AlertSize.mid: [ImageFont.truetype(bold_font_path, 88), + ImageFont.truetype(regular_font_path, 66)], + } + + for alert in ALERTS: + if not isinstance(alert, Alert): + alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100) + + # for full size alerts, both text fields wrap the text, + # so it's unlikely that they would go past the max width + if alert.alert_size in (AlertSize.none, AlertSize.full): + continue + + for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): + if i >= len(fonts[alert.alert_size]): + break + + font = fonts[alert.alert_size][i] + w, _ = draw.textsize(txt, font) + msg = f"type: {alert.alert_type} msg: {txt}" + self.assertLessEqual(w, max_text_width, msg=msg) + + def test_alert_sanity_check(self): + for event_types in EVENTS.values(): + for event_type, a in event_types.items(): + # TODO: add callback alerts + if not isinstance(a, Alert): + continue + + if a.alert_size == AlertSize.none: + self.assertEqual(len(a.alert_text_1), 0) + self.assertEqual(len(a.alert_text_2), 0) + elif a.alert_size == AlertSize.small: + self.assertGreater(len(a.alert_text_1), 0) + self.assertEqual(len(a.alert_text_2), 0) + elif a.alert_size == AlertSize.mid: + self.assertGreater(len(a.alert_text_1), 0) + self.assertGreater(len(a.alert_text_2), 0) + else: + self.assertGreater(len(a.alert_text_1), 0) + + self.assertGreaterEqual(a.duration, 0.) + + if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE): + self.assertEqual(a.creation_delay, 0.) + + def test_offroad_alerts(self): + params = Params() + for a in self.offroad_alerts: + # set the alert + alert = self.offroad_alerts[a] + set_offroad_alert(a, True) + self.assertTrue(json.dumps(alert) == params.get(a, encoding='utf8')) + + # then delete it + set_offroad_alert(a, False) + self.assertTrue(params.get(a) is None) + + def test_offroad_alerts_extra_text(self): + params = Params() + for i in range(50): + # set the alert + a = random.choice(list(self.offroad_alerts)) + alert = self.offroad_alerts[a] + set_offroad_alert(a, True, extra_text="a"*i) + + expected_txt = alert['text'] + "a"*i + written_txt = json.loads(params.get(a, encoding='utf8'))['text'] + self.assertTrue(expected_txt == written_txt) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_clustering.py b/selfdrive/controls/tests/test_clustering.py new file mode 100644 index 00000000000000..290b2670299910 --- /dev/null +++ b/selfdrive/controls/tests/test_clustering.py @@ -0,0 +1,140 @@ +# pylint: skip-file + +import time +import unittest +import numpy as np +from fastcluster import linkage_vector +from scipy.cluster import _hierarchy +from scipy.spatial.distance import pdist + +from selfdrive.controls.lib.cluster.fastcluster_py import hclust, ffi +from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid + + +def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None): + # supersimplified function to get fast clustering. Got it from scipy + Z = np.asarray(Z, order='c') + n = Z.shape[0] + 1 + T = np.zeros((n,), dtype='i') + _hierarchy.cluster_dist(Z, T, float(t), int(n)) + return T + + +TRACK_PTS = np.array([[59.26000137, -9.35999966, -5.42500019], + [91.61999817, -0.31999999, -2.75], + [31.38000031, 0.40000001, -0.2], + [89.57999725, -8.07999992, -18.04999924], + [53.42000122, 0.63999999, -0.175], + [31.38000031, 0.47999999, -0.2], + [36.33999939, 0.16, -0.2], + [53.33999939, 0.95999998, -0.175], + [59.26000137, -9.76000023, -5.44999981], + [33.93999977, 0.40000001, -0.22499999], + [106.74000092, -5.76000023, -18.04999924]]) + +CORRECT_LINK = np.array([[2., 5., 0.07999998, 2.], + [4., 7., 0.32984889, 2.], + [0., 8., 0.40078104, 2.], + [6., 9., 2.41209933, 2.], + [11., 14., 3.76342275, 4.], + [12., 13., 13.02297651, 4.], + [1., 3., 17.27626057, 2.], + [10., 17., 17.92918845, 3.], + [15., 16., 23.68525366, 8.], + [18., 19., 52.52351319, 11.]]) + +CORRECT_LABELS = np.array([7, 1, 4, 2, 6, 4, 5, 6, 7, 5, 3], dtype=np.int32) + + +def plot_cluster(pts, idx_old, idx_new): + import matplotlib.pyplot as plt + m = 'Set1' + + plt.figure() + plt.subplot(1, 2, 1) + plt.scatter(pts[:, 0], pts[:, 1], c=idx_old, cmap=m) + plt.title("Old") + plt.colorbar() + plt.subplot(1, 2, 2) + plt.scatter(pts[:, 0], pts[:, 1], c=idx_new, cmap=m) + plt.title("New") + plt.colorbar() + + plt.show() + + +def same_clusters(correct, other): + correct = np.asarray(correct) + other = np.asarray(other) + if len(correct) != len(other): + return False + + for i in range(len(correct)): + c = np.where(correct == correct[i]) + o = np.where(other == other[i]) + if not np.array_equal(c, o): + return False + return True + + +class TestClustering(unittest.TestCase): + def test_scipy_clustering(self): + old_link = linkage_vector(TRACK_PTS, method='centroid') + old_cluster_idxs = fcluster(old_link, 2.5, criterion='distance') + + np.testing.assert_allclose(old_link, CORRECT_LINK) + np.testing.assert_allclose(old_cluster_idxs, CORRECT_LABELS) + + def test_pdist(self): + pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64) + pts_ptr = ffi.cast("double *", pts.ctypes.data) + + n, m = pts.shape + out = np.zeros((n * (n - 1) // 2, ), dtype=np.float64) + out_ptr = ffi.cast("double *", out.ctypes.data) + hclust.hclust_pdist(n, m, pts_ptr, out_ptr) + + np.testing.assert_allclose(out, np.power(pdist(TRACK_PTS), 2)) + + def test_cpp_clustering(self): + pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64) + pts_ptr = ffi.cast("double *", pts.ctypes.data) + n, m = pts.shape + + labels = np.zeros((n, ), dtype=np.int32) + labels_ptr = ffi.cast("int *", labels.ctypes.data) + hclust.cluster_points_centroid(n, m, pts_ptr, 2.5**2, labels_ptr) + self.assertTrue(same_clusters(CORRECT_LABELS, labels)) + + def test_cpp_wrapper_clustering(self): + labels = cluster_points_centroid(TRACK_PTS, 2.5) + self.assertTrue(same_clusters(CORRECT_LABELS, labels)) + + def test_random_cluster(self): + np.random.seed(1337) + N = 1000 + + t_old = 0. + t_new = 0. + + for _ in range(N): + n = int(np.random.uniform(2, 32)) + x = np.random.uniform(-10, 50, (n, 1)) + y = np.random.uniform(-5, 5, (n, 1)) + vrel = np.random.uniform(-5, 5, (n, 1)) + pts = np.hstack([x, y, vrel]) + + t = time.time() + old_link = linkage_vector(pts, method='centroid') + old_cluster_idx = fcluster(old_link, 2.5, criterion='distance') + t_old += time.time() - t + + t = time.time() + cluster_idx = cluster_points_centroid(pts, 2.5) + t_new += time.time() - t + + self.assertTrue(same_clusters(old_cluster_idx, cluster_idx)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py new file mode 100644 index 00000000000000..a972bfb0734f75 --- /dev/null +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from common.params import Params + + +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + +def run_cruise_simulation(cruise, t_end=20.): + man = Maneuver( + '', + duration=t_end, + initial_speed=max(cruise - 1., 0.0), + lead_relevancy=True, + initial_distance_lead=100, + cruise_values=[cruise], + prob_lead_values=[0.0], + breakpoints=[0.], + ) + valid, output = man.evaluate() + assert valid + return output[-1,3] + + +class TestCruiseSpeed(unittest.TestCase): + def test_cruise_speed(self): + params = Params() + for e2e in [False, True]: + params.put_bool("EndToEndLong", e2e) + for speed in np.arange(5, 40, 5): + print(f'Testing {speed} m/s') + cruise_speed = float(speed) + + simulation_steady_state = run_cruise_simulation(cruise_speed) + self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s') + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 8f66d89bf871d8..3534f58235e2d4 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -1,19 +1,13 @@ -import pytest -import itertools -from parameterized import parameterized_class +#!/usr/bin/env python3 +import unittest +import numpy as np +from common.params import Params -from cereal import log -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW -from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver - -def desired_follow_distance(v_ego, v_lead, t_follow=None): - if t_follow is None: - t_follow = get_T_FOLLOW() - return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) - -def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0): +def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): man = Maneuver( '', duration=t_end, @@ -22,24 +16,26 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit initial_distance_lead=100, speed_lead_values=[v_lead], breakpoints=[0.], - e2e=e2e, - personality=personality, + e2e=e2e ) valid, output = man.evaluate() assert valid return output[-1,2] - output[-1,1] -@parameterized_class(("e2e", "personality", "speed"), itertools.product( - [True, False], # e2e - [log.LongitudinalPersonality.relaxed, # personality - log.LongitudinalPersonality.standard, - log.LongitudinalPersonality.aggressive], - [0,10,35])) # speed -class TestFollowingDistance: +class TestFollowingDistance(unittest.TestCase): def test_following_distance(self): - v_lead = float(self.speed) - simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality) - correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) - err_ratio = 0.2 if self.e2e else 0.1 - assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + .5) + params = Params() + for e2e in [False, True]: + params.put_bool("EndToEndLong", e2e) + for speed in np.arange(0, 40, 5): + print(f'Testing {speed} m/s') + v_lead = float(speed) + simulation_steady_state = run_following_distance_simulation(v_lead) + correct_steady_state = desired_follow_distance(v_lead, v_lead) + err_ratio = 0.2 if e2e else 0.1 + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py deleted file mode 100644 index 354c7f00add64c..00000000000000 --- a/selfdrive/controls/tests/test_latcontrol.py +++ /dev/null @@ -1,45 +0,0 @@ -from parameterized import parameterized - -from cereal import car, log -from opendbc.car.car_helpers import interfaces -from opendbc.car.honda.values import CAR as HONDA -from opendbc.car.toyota.values import CAR as TOYOTA -from opendbc.car.nissan.values import CAR as NISSAN -from opendbc.car.gm.values import CAR as GM -from opendbc.car.vehicle_model import VehicleModel -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID -from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque -from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle - - -class TestLatControl: - - @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), - (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)]) - def test_saturation(self, car_name, controller): - CarInterface = interfaces[car_name] - CP = CarInterface.get_non_essential_params(car_name) - CI = CarInterface(CP) - VM = VehicleModel(CP) - - controller = controller(CP.as_reader(), CI, DT_CTRL) - - CS = car.CarState.new_message() - CS.vEgo = 30 - CS.steeringPressed = False - - params = log.LiveParametersData.new_message() - - # Saturate for curvature limited and controller limited - for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True, 0.2) - assert lac_log.saturated - - for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False, 0.2) - assert not lac_log.saturated - - for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0.2) - assert lac_log.saturated diff --git a/selfdrive/controls/tests/test_latcontrol_torque_buffer.py b/selfdrive/controls/tests/test_latcontrol_torque_buffer.py deleted file mode 100644 index 76d0c28423c97d..00000000000000 --- a/selfdrive/controls/tests/test_latcontrol_torque_buffer.py +++ /dev/null @@ -1,36 +0,0 @@ -from parameterized import parameterized - -from cereal import car, log -from opendbc.car.car_helpers import interfaces -from opendbc.car.toyota.values import CAR as TOYOTA -from opendbc.car.vehicle_model import VehicleModel -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS - -def get_controller(car_name): - CarInterface = interfaces[car_name] - CP = CarInterface.get_non_essential_params(car_name) - CI = CarInterface(CP) - VM = VehicleModel(CP) - controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) - return controller, VM - -class TestLatControlTorqueBuffer: - - @parameterized.expand([(TOYOTA.TOYOTA_COROLLA_TSS2,)]) - def test_request_buffer_consistency(self, car_name): - buffer_steps = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / DT_CTRL) - controller, VM = get_controller(car_name) - - CS = car.CarState.new_message() - CS.vEgo = 30 - CS.steeringPressed = False - params = log.LiveParametersData.new_message() - - for _ in range(buffer_steps): - controller.update(True, CS, VM, params, False, 0.001, False, 0.2) - assert all(val != 0 for val in controller.lat_accel_request_buffer) - - for _ in range(buffer_steps): - controller.update(False, CS, VM, params, False, 0.0, False, 0.2) - assert all(val == 0 for val in controller.lat_accel_request_buffer) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 3aa0fd1bce4804..4864dbdc068328 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -1,8 +1,7 @@ -import pytest +import unittest import numpy as np -from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc -from openpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS -from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N +from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc +from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., @@ -10,15 +9,14 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur if lat_mpc is None: lat_mpc = LateralMpc() - lat_mpc.set_weights(1., .1, 0.0, .05, 800) + lat_mpc.set_weights(1., 1., 1.) y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) curv_rate_pts = np.zeros(LAT_MPC_N + 1) x0 = np.array([x_init, y_init, psi_init, curvature_init]) - p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1), - CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)]) + p = np.array([v_ref, CAR_ROTATION_RADIUS]) # converge in no more than 10 iterations for _ in range(10): @@ -27,20 +25,20 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur return lat_mpc.x_sol -class TestLateralMpc: +class TestLateralMpc(unittest.TestCase): def _assert_null(self, sol, curvature=1e-6): for i in range(len(sol)): - assert sol[0,i,1] == pytest.approx(0, abs=curvature) - assert sol[0,i,2] == pytest.approx(0, abs=curvature) - assert sol[0,i,3] == pytest.approx(0, abs=curvature) + self.assertAlmostEqual(sol[0,i,1], 0., delta=curvature) + self.assertAlmostEqual(sol[0,i,2], 0., delta=curvature) + self.assertAlmostEqual(sol[0,i,3], 0., delta=curvature) def _assert_simmetry(self, sol, curvature=1e-6): for i in range(len(sol)): - assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature) - assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature) - assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature) - assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature) + self.assertAlmostEqual(sol[0,i,1], -sol[1,i,1], delta=curvature) + self.assertAlmostEqual(sol[0,i,2], -sol[1,i,2], delta=curvature) + self.assertAlmostEqual(sol[0,i,3], -sol[1,i,3], delta=curvature) + self.assertAlmostEqual(sol[0,i,0], sol[1,i,0], delta=curvature) def test_straight(self): sol = run_mpc() @@ -74,12 +72,16 @@ def test_no_overshoot(self): y_init = 1. sol = run_mpc(y_init=y_init) for y in list(sol[:,1]): - assert y_init >= abs(y) + self.assertGreaterEqual(y_init, abs(y)) def test_switch_convergence(self): lat_mpc = LateralMpc() - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0) + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0) right_psi_deg = np.degrees(sol[:,2]) - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0) + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0) left_psi_deg = np.degrees(sol[:,2]) np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py deleted file mode 100644 index 77384fea20ffad..00000000000000 --- a/selfdrive/controls/tests/test_leads.py +++ /dev/null @@ -1,31 +0,0 @@ -import cereal.messaging as messaging - -from opendbc.car.toyota.values import CAR as TOYOTA -from openpilot.selfdrive.test.process_replay import replay_process_with_name - - -class TestLeads: - def test_radar_fault(self): - # if there's no radar-related can traffic, radard should either not respond or respond with an error - # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check - def single_iter_pkg(): - # single iter package, with meaningless cans and empty carState/modelV2 - msgs = [] - for _ in range(500): - can = messaging.new_message("can", 1) - cs = messaging.new_message("carState") - cp = messaging.new_message("carParams") - msgs.append(can.as_reader()) - msgs.append(cs.as_reader()) - msgs.append(cp.as_reader()) - model = messaging.new_message("modelV2") - msgs.append(model.as_reader()) - - return msgs - - msgs = [m for _ in range(3) for m in single_iter_pkg()] - out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2) - states = [m for m in out if m.which() == "liveTracks"] - failures = [not state.valid for state in states] - - assert len(states) == 0 or all(failures) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py deleted file mode 100644 index ab50810d894f61..00000000000000 --- a/selfdrive/controls/tests/test_longcontrol.py +++ /dev/null @@ -1,56 +0,0 @@ -from cereal import car -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans - - - - -class TestLongControlStateTransition: - - def test_stay_stopped(self): - CP = car.CarParams.new_message() - active = True - current_state = LongCtrlState.stopping - next_state = long_control_state_trans(CP, 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, - 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, - 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, - 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, - should_stop=False, brake_pressed=False, cruise_standstill=False) - assert next_state == LongCtrlState.off - -def test_engage(): - CP = car.CarParams.new_message() - active = True - current_state = LongCtrlState.off - next_state = long_control_state_trans(CP, 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, - 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, - 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, - 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) - active = True - current_state = LongCtrlState.starting - next_state = long_control_state_trans(CP, 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, - should_stop=False, brake_pressed=False, cruise_standstill=False) - assert next_state == LongCtrlState.pid diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py new file mode 100755 index 00000000000000..63af4c7d95c08e --- /dev/null +++ b/selfdrive/controls/tests/test_startup.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +import time +import unittest +from parameterized import parameterized + +from cereal import log, car +import cereal.messaging as messaging +from common.params import Params +from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp # pylint: disable=no-name-in-module,import-error +from selfdrive.car.fingerprints import _FINGERPRINTS +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.mazda.values import CAR as MAZDA +from selfdrive.controls.lib.events import EVENT_NAME +from selfdrive.test.helpers import with_processes + +EventName = car.CarEvent.EventName +Ecu = car.CarParams.Ecu + +COROLLA_FW_VERSIONS = [ + (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'), + (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'), + (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'), + (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'), +] +COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')] +COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1] + +CX5_FW_VERSIONS = [ + (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), +] + +class TestStartup(unittest.TestCase): + + @parameterized.expand([ + # TODO: test EventName.startup for release branches + + # officially supported car + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), + + # dashcamOnly car + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), + + # unrecognized car with no fw + (EventName.startupNoFw, None, None, ""), + (EventName.startupNoFw, None, None, ""), + + # unrecognized car + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + + # fuzzy match + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), + ]) + @with_processes(['controlsd']) + def test_startup_alert(self, expected_event, car_model, fw_versions, brand): + + # TODO: this should be done without any real sockets + controls_sock = messaging.sub_sock("controlsState") + pm = messaging.PubMaster(['can', 'pandaStates']) + + params = Params() + params.clear_all() + params.put_bool("Passive", False) + params.put_bool("OpenpilotEnabledToggle", True) + + # Build capnn version of FW array + if fw_versions is not None: + car_fw = [] + cp = car.CarParams.new_message() + for ecu, addr, subaddress, version in fw_versions: + f = car.CarParams.CarFw.new_message() + f.ecu = ecu + f.address = addr + f.fwVersion = version + f.brand = brand + + if subaddress is not None: + f.subAddress = subaddress + + car_fw.append(f) + cp.carVin = "1" * 17 + cp.carFw = car_fw + params.put("CarParamsCache", cp.to_bytes()) + + time.sleep(2) # wait for controlsd to be ready + + msg = messaging.new_message('pandaStates', 1) + msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno + pm.send('pandaStates', msg) + + # fingerprint + if (car_model is None) or (fw_versions is not None): + finger = {addr: 1 for addr in range(1, 100)} + else: + finger = _FINGERPRINTS[car_model][0] + + for _ in range(1000): + msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()] + pm.send('can', can_list_to_can_capnp(msgs)) + + time.sleep(0.01) + msgs = messaging.drain_sock(controls_sock) + if len(msgs): + event_name = msgs[0].controlsState.alertType.split("/")[0] + self.assertEqual(EVENT_NAME[expected_event], event_name, + f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}") + break + else: + self.fail(f"failed to fingerprint {car_model}") + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py new file mode 100755 index 00000000000000..244c56687c54e4 --- /dev/null +++ b/selfdrive/controls/tests/test_state_machine.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +import unittest + +from cereal import car, log +from common.realtime import DT_CTRL +from selfdrive.car.car_helpers import interfaces +from selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME +from selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \ + AudibleAlert, EVENTS + +State = log.ControlsState.OpenpilotState + +# The event types that maintain the current state +MAINTAIN_STATES = {State.enabled: None, State.disabled: None, State.softDisabling: ET.SOFT_DISABLE, + State.preEnabled: ET.PRE_ENABLE, State.overriding: ET.OVERRIDE} +ALL_STATES = tuple(State.schema.enumerants.values()) +# The event types checked in DISABLED section of state machine +ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE) + + +def make_event(event_types): + event = {} + for ev in event_types: + event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW, + VisualAlert.none, AudibleAlert.none, 1.) + EVENTS[0] = event + return 0 + + +class TestStateMachine(unittest.TestCase): + + def setUp(self): + CarInterface, CarController, CarState = interfaces["mock"] + CP = CarInterface.get_params("mock") + CI = CarInterface(CP, CarController, CarState) + + self.controlsd = Controls(CI=CI) + self.controlsd.events = Events() + self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) + self.CS = car.CarState() + + def test_immediate_disable(self): + for state in ALL_STATES: + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.IMMEDIATE_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() + + def test_user_disable(self): + for state in ALL_STATES: + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.USER_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() + + def test_soft_disable(self): + for state in ALL_STATES: + if state == State.preEnabled: # preEnabled considers NO_ENTRY instead + continue + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.SOFT_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling) + self.controlsd.events.clear() + + def test_soft_disable_timer(self): + self.controlsd.state = State.enabled + self.controlsd.events.add(make_event([ET.SOFT_DISABLE])) + self.controlsd.state_transition(self.CS) + for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): + self.assertEqual(self.controlsd.state, State.softDisabling) + self.controlsd.state_transition(self.CS) + + self.assertEqual(self.controlsd.state, State.disabled) + + def test_no_entry(self): + # disabled with enable events + for et in ENABLE_EVENT_TYPES: + self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled) + self.controlsd.events.clear() + + def test_no_entry_pre_enable(self): + # preEnabled with preEnabled event + self.controlsd.state = State.preEnabled + self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled) + + def test_maintain_states(self): + # Given current state's event type, we should maintain state + for state in ALL_STATES: + self.controlsd.state = state + self.controlsd.events.add(make_event([MAINTAIN_STATES[state]])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, state) + self.controlsd.events.clear() + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py deleted file mode 100644 index 84389856b64bab..00000000000000 --- a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py +++ /dev/null @@ -1,70 +0,0 @@ -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/debug/adb.sh b/selfdrive/debug/adb.sh new file mode 100755 index 00000000000000..919a82fefc726b --- /dev/null +++ b/selfdrive/debug/adb.sh @@ -0,0 +1,11 @@ +#!/usr/bin/bash +set -e + +PORT=5555 + +setprop service.adb.tcp.port $PORT +sudo systemctl start adbd + +IP=$(echo $SSH_CONNECTION | awk '{ print $3}') +echo "then, connect on your computer:" +echo "adb connect $IP:$PORT" diff --git a/selfdrive/debug/analyze-msg-size.py b/selfdrive/debug/analyze-msg-size.py deleted file mode 100755 index 69015a6be2b96e..00000000000000 --- a/selfdrive/debug/analyze-msg-size.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python3 -import argparse -from tqdm import tqdm - -from cereal.services import SERVICE_LIST, QueueSize -from openpilot.tools.lib.logreader import LogReader - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Analyze message sizes from a log route") - parser.add_argument("route", nargs="?", default="98395b7c5b27882e/000000a8--f87e7cd255", - help="Log route to analyze (default: 98395b7c5b27882e/000000a8--f87e7cd255)") - args = parser.parse_args() - - lr = LogReader(args.route) - - szs = {} - for msg in tqdm(lr): - sz = len(msg.as_builder().to_bytes()) - msg_type = msg.which() - if msg_type not in szs: - szs[msg_type] = {'min': sz, 'max': sz, 'sum': sz, 'count': 1} - else: - szs[msg_type]['min'] = min(szs[msg_type]['min'], sz) - szs[msg_type]['max'] = max(szs[msg_type]['max'], sz) - szs[msg_type]['sum'] += sz - szs[msg_type]['count'] += 1 - - print() - print(f"{'Service':<36} {'Min (KB)':>12} {'Max (KB)':>12} {'Avg (KB)':>12} {'KB/min':>12} {'KB/sec':>12} {'Minutes in 10MB':>18} {'Seconds in Queue':>18}") - print("-" * 132) - def sort_key(x): - k, v = x - avg = v['sum'] / v['count'] - freq = SERVICE_LIST.get(k, None) - freq_val = freq.frequency if freq else 0.0 - kb_per_min = (avg * freq_val * 60) / 1024 if freq_val > 0 else 0.0 - return kb_per_min - total_kb_per_min = 0.0 - RINGBUFFER_SIZE_KB = 10 * 1024 # 10MB old default - for k, v in sorted(szs.items(), key=sort_key, reverse=True): - avg = v['sum'] / v['count'] - service = SERVICE_LIST.get(k, None) - freq_val = service.frequency if service else 0.0 - queue_size_kb = (service.queue_size / 1024) if service else 250 # default to SMALL - kb_per_min = (avg * freq_val * 60) / 1024 if freq_val > 0 else 0.0 - kb_per_sec = kb_per_min / 60 - minutes_in_buffer = RINGBUFFER_SIZE_KB / kb_per_min if kb_per_min > 0 else float('inf') - seconds_in_queue = (queue_size_kb / kb_per_sec) if kb_per_sec > 0 else float('inf') - total_kb_per_min += kb_per_min - min_str = f"{minutes_in_buffer:.2f}" if minutes_in_buffer != float('inf') else "inf" - sec_queue_str = f"{seconds_in_queue:.2f}" if seconds_in_queue != float('inf') else "inf" - print(f"{k:<36} {v['min']/1024:>12.2f} {v['max']/1024:>12.2f} {avg/1024:>12.2f} {kb_per_min:>12.2f} {kb_per_sec:>12.2f} {min_str:>18} {sec_queue_str:>18}") - - # Summary section - print() - print(f"Total usage: {total_kb_per_min / 1024:.2f} MB/min") - - # Calculate memory usage: old (10MB for all) vs new (from services.py) - OLD_SIZE = 10 * 1024 * 1024 # 10MB was the old default - old_total = len(SERVICE_LIST) * OLD_SIZE - - new_total = sum(s.queue_size for s in SERVICE_LIST.values()) - - # Count by queue size - size_counts = {QueueSize.BIG: 0, QueueSize.MEDIUM: 0, QueueSize.SMALL: 0} - for s in SERVICE_LIST.values(): - size_counts[s.queue_size] += 1 - - savings_pct = (1 - new_total / old_total) * 100 - - print() - print(f"{'Queue Size Comparison':<40}") - print("-" * 60) - print(f"{'Old (10MB default):':<30} {old_total / 1024 / 1024:>10.2f} MB") - print(f"{'New (from services.py):':<30} {new_total / 1024 / 1024:>10.2f} MB") - print(f"{'Savings:':<30} {savings_pct:>10.1f}%") - print() - print(f"{'Breakdown:':<30}") - print(f" BIG (10MB): {size_counts[QueueSize.BIG]:>3} services") - print(f" MEDIUM (2MB): {size_counts[QueueSize.MEDIUM]:>3} services") - print(f" SMALL (250KB): {size_counts[QueueSize.SMALL]:>3} services") diff --git a/selfdrive/debug/can_print_changes.py b/selfdrive/debug/can_print_changes.py index 97d60b2b05ed9f..b883fbc23f2499 100755 --- a/selfdrive/debug/can_print_changes.py +++ b/selfdrive/debug/can_print_changes.py @@ -5,8 +5,8 @@ from collections import defaultdict import cereal.messaging as messaging -from openpilot.selfdrive.debug.can_table import can_table -from openpilot.tools.lib.logreader import LogIterable, LogReader +from selfdrive.debug.can_table import can_table +from tools.lib.logreader import logreader_from_route_or_segment RED = '\033[91m' CLEAR = '\033[0m' @@ -54,7 +54,6 @@ def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False): update(new_msgs, bus, dat, low_to_high, high_to_low) else: # Live mode - print(f"Waiting for messages on bus {bus}") try: while 1: can_recv = messaging.drain_sock(logcan) @@ -90,20 +89,15 @@ def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False): formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0) parser.add_argument("--table", action="store_true", help="Print a cabana-like table") - parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with. Use empty quotes to compare against all zeros.") + parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with") parser.add_argument("comp", type=str, nargs='?', help="Route or segment to compare against init") args = parser.parse_args() - init_lr: LogIterable | None = None - new_lr: LogIterable | None = None - + init_lr, new_lr = None, None if args.init: - if args.init == '': - init_lr = [] - else: - init_lr = LogReader(args.init) + init_lr = logreader_from_route_or_segment(args.init) if args.comp: - new_lr = LogReader(args.comp) + new_lr = logreader_from_route_or_segment(args.comp) can_printer(args.bus, init_msgs=init_lr, new_msgs=new_lr, table=args.table) diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index f2ed6730d37a30..3f991d4e6c042c 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -1,17 +1,17 @@ #!/usr/bin/env python3 import argparse import binascii -import time from collections import defaultdict import cereal.messaging as messaging +from common.realtime import sec_since_boot def can_printer(bus, max_msg, addr, ascii_decode): logcan = messaging.sub_sock('can', addr=addr) - start = time.monotonic() - lp = time.monotonic() + start = sec_since_boot() + lp = sec_since_boot() msgs = defaultdict(list) while 1: can_recv = messaging.drain_sock(logcan, wait_for_one=True) @@ -20,17 +20,17 @@ def can_printer(bus, max_msg, addr, ascii_decode): if y.src == bus: msgs[y.address].append(y.dat) - if time.monotonic() - lp > 0.1: + if sec_since_boot() - lp > 0.1: dd = chr(27) + "[2J" - dd += f"{time.monotonic() - start:5.2f}\n" - for _addr in sorted(msgs.keys()): - a = f"\"{msgs[_addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else "" - x = binascii.hexlify(msgs[_addr][-1]).decode('ascii') - freq = len(msgs[_addr]) / (time.monotonic() - start) - if max_msg is None or _addr < max_msg: - dd += f"{_addr:04X}({_addr:4d})({len(msgs[_addr]):6d})({freq:3}dHz) {x.ljust(20)} {a}\n" + dd += f"{sec_since_boot() - start:5.2f}\n" + for addr in sorted(msgs.keys()): + a = f"\"{msgs[addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else "" + x = binascii.hexlify(msgs[addr][-1]).decode('ascii') + freq = len(msgs[addr]) / (sec_since_boot() - start) + if max_msg is None or addr < max_msg: + dd += "%04X(%4d)(%6d)(%3dHz) %s %s\n" % (addr, addr, len(msgs[addr]), freq, x.ljust(20), a) print(dd) - lp = time.monotonic() + lp = sec_since_boot() if __name__ == "__main__": parser = argparse.ArgumentParser(description="simple CAN data viewer", diff --git a/selfdrive/debug/car/clear_dtc.py b/selfdrive/debug/car/clear_dtc.py deleted file mode 100755 index b2c5fe3db51f45..00000000000000 --- a/selfdrive/debug/car/clear_dtc.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python3 -import sys -import argparse -from subprocess import check_output, CalledProcessError -from opendbc.car.carlog import carlog -from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE -from opendbc.car.structs import CarParams -from panda import Panda - -parser = argparse.ArgumentParser(description="clear DTC status") -parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address -parser.add_argument("--bus", type=int, default=0) -parser.add_argument('--debug', action='store_true') -args = parser.parse_args() - -if args.debug: - carlog.setLevel('DEBUG') - -try: - check_output(["pidof", "pandad"]) - print("pandad is running, please kill openpilot before running this script! (aborted)") - sys.exit(1) -except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (pandad not running) - raise e - -panda = Panda() -panda.set_safety_mode(CarParams.SafetyModel.elm327) -uds_client = UdsClient(panda, args.addr, bus=args.bus) -print("extended diagnostic session ...") -try: - uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) -except MessageTimeoutError: - # functional address isn't properly handled so a timeout occurs - if args.addr != 0x7DF: - raise -print("clear diagnostic info ...") -try: - uds_client.clear_diagnostic_information(DTC_GROUP_TYPE.ALL) -except MessageTimeoutError: - # functional address isn't properly handled so a timeout occurs - if args.addr != 0x7DF: - pass -print("") -print("you may need to power cycle your vehicle now") diff --git a/selfdrive/debug/car/disable_ecu.py b/selfdrive/debug/car/disable_ecu.py deleted file mode 100755 index 14d0cbb9cf3c95..00000000000000 --- a/selfdrive/debug/car/disable_ecu.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python3 -import time -import cereal.messaging as messaging -from opendbc.car.disable_ecu import disable_ecu -from openpilot.selfdrive.car.card import can_comm_callbacks - -if __name__ == "__main__": - sendcan = messaging.pub_sock('sendcan') - logcan = messaging.sub_sock('can') - can_callbacks = can_comm_callbacks(logcan, sendcan) - time.sleep(1) - - # honda bosch radar disable - disabled = disable_ecu(*can_callbacks, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5) - print(f"disabled: {disabled}") diff --git a/selfdrive/debug/car/ecu_addrs.py b/selfdrive/debug/car/ecu_addrs.py deleted file mode 100755 index 584c930ebfd3f4..00000000000000 --- a/selfdrive/debug/car/ecu_addrs.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import time -import cereal.messaging as messaging -from opendbc.car.carlog import carlog -from opendbc.car.ecu_addrs import get_all_ecu_addrs -from openpilot.common.params import Params -from openpilot.selfdrive.car.card import can_comm_callbacks, obd_callback - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='Get addresses of all ECUs') - parser.add_argument('--debug', action='store_true') - parser.add_argument('--bus', type=int, default=1) - parser.add_argument('--no-obd', action='store_true') - parser.add_argument('--timeout', type=float, default=1.0) - args = parser.parse_args() - - if args.debug: - carlog.setLevel('DEBUG') - - logcan = messaging.sub_sock('can') - sendcan = messaging.pub_sock('sendcan') - can_callbacks = can_comm_callbacks(logcan, sendcan) - - # Set up params for pandad - params = Params() - params.remove("FirmwareQueryDone") - params.put_bool("IsOnroad", False) - time.sleep(0.2) # thread is 10 Hz - params.put_bool("IsOnroad", True) - - obd_callback(params)(not args.no_obd) - - print("Getting ECU addresses ...") - ecu_addrs = get_all_ecu_addrs(*can_callbacks, args.bus, args.timeout) - - print() - print("Found ECUs on rx addresses:") - for addr, subaddr, _ in ecu_addrs: - msg = f" {hex(addr)}" - if subaddr is not None: - msg += f" (sub-address: {hex(subaddr)})" - print(msg) diff --git a/selfdrive/debug/car/fw_versions.py b/selfdrive/debug/car/fw_versions.py deleted file mode 100755 index 6ae10d2fb23ff2..00000000000000 --- a/selfdrive/debug/car/fw_versions.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python3 -import time -import argparse -import cereal.messaging as messaging -from cereal import car -from opendbc.car.carlog import carlog -from opendbc.car.fw_versions import get_fw_versions, match_fw_to_car -from opendbc.car.vin import get_vin -from openpilot.common.params import Params -from openpilot.selfdrive.car.card import can_comm_callbacks, obd_callback -from typing import Any - -Ecu = car.CarParams.Ecu - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='Get firmware version of ECUs') - parser.add_argument('--scan', action='store_true') - parser.add_argument('--debug', action='store_true') - parser.add_argument('--brand', help='Only query addresses/with requests for this brand') - args = parser.parse_args() - - if args.debug: - carlog.setLevel('DEBUG') - - logcan = messaging.sub_sock('can') - pandaStates_sock = messaging.sub_sock('pandaStates') - sendcan = messaging.pub_sock('sendcan') - can_callbacks = can_comm_callbacks(logcan, sendcan) - - # Set up params for pandad - params = Params() - params.remove("FirmwareQueryDone") - params.put_bool("IsOnroad", False) - time.sleep(0.2) # thread is 10 Hz - params.put_bool("IsOnroad", True) - set_obd_multiplexing = obd_callback(params) - - extra: Any = None - if args.scan: - extra = {} - # Honda - for i in range(256): - extra[(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = [] - extra[(Ecu.unknown, 0x700 + i, None)] = [] - extra[(Ecu.unknown, 0x750, i)] = [] - extra = {"any": {"debug": extra}} - - num_pandas = len(messaging.recv_one_retry(pandaStates_sock).pandaStates) - - t = time.monotonic() - print("Getting vin...") - set_obd_multiplexing(True) - vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (0, 1)) - print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}') - print(f"Getting VIN took {time.monotonic() - t:.3f} s") - print() - - t = time.monotonic() - fw_vers = get_fw_versions(*can_callbacks, set_obd_multiplexing, query_brand=args.brand, extra=extra, num_pandas=num_pandas, progress=True) - _, candidates = match_fw_to_car(fw_vers, vin) - - print() - print("Found FW versions") - print("{") - padding = max([len(fw.brand) for fw in fw_vers] or [0]) - for version in fw_vers: - subaddr = None if version.subAddress == 0 else hex(version.subAddress) - print(f" Brand: {version.brand:{padding}}, bus: {version.bus}, OBD: {version.obdMultiplexing} - " + - f"(Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion!r}]") - print("}") - - print() - print("Possible matches:", candidates) - print(f"Getting fw took {time.monotonic() - t:.3f} s") diff --git a/selfdrive/debug/car/vin.py b/selfdrive/debug/car/vin.py deleted file mode 100755 index 9b1d6528ccb3d1..00000000000000 --- a/selfdrive/debug/car/vin.py +++ /dev/null @@ -1,26 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import time -import cereal.messaging as messaging -from opendbc.car.carlog import carlog -from opendbc.car.vin import get_vin -from openpilot.selfdrive.car.card import can_comm_callbacks - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='Get VIN of the car') - parser.add_argument('--debug', action='store_true') - parser.add_argument('--bus', type=int, default=1) - parser.add_argument('--timeout', type=float, default=0.1) - parser.add_argument('--retry', type=int, default=5) - args = parser.parse_args() - - if args.debug: - carlog.setLevel('DEBUG') - - sendcan = messaging.pub_sock('sendcan') - logcan = messaging.sub_sock('can') - can_callbacks = can_comm_callbacks(logcan, sendcan) - time.sleep(1) - - vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (args.bus,), args.timeout, args.retry) - print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}') diff --git a/selfdrive/debug/check_can_parser_performance.py b/selfdrive/debug/check_can_parser_performance.py deleted file mode 100755 index 20987b3cf41286..00000000000000 --- a/selfdrive/debug/check_can_parser_performance.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np -import time -from tqdm import tqdm - -from cereal import car -from opendbc.car.tests.routes import CarTestRoute -from openpilot.selfdrive.car.tests.test_models import TestCarModelBase -from openpilot.tools.plotjuggler.juggle import DEMO_ROUTE - -N_RUNS = 10 - - -class CarModelTestCase(TestCarModelBase): - test_route = CarTestRoute(DEMO_ROUTE, None) - - -if __name__ == '__main__': - # Get CAN messages and parsers - tm = CarModelTestCase() - tm.setUpClass() - tm.setUp() - - CC = car.CarControl.new_message() - ets = [] - for _ in tqdm(range(N_RUNS)): - start_t = time.process_time_ns() - for msg in tm.can_msgs: - for cp in tm.CI.can_parsers.values(): - if cp is not None: - cp.update_strings(msg) - ets.append((time.process_time_ns() - start_t) * 1e-6) - - print(f'{len(tm.can_msgs)} CAN packets, {N_RUNS} runs') - print(f'{np.mean(ets):.2f} mean ms, {max(ets):.2f} max ms, {min(ets):.2f} min ms, {np.std(ets):.2f} std ms') - print(f'{np.mean(ets) / len(tm.can_msgs):.4f} mean ms / CAN packet') diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py index 1765aeb86b6752..b6f3c91bd09a31 100755 --- a/selfdrive/debug/check_freq.py +++ b/selfdrive/debug/check_freq.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 import argparse import numpy as np -import time from collections import defaultdict, deque -from collections.abc import MutableSequence +from typing import DefaultDict, Deque +from common.realtime import sec_since_boot import cereal.messaging as messaging @@ -19,10 +19,10 @@ socket_names = args.socket sockets = {} - rcv_times: defaultdict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) - valids: defaultdict[str, deque[bool]] = defaultdict(lambda: deque(maxlen=100)) + rcv_times: DefaultDict[str, Deque[float]] = defaultdict(lambda: deque(maxlen=100)) + valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100)) - t = time.monotonic() + t = sec_since_boot() for name in socket_names: sock = messaging.sub_sock(name, poller=poller) sockets[sock] = name @@ -36,7 +36,7 @@ name = msg.which() - t = time.monotonic() + t = sec_since_boot() rcv_times[name].append(msg.logMonoTime / 1e9) valids[name].append(msg.valid) diff --git a/selfdrive/debug/check_lag.py b/selfdrive/debug/check_lag.py index 341ae79c8b5138..141156db913934 100755 --- a/selfdrive/debug/check_lag.py +++ b/selfdrive/debug/check_lag.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 +from typing import Dict import cereal.messaging as messaging -from cereal.services import SERVICE_LIST +from cereal.services import service_list TO_CHECK = ['carState'] @@ -9,7 +10,7 @@ if __name__ == "__main__": sm = messaging.SubMaster(TO_CHECK) - prev_t: dict[str, float] = {} + prev_t: Dict[str, float] = {} while True: sm.update() @@ -19,7 +20,7 @@ t = sm.logMonoTime[s] / 1e9 if s in prev_t: - expected = 1.0 / (SERVICE_LIST[s].frequency) + expected = 1.0 / (service_list[s].frequency) dt = t - prev_t[s] if dt > 10 * expected: print(t, s, dt) diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py index fc527cd81b81d7..69304f97b56ff1 100755 --- a/selfdrive/debug/check_timings.py +++ b/selfdrive/debug/check_timings.py @@ -1,36 +1,25 @@ #!/usr/bin/env python3 + import sys import time import numpy as np -import datetime -from collections.abc import MutableSequence -from collections import defaultdict +from typing import DefaultDict, Deque +from collections import defaultdict, deque import cereal.messaging as messaging +socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]} +ts: DefaultDict[str, Deque[float]] = defaultdict(lambda: deque(maxlen=100)) if __name__ == "__main__": - ts: defaultdict[str, MutableSequence[float]] = defaultdict(list) - socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]} - try: - st = time.monotonic() - while True: - print() - for s, sock in socks.items(): - msgs = messaging.drain_sock(sock) - for m in msgs: - ts[s].append(m.logMonoTime / 1e6) - - if len(ts[s]) > 2: - d = np.diff(ts[s])[-100:] - print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}") - time.sleep(1) - except KeyboardInterrupt: - print("\n") - print("="*5, "timing summary", "="*5) + while True: + print() for s, sock in socks.items(): msgs = messaging.drain_sock(sock) - if len(ts[s]) > 2: + for m in msgs: + ts[s].append(m.logMonoTime / 1e6) + + if len(ts[s]): d = np.diff(ts[s]) - print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}") - print("="*5, datetime.timedelta(seconds=time.monotonic()-st), "="*5) + print(f"{s:25} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}") + time.sleep(1) diff --git a/selfdrive/debug/clear_dtc.py b/selfdrive/debug/clear_dtc.py new file mode 100755 index 00000000000000..d84828079d9a48 --- /dev/null +++ b/selfdrive/debug/clear_dtc.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import sys +import argparse +from subprocess import check_output, CalledProcessError +from panda import Panda +from panda.python.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE + +parser = argparse.ArgumentParser(description="clear DTC status") +parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address +parser.add_argument("--bus", type=int, default=0) +parser.add_argument('--debug', action='store_true') +args = parser.parse_args() + +try: + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) +except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (boardd not running) + raise e + +panda = Panda() +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) +print("extended diagnostic session ...") +try: + uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + raise +print("clear diagnostic info ...") +try: + uds_client.clear_diagnostic_information(DTC_GROUP_TYPE.ALL) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + pass +print("") +print("you may need to power cycle your vehicle now") diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py index 76e02d414ec873..93dd5bdc470f25 100755 --- a/selfdrive/debug/count_events.py +++ b/selfdrive/debug/count_events.py @@ -4,49 +4,40 @@ import datetime from collections import Counter from pprint import pprint +from tqdm import tqdm from typing import cast -from cereal.services import SERVICE_LIST -from openpilot.tools.lib.logreader import LogReader, ReadMode -from openpilot.selfdrive.test.process_replay.migration import migrate_all +from cereal.services import service_list +from tools.lib.route import Route +from tools.lib.logreader import LogReader if __name__ == "__main__": + r = Route(sys.argv[1]) + + cnt_valid: Counter = Counter() cnt_events: Counter = Counter() - cams = [s for s in SERVICE_LIST if s.endswith('CameraState')] + cams = [s for s in service_list if s.endswith('CameraState')] cnt_cameras = dict.fromkeys(cams, 0) - events: list[tuple[float, set[str]]] = [] - alerts: list[tuple[float, str]] = [] start_time = math.inf end_time = -math.inf - ignition_off = None - for msg in migrate_all(LogReader(sys.argv[1], ReadMode.QLOG)): - t = (msg.logMonoTime - start_time) / 1e9 - end_time = max(end_time, msg.logMonoTime) - start_time = min(start_time, msg.logMonoTime) - - if msg.which() == 'onroadEvents': - for e in msg.onroadEvents: - cnt_events[e.name] += 1 - - ae = {str(e.name) for e in msg.onroadEvents if e.name not in ('pedalPressed', 'steerOverride', 'gasPressedOverride')} - if len(events) == 0 or ae != events[-1][1]: - events.append((t, ae)) - - elif msg.which() == 'selfdriveState': - at = msg.selfdriveState.alertType - if "/override" not in at or "lanechange" in at.lower(): - if len(alerts) == 0 or alerts[-1][1] != at: - alerts.append((t, at)) - elif msg.which() == 'pandaStates': - if ignition_off is None: - ign = any(ps.ignitionLine or ps.ignitionCan for ps in msg.pandaStates) - if not ign: - ignition_off = msg.logMonoTime - break - elif msg.which() in cams: - cnt_cameras[msg.which()] += 1 + for q in tqdm(r.qlog_paths()): + if q is None: + continue + lr = list(LogReader(q)) + for msg in lr: + if msg.which() == 'carEvents': + for e in msg.carEvents: + cnt_events[e.name] += 1 + elif msg.which() in cams: + cnt_cameras[msg.which()] += 1 + + if not msg.valid: + cnt_valid[msg.which()] += 1 + + end_time = max(end_time, msg.logMonoTime) + start_time = min(start_time, msg.logMonoTime) duration = (end_time - start_time) / 1e9 @@ -54,24 +45,15 @@ pprint(cnt_events) print("\n") - print("Events") - for t, evt in events: - print(f"{t:8.2f} {evt}") + print("Not valid") + pprint(cnt_valid) print("\n") print("Cameras") for k, v in cnt_cameras.items(): - s = SERVICE_LIST[k] + s = service_list[k] expected_frames = int(s.frequency * duration / cast(float, s.decimation)) print(" ", k.ljust(20), f"{v}, {v/expected_frames:.1%} of expected") print("\n") - print("Alerts") - for t, a in alerts: - print(f"{t:8.2f} {a}") - - print("\n") - if ignition_off is not None: - ignition_off = round((ignition_off - start_time) / 1e9, 2) - print("Ignition off at", ignition_off) print("Route duration", datetime.timedelta(seconds=duration)) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 089685103f7cda..b3294c8728412a 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# type: ignore ''' System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. Features: @@ -8,10 +9,10 @@ Calculate minumium/maximum/accumulated_average cpu usage as long term inspections. Monitor multiple processes simuteneously. Sample usage: - root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py pandad,ubloxd - ('Add monitored proc:', './pandad') + root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd + ('Add monitored proc:', './boardd') ('Add monitored proc:', 'python locationd/ubloxd.py') - pandad: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% + boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39% ''' import psutil @@ -23,7 +24,7 @@ import re from collections import defaultdict -from openpilot.system.manager.process_config import managed_processes +from selfdrive.manager.process_config import managed_processes # Do statistics every 5 seconds PRINT_INTERVAL = 5 @@ -36,6 +37,8 @@ cpu_time_names = ['user', 'system', 'children_user', 'children_system'] +timer = getattr(time, 'monotonic', time.time) + def get_arg_parser(): parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) @@ -63,13 +66,13 @@ def get_arg_parser(): for p in psutil.process_iter(): if p == psutil.Process(): continue - matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(fr'.*{pn}.*', l, re.M | re.I))) + matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I))) if matched: k = ' '.join(p.cmdline()) print('Add monitored proc:', k) stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None), - 'avg': defaultdict(float), 'last_cpu_times': None, 'last_sys_time': None} - stats[k]['last_sys_time'] = time.monotonic() + 'avg': defaultdict(lambda: 0.0), 'last_cpu_times': None, 'last_sys_time': None} + stats[k]['last_sys_time'] = timer() stats[k]['last_cpu_times'] = p.cpu_times() monitored_procs.append(p) i = 0 @@ -77,7 +80,7 @@ def get_arg_parser(): while True: for p in monitored_procs: k = ' '.join(p.cmdline()) - cur_sys_time = time.monotonic() + cur_sys_time = timer() cur_cpu_times = p.cpu_times() cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time']) stats[k]['last_sys_time'] = cur_sys_time diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 00fa33ac63a2f0..b40c8e304c8336 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -4,13 +4,13 @@ from cereal import car, log import cereal.messaging as messaging -from opendbc.car.honda.interface import CarInterface -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.selfdrived.events import ET, Events -from openpilot.selfdrive.selfdrived.alertmanager import AlertManager -from openpilot.system.manager.process_config import managed_processes +from common.realtime import DT_CTRL +from selfdrive.car.honda.interface import CarInterface +from selfdrive.controls.lib.events import ET, Events +from selfdrive.controls.lib.alertmanager import AlertManager +from selfdrive.manager.process_config import managed_processes -EventName = log.OnroadEvent.EventName +EventName = car.CarEvent.EventName def randperc() -> float: return 100. * random.random() @@ -25,8 +25,7 @@ def cycle_alerts(duration=200, is_metric=False): (EventName.buttonCancel, ET.USER_DISABLE), (EventName.wrongGear, ET.NO_ENTRY), - (EventName.locationdTemporaryError, ET.SOFT_DISABLE), - (EventName.paramsdTemporaryError, ET.SOFT_DISABLE), + (EventName.vehicleModelInvalid, ET.SOFT_DISABLE), (EventName.accFaulted, ET.IMMEDIATE_DISABLE), # DM sequence @@ -52,12 +51,12 @@ def cycle_alerts(duration=200, is_metric=False): cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] CS = car.CarState.new_message() - CP = CarInterface.get_non_essential_params("HONDA_CIVIC") + CP = CarInterface.get_params("HONDA CIVIC 2016") sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'livePose', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState'] + cameras) - pm = messaging.PubMaster(['selfdriveState', 'pandaStates', 'deviceState']) + pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) events = Events() AM = AlertManager() @@ -99,19 +98,22 @@ def cycle_alerts(duration=200, is_metric=False): alert = AM.process_alerts(frame, []) print(alert) for _ in range(duration): - dat = messaging.new_message('selfdriveState') - dat.selfdriveState.enabled = False + dat = messaging.new_message() + dat.init('controlsState') + dat.controlsState.enabled = False if alert: - dat.selfdriveState.alertText1 = alert.alert_text_1 - dat.selfdriveState.alertText2 = alert.alert_text_2 - dat.selfdriveState.alertSize = alert.alert_size - dat.selfdriveState.alertStatus = alert.alert_status - dat.selfdriveState.alertType = alert.alert_type - dat.selfdriveState.alertSound = alert.audible_alert - pm.send('selfdriveState', dat) - - dat = messaging.new_message('deviceState') + dat.controlsState.alertText1 = alert.alert_text_1 + dat.controlsState.alertText2 = alert.alert_text_2 + dat.controlsState.alertSize = alert.alert_size + dat.controlsState.alertStatus = alert.alert_status + dat.controlsState.alertBlinkingRate = alert.alert_rate + dat.controlsState.alertType = alert.alert_type + dat.controlsState.alertSound = alert.audible_alert + pm.send('controlsState', dat) + + dat = messaging.new_message() + dat.init('deviceState') dat.deviceState.started = True pm.send('deviceState', dat) diff --git a/selfdrive/debug/debug_fw_fingerprinting_offline.py b/selfdrive/debug/debug_fw_fingerprinting_offline.py deleted file mode 100755 index d841e91053d789..00000000000000 --- a/selfdrive/debug/debug_fw_fingerprinting_offline.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python3 -import argparse - -from opendbc.car import uds -from openpilot.tools.lib.live_logreader import live_logreader -from openpilot.tools.lib.logreader import LogReader, ReadMode - - -def main(route: str | None, addrs: list[int], rxoffset: int | None): - """ - TODO: - - highlight TX vs RX clearly - - disambiguate sendcan and can (useful to know if something sent on sendcan made it to the bus on can->128) - - print as fixed width table, easier to read - """ - - if route is None: - lr = live_logreader() - else: - lr = LogReader(route, default_mode=ReadMode.RLOG, sort_by_time=True) - - start_mono_time = None - prev_mono_time = 0 - - # include rx addresses - addrs = addrs + [uds.get_rx_addr_for_tx_addr(addr, rxoffset) for addr in addrs] - - for msg in lr: - if msg.which() == 'can': - if start_mono_time is None: - start_mono_time = msg.logMonoTime - - if msg.which() in ("can", 'sendcan'): - for can in getattr(msg, msg.which()): - if can.address in addrs or not len(addrs): - if msg.logMonoTime != prev_mono_time: - print() - prev_mono_time = msg.logMonoTime - print(f"{msg.which():>7}: rxaddr={can.address}, bus={str(can.src) + ',':<4} {round((msg.logMonoTime - start_mono_time) * 1e-6)} ms, " + - f"0x{can.dat.hex()}, {can.dat}, {len(can.dat)=}") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='View back and forth ISO-TP communication between various ECUs given an address') - parser.add_argument('route', nargs='?', help='Route name, live if not specified') - parser.add_argument('--addrs', nargs='*', default=[], help='List of tx address to view (0x7e0 for engine)') - parser.add_argument('--rxoffset', default='') - args = parser.parse_args() - - addrs = [int(addr, base=16) if addr.startswith('0x') else int(addr) for addr in args.addrs] - rxoffset = int(args.rxoffset, base=16) if args.rxoffset else None - main(args.route, addrs, rxoffset) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 78cf51e3db5fb0..fdb825eead615f 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -1,27 +1,15 @@ #!/usr/bin/env python3 +import os import sys import argparse import json +from hexdump import hexdump import codecs - -from cereal import log -from cereal.services import SERVICE_LIST -from openpilot.tools.lib.live_logreader import raw_live_logreader - - codecs.register_error("strict", codecs.backslashreplace_errors) -def hexdump(msg): - m = str.upper(msg.hex()) - m = [m[i:i+2] for i in range(0,len(m),2)] - m = [m[i:i+16] for i in range(0,len(m),16)] - for row,dump in enumerate(m): - addr = '%08X:' % (row*16) - raw = ' '.join(dump[:8]) + ' ' + ' '.join(dump[8:]) - space = ' ' * (48 - len(raw)) - asci = ''.join(chr(int(x,16)) if 0x20 <= int(x,16) <= 0x7E else '.' for x in dump) - print(f'{addr} {raw} {space} {asci}') - +from cereal import log +import cereal.messaging as messaging +from cereal.services import service_list if __name__ == "__main__": @@ -33,20 +21,31 @@ def hexdump(msg): parser.add_argument('--no-print', action='store_true') parser.add_argument('--addr', default='127.0.0.1') parser.add_argument('--values', help='values to monitor (instead of entire event)') - parser.add_argument("socket", type=str, nargs='*', default=list(SERVICE_LIST.keys()), help="socket names to dump. defaults to all services defined in cereal") + parser.add_argument("socket", type=str, nargs='*', help="socket names to dump. defaults to all services defined in cereal") args = parser.parse_args() - lr = raw_live_logreader(args.socket, args.addr) + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + poller = messaging.Poller() + + for m in args.socket if len(args.socket) > 0 else service_list: + messaging.sub_sock(m, poller, addr=args.addr) values = None if args.values: values = [s.strip().split(".") for s in args.values.split(",")] - for msg in lr: - with log.Event.from_bytes(msg) as evt: + while 1: + polld = poller.poll(100) + for sock in polld: + msg = sock.receive() + evt = log.Event.from_bytes(msg) + if not args.no_print: if args.pipe: - sys.stdout.write(str(msg)) + sys.stdout.write(msg) sys.stdout.flush() elif args.raw: hexdump(msg) diff --git a/selfdrive/debug/dump_car_docs.py b/selfdrive/debug/dump_car_docs.py deleted file mode 100755 index f0e99cda24ad6f..00000000000000 --- a/selfdrive/debug/dump_car_docs.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import pickle - -from opendbc.car.docs import get_all_car_docs - - -def dump_car_docs(path): - with open(path, 'wb') as f: - pickle.dump(get_all_car_docs(), f) - print(f'Dumping car info to {path}') - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("--path", required=True) - args = parser.parse_args() - dump_car_docs(args.path) diff --git a/selfdrive/debug/dump_car_info.py b/selfdrive/debug/dump_car_info.py new file mode 100755 index 00000000000000..c9a21c2848d60c --- /dev/null +++ b/selfdrive/debug/dump_car_info.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 +import argparse +import pickle + +from selfdrive.car.docs import get_all_car_info + + +def dump_car_info(path): + with open(path, 'wb') as f: + pickle.dump(get_all_car_info(), f) + print(f'Dumping car info to {path}') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--path", required=True) + args = parser.parse_args() + dump_car_info(args.path) diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py index 9cbab0b41fe1c6..af52953936e700 100755 --- a/selfdrive/debug/filter_log_message.py +++ b/selfdrive/debug/filter_log_message.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 +import os import argparse import json import cereal.messaging as messaging -from openpilot.tools.lib.logreader import LogReader +from tools.lib.logreader import LogReader +from tools.lib.route import Route LEVELS = { "DEBUG": 10, @@ -46,27 +48,36 @@ def print_androidlog(t, msg): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('--absolute', action='store_true') parser.add_argument('--level', default='DEBUG') parser.add_argument('--addr', default='127.0.0.1') parser.add_argument("route", type=str, nargs='*', help="route name + segment number for offline usage") args = parser.parse_args() + logs = None + if len(args.route): + if os.path.exists(args.route[0]): + logs = [args.route[0]] + else: + r = Route(args.route[0]) + logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths())] + + if len(args.route) == 2 and logs: + n = int(args.route[1]) + logs = [logs[n]] + min_level = LEVELS[args.level] - if args.route: - st = None if not args.absolute else 0 - for route in args.route: - lr = LogReader(route, sort_by_time=True) - for m in lr: - if st is None: - st = m.logMonoTime - if m.which() == 'logMessage': - print_logmessage(m.logMonoTime-st, m.logMessage, min_level) - elif m.which() == 'errorLogMessage': - print_logmessage(m.logMonoTime-st, m.errorLogMessage, min_level) - elif m.which() == 'androidLog': - print_androidlog(m.logMonoTime-st, m.androidLog) + if logs: + for log in logs: + if log: + lr = LogReader(log) + for m in lr: + if m.which() == 'logMessage': + print_logmessage(m.logMonoTime, m.logMessage, min_level) + elif m.which() == 'errorLogMessage' and 'qlog' in log: + print_logmessage(m.logMonoTime, m.errorLogMessage, min_level) + elif m.which() == 'androidLog': + print_androidlog(m.logMonoTime, m.androidLog) else: sm = messaging.SubMaster(['logMessage', 'androidLog'], addr=args.addr) while True: diff --git a/selfdrive/debug/fingerprint_from_route.py b/selfdrive/debug/fingerprint_from_route.py index 5fd46f5b767853..326e68f8e79b97 100755 --- a/selfdrive/debug/fingerprint_from_route.py +++ b/selfdrive/debug/fingerprint_from_route.py @@ -1,41 +1,38 @@ #!/usr/bin/env python3 import sys -from openpilot.tools.lib.logreader import LogReader, ReadMode +from tools.lib.route import Route +from tools.lib.logreader import MultiLogIterator def get_fingerprint(lr): # TODO: make this a nice tool for car ports. should also work with qlogs for FW fw = None - vin = None msgs = {} for msg in lr: if msg.which() == 'carParams': fw = msg.carParams.carFw - vin = msg.carParams.carVin elif msg.which() == 'can': for c in msg.can: # read also msgs sent by EON on CAN bus 0x80 and filter out the # addr with more than 11 bits - if c.src % 0x80 == 0 and c.address < 0x800 and c.address not in (0x7df, 0x7e0, 0x7e8): + if c.src % 0x80 == 0 and c.address < 0x800: msgs[c.address] = len(c.dat) # show CAN fingerprint - fingerprint = ', '.join(f"{v[0]}: {v[1]}" for v in sorted(msgs.items())) + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) print(f"\nfound {len(msgs)} messages. CAN fingerprint:\n") print(fingerprint) # TODO: also print the fw fingerprint merged with the existing ones # show FW fingerprint - if fw: - print("\nFW fingerprint:\n") - for f in fw: - print(f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): [") - print(f" {f.fwVersion},") - print(" ],") + print("\nFW fingerprint:\n") + for f in fw: + print(f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): [") + print(f" {f.fwVersion},") + print(" ],") print() - print(f"VIN: {vin}") if __name__ == "__main__": @@ -43,5 +40,6 @@ def get_fingerprint(lr): print("Usage: ./fingerprint_from_route.py ") sys.exit(1) - lr = LogReader(sys.argv[1], ReadMode.QLOG) + route = Route(sys.argv[1]) + lr = MultiLogIterator(route.log_paths()[:5]) get_fingerprint(lr) diff --git a/selfdrive/debug/fuzz_fw_fingerprint.py b/selfdrive/debug/fuzz_fw_fingerprint.py deleted file mode 100755 index b4276c0c1af93d..00000000000000 --- a/selfdrive/debug/fuzz_fw_fingerprint.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -import random -from collections import defaultdict - -from tqdm import tqdm - -from opendbc.car.fw_versions import match_fw_to_car_fuzzy -from opendbc.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS -from opendbc.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS -from opendbc.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS -from opendbc.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS - - -FWS = {} -FWS.update(TOYOTA_FW_VERSIONS) -FWS.update(HONDA_FW_VERSIONS) -FWS.update(HYUNDAI_FW_VERSIONS) -FWS.update(VW_FW_VERSIONS) - -if __name__ == "__main__": - total = 0 - match = 0 - wrong_match = 0 - confusions = defaultdict(set) - - for _ in tqdm(range(1000)): - for candidate, fws in FWS.items(): - fw_dict = {} - for (_, addr, subaddr), fw_list in fws.items(): - fw_dict[(addr, subaddr)] = [random.choice(fw_list)] - - matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate) - - total += 1 - if len(matches) == 1: - if list(matches)[0] == candidate: - match += 1 - else: - confusions[candidate] |= matches - wrong_match += 1 - - print() - for candidate, wrong_matches in sorted(confusions.items()): - print(candidate, wrong_matches) - - print() - print(f"Total fuzz cases: {total}") - print(f"Correct matches: {match}") - print(f"Wrong matches: {wrong_match}") - - diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index 75c3d5579fdd37..e678db4f178b5d 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -4,7 +4,7 @@ # Instructions: # - connect to a Panda -# - run selfdrive/pandad/pandad +# - run selfdrive/boardd/boardd # - launching this script # Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint # - since some messages are published at low frequency, keep this script running for at least 30s, @@ -22,10 +22,10 @@ for c in lc.can: # read also msgs sent by EON on CAN bus 0x80 and filter out the # addr with more than 11 bits - if c.src % 0x80 == 0 and c.address < 0x800 and c.address not in (0x7df, 0x7e0, 0x7e8): + if c.src in [0, 2] and c.address < 0x800: msgs[c.address] = len(c.dat) - fingerprint = ', '.join(f"{v[0]}: {v[1]}" for v in sorted(msgs.items())) + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) print(f"number of messages {len(msgs)}:") print(f"fingerprint {fingerprint}") diff --git a/selfdrive/debug/car/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py similarity index 75% rename from selfdrive/debug/car/hyundai_enable_radar_points.py rename to selfdrive/debug/hyundai_enable_radar_points.py index df150a5224d986..ac7e7102d05d31 100755 --- a/selfdrive/debug/car/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -16,10 +16,8 @@ from typing import NamedTuple from subprocess import check_output, CalledProcessError -from opendbc.car.carlog import carlog -from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE -from opendbc.car.structs import CarParams from panda.python import Panda +from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE class ConfigValues(NamedTuple): default_config: bytes @@ -34,13 +32,7 @@ class ConfigValues(NamedTuple): b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), - b"DN8_ SCC F-CUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues( - default_config=b"\x00\x00\x00\x01\x00\x00", - tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2021 SONATA HYBRID - b"DNhe SCC FHCUP 1.00 1.00 99110-L5000\x19\x04&\x13' ": ConfigValues( - default_config=b"\x00\x00\x00\x01\x00\x00", - tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), @@ -60,17 +52,6 @@ class ConfigValues(NamedTuple): b'IK__ SCC F-CUP 1.00 1.02 96400-G9100\x18\x07\x06\x17\x12 ': ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), - # 2019 SANTA FE - b"TM__ SCC F-CUP 1.00 1.00 99110-S1210\x19\x01%\x168 ": ConfigValues( - default_config=b"\x00\x00\x00\x01\x00\x00", - tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), - b"TM__ SCC F-CUP 1.00 1.02 99110-S2000\x18\x07\x08\x18W ": ConfigValues( - default_config=b"\x00\x00\x00\x01\x00\x00", - tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), - # 2021 K5 HEV - b"DLhe SCC FHCUP 1.00 1.02 99110-L7000 \x01 \x102 ": ConfigValues( - default_config=b"\x00\x00\x00\x01\x00\x00", - tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": @@ -80,15 +61,12 @@ class ConfigValues(NamedTuple): parser.add_argument('--bus', type=int, default=0, help='can bus to use (default: 0)') args = parser.parse_args() - if args.debug: - carlog.setLevel('DEBUG') - try: - check_output(["pidof", "pandad"]) - print("pandad is running, please kill openpilot before running this script! (aborted)") + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") sys.exit(1) except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (pandad not running) + if e.returncode != 1: # 1 == no process found (boardd not running) raise e confirm = input("power on the vehicle keeping the engine off (press start button twice) then type OK to continue: ").upper().strip() @@ -97,15 +75,15 @@ class ConfigValues(NamedTuple): sys.exit(0) panda = Panda() - panda.set_safety_mode(CarParams.SafetyModel.elm327) - uds_client = UdsClient(panda, 0x7D0, bus=args.bus) + panda.set_safety_mode(Panda.SAFETY_ELM327) + uds_client = UdsClient(panda, 0x7D0, bus=args.bus, debug=args.debug) print("\n[START DIAGNOSTIC SESSION]") - session_type : SESSION_TYPE = 0x07 + session_type : SESSION_TYPE = 0x07 # type: ignore uds_client.diagnostic_session_control(session_type) print("[HARDWARE/SOFTWARE VERSION]") - fw_version_data_id : DATA_IDENTIFIER_TYPE = 0xf100 + fw_version_data_id : DATA_IDENTIFIER_TYPE = 0xf100 # type: ignore fw_version = uds_client.read_data_by_identifier(fw_version_data_id) print(fw_version) if fw_version not in SUPPORTED_FW_VERSIONS.keys(): @@ -113,7 +91,7 @@ class ConfigValues(NamedTuple): sys.exit(1) print("[GET CONFIGURATION]") - config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 + config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 # type: ignore current_config = uds_client.read_data_by_identifier(config_data_id) config_values = SUPPORTED_FW_VERSIONS[fw_version] new_config = config_values.default_config if args.default else config_values.tracks_enabled diff --git a/system/webrtc/__init__.py b/selfdrive/debug/internal/__init__.py similarity index 100% rename from system/webrtc/__init__.py rename to selfdrive/debug/internal/__init__.py diff --git a/selfdrive/debug/internal/check_alive_valid.py b/selfdrive/debug/internal/check_alive_valid.py new file mode 100755 index 00000000000000..da488c2140da36 --- /dev/null +++ b/selfdrive/debug/internal/check_alive_valid.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 +import time +import cereal.messaging as messaging + + +if __name__ == "__main__": + sm = messaging.SubMaster(['deviceState', 'pandaStates', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan']) + + i = 0 + while True: + sm.update(0) + + i += 1 + if i % 100 == 0: + print() + print("alive", sm.alive) + print("valid", sm.valid) + + time.sleep(0.01) diff --git a/selfdrive/debug/internal/design_lqr.py b/selfdrive/debug/internal/design_lqr.py new file mode 100755 index 00000000000000..e86926f607b26c --- /dev/null +++ b/selfdrive/debug/internal/design_lqr.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +import numpy as np +import control # pylint: disable=import-error + +dt = 0.01 +A = np.array([[ 0. , 1. ], [-0.78823806, 1.78060701]]) +B = np.array([[-2.23399437e-05], [ 7.58330763e-08]]) +C = np.array([[1., 0.]]) + + +# Kalman tuning +Q = np.diag([1, 1]) +R = np.atleast_2d(1e5) + +(_, _, L) = control.dare(A.T, C.T, Q, R) +L = L.T + +# LQR tuning +Q = np.diag([2e5, 1e-5]) +R = np.atleast_2d(1) +(_, _, K) = control.dare(A, B, Q, R) + +A_cl = (A - B.dot(K)) +sys = control.ss(A_cl, B, C, 0, dt) +dc_gain = control.dcgain(sys) + +print(("self.A = np." + A.__repr__()).replace('\n', '')) +print(("self.B = np." + B.__repr__()).replace('\n', '')) +print(("self.C = np." + C.__repr__()).replace('\n', '')) +print(("self.K = np." + K.__repr__()).replace('\n', '')) +print(("self.L = np." + L.__repr__()).replace('\n', '')) +print("self.dc_gain = " + str(dc_gain)) diff --git a/selfdrive/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py new file mode 100755 index 00000000000000..1ea133cc198bad --- /dev/null +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# type: ignore +import random +from collections import defaultdict + +from tqdm import tqdm + +from selfdrive.car.fw_versions import match_fw_to_car_fuzzy +from selfdrive.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS +from selfdrive.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS +from selfdrive.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS +from selfdrive.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS + + +FWS = {} +FWS.update(TOYOTA_FW_VERSIONS) +FWS.update(HONDA_FW_VERSIONS) +FWS.update(HYUNDAI_FW_VERSIONS) +FWS.update(VW_FW_VERSIONS) + +if __name__ == "__main__": + total = 0 + match = 0 + wrong_match = 0 + confusions = defaultdict(set) + + for _ in tqdm(range(1000)): + for candidate, fws in FWS.items(): + fw_dict = {} + for (tp, addr, subaddr), fw_list in fws.items(): + fw_dict[(addr, subaddr)] = random.choice(fw_list) + + matches = match_fw_to_car_fuzzy(fw_dict, log=False, exclude=candidate) + + total += 1 + if len(matches) == 1: + if list(matches)[0] == candidate: + match += 1 + else: + confusions[candidate] |= matches + wrong_match += 1 + + print() + for candidate, wrong_matches in sorted(confusions.items()): + print(candidate, wrong_matches) + + print() + print(f"Total fuzz cases: {total}") + print(f"Correct matches: {match}") + print(f"Wrong matches: {wrong_match}") + + diff --git a/selfdrive/debug/internal/measure_modeld_packet_drop.py b/selfdrive/debug/internal/measure_modeld_packet_drop.py new file mode 100755 index 00000000000000..6b7f7dbd13f8ac --- /dev/null +++ b/selfdrive/debug/internal/measure_modeld_packet_drop.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +import cereal.messaging as messaging +from typing import Optional + +if __name__ == "__main__": + modeld_sock = messaging.sub_sock("modelV2") + + last_frame_id = None + start_t: Optional[int] = None + frame_cnt = 0 + dropped = 0 + + while True: + m = messaging.recv_one(modeld_sock) + if m is None: + continue + + frame_id = m.modelV2.frameId + t = m.logMonoTime / 1e9 + frame_cnt += 1 + + if start_t is None: + start_t = t + last_frame_id = frame_id + continue + + d_frame = frame_id - last_frame_id + dropped += d_frame - 1 + + expected_num_frames = int((t - start_t) * 20) + frame_drop = 100 * (1 - (expected_num_frames / frame_cnt)) + print(f"Num dropped {dropped}, Drop compared to 20Hz: {frame_drop:.2f}%") + + last_frame_id = frame_id diff --git a/selfdrive/debug/measure_torque_time_to_max.py b/selfdrive/debug/internal/measure_torque_time_to_max.py similarity index 93% rename from selfdrive/debug/measure_torque_time_to_max.py rename to selfdrive/debug/internal/measure_torque_time_to_max.py index e99aeae464b8e9..2cc68df438940e 100755 --- a/selfdrive/debug/measure_torque_time_to_max.py +++ b/selfdrive/debug/internal/measure_torque_time_to_max.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# type: ignore import os import argparse @@ -17,7 +18,7 @@ if args.addr != "127.0.0.1": os.environ["ZMQ"] = "1" - messaging.reset_context() + messaging.context = messaging.Context() poller = messaging.Poller() messaging.sub_sock('can', poller, addr=args.addr) @@ -33,8 +34,7 @@ polld = poller.poll(1000) for sock in polld: msg = sock.receive() - with log.Event.from_bytes(msg) as log_evt: - evt = log_evt + evt = log.Event.from_bytes(msg) for item in evt.can: if item.address == 0xe4 and item.src == 128: diff --git a/selfdrive/debug/internal/power_monitor.py b/selfdrive/debug/internal/power_monitor.py new file mode 100755 index 00000000000000..34d2a12adcb1c6 --- /dev/null +++ b/selfdrive/debug/internal/power_monitor.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +import os +import time +import sys +from datetime import datetime + +def average(avg, sample): + # Weighted avg between existing value and new sample + return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1) + + +if __name__ == '__main__': + start_time = datetime.now() + try: + if len(sys.argv) > 1 and sys.argv[1] == "--charge": + print("not disabling charging") + else: + print("disabling charging") + os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') + + voltage_average = (0., 0) # average, count + current_average = (0., 0) + power_average = (0., 0) + capacity_average = (0., 0) + bat_temp_average = (0., 0) + while 1: + with open("/sys/class/power_supply/bms/voltage_now") as f: + voltage = int(f.read()) / 1e6 # volts + + with open("/sys/class/power_supply/bms/current_now") as f: + current = int(f.read()) / 1e3 # ma + + power = voltage * current + + with open("/sys/class/power_supply/bms/capacity_raw") as f: + capacity = int(f.read()) / 1e2 # percent + + with open("/sys/class/power_supply/bms/temp") as f: + bat_temp = int(f.read()) / 1e1 # celsius + + # compute averages + voltage_average = average(voltage_average, voltage) + current_average = average(current_average, current) + power_average = average(power_average, power) + capacity_average = average(capacity_average, capacity) + bat_temp_average = average(bat_temp_average, bat_temp) + + print(f"{voltage:.2f} volts {current:12.2f} ma {power:12.2f} mW {capacity:8.2f}% battery {bat_temp:8.1f} degC") + time.sleep(0.1) + finally: + stop_time = datetime.now() + print("\n----------------------Average-----------------------------------") + voltage = voltage_average[0] + current = current_average[0] + power = power_average[0] + capacity = capacity_average[0] + bat_temp = bat_temp_average[0] + print(f"{voltage:.2f} volts {current:12.2f} ma {power:12.2f} mW {capacity:8.2f}% battery {bat_temp:8.1f} degC") + print(f" {(stop_time - start_time).total_seconds():.2f} Seconds {voltage_average[1]} samples") + print("----------------------------------------------------------------") + + # re-enable charging + os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') + print("charging enabled\n") diff --git a/selfdrive/debug/internal/qlog_size.py b/selfdrive/debug/internal/qlog_size.py new file mode 100755 index 00000000000000..a98a71181db97e --- /dev/null +++ b/selfdrive/debug/internal/qlog_size.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import argparse +import bz2 +from collections import defaultdict + +import matplotlib.pyplot as plt + +from cereal.services import service_list +from tools.lib.logreader import LogReader +from tools.lib.route import Route + +MIN_SIZE = 0.5 # Percent size of total to show as separate entry + + +def make_pie(msgs, typ): + compressed_length_by_type = {k: len(bz2.compress(b"".join(v))) for k, v in msgs.items()} + + total = sum(compressed_length_by_type.values()) + + sizes = sorted(compressed_length_by_type.items(), key=lambda kv: kv[1]) + + print(f"{typ} - Total {total / 1024:.2f} kB") + for (name, sz) in sizes: + print(f"{name} - {sz / 1024:.2f} kB") + print() + + sizes_large = [(k, sz) for (k, sz) in sizes if sz >= total * MIN_SIZE / 100] + sizes_large += [('other', sum(sz for (_, sz) in sizes if sz < total * MIN_SIZE / 100))] + + labels, sizes = zip(*sizes_large) + + plt.figure() + plt.title(f"{typ}") + plt.pie(sizes, labels=labels, autopct='%1.1f%%') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Check qlog size based on a rlog') + parser.add_argument('route', help='route to use') + parser.add_argument('segment', type=int, help='segment number to use') + args = parser.parse_args() + + r = Route(args.route) + rlog = r.log_paths()[args.segment] + msgs = list(LogReader(rlog)) + + msgs_by_type = defaultdict(list) + for m in msgs: + msgs_by_type[m.which()].append(m.as_builder().to_bytes()) + + qlog_by_type = defaultdict(list) + for name, service in service_list.items(): + if service.decimation is None: + continue + + for i, msg in enumerate(msgs_by_type[name]): + if i % service.decimation == 0: + qlog_by_type[name].append(msg) + + make_pie(msgs_by_type, 'rlog') + make_pie(qlog_by_type, 'qlog') + plt.show() diff --git a/selfdrive/debug/internal/run_paramsd_on_route.py b/selfdrive/debug/internal/run_paramsd_on_route.py new file mode 100755 index 00000000000000..54519e37774e15 --- /dev/null +++ b/selfdrive/debug/internal/run_paramsd_on_route.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +# pylint: skip-file +# flake8: noqa +# type: ignore + +import math +import multiprocessing + +import numpy as np +from tqdm import tqdm + +from selfdrive.locationd.paramsd import ParamsLearner, States +from tools.lib.logreader import LogReader +from tools.lib.route import Route + +ROUTE = "b2f1615665781088|2021-03-14--17-27-47" +PLOT = True + + +def load_segment(segment_name): + print(f"Loading {segment_name}") + if segment_name is None: + return [] + + try: + return list(LogReader(segment_name)) + except ValueError as e: + print(f"Error parsing {segment_name}: {e}") + return [] + + +if __name__ == "__main__": + route = Route(ROUTE) + + msgs = [] + with multiprocessing.Pool(24) as pool: + for d in pool.map(load_segment, route.log_paths()): + msgs += d + + for m in msgs: + if m.which() == 'carParams': + CP = m.carParams + break + + params = { + 'carFingerprint': CP.carFingerprint, + 'steerRatio': CP.steerRatio, + 'stiffnessFactor': 1.0, + 'angleOffsetAverageDeg': 0.0, + } + + for m in msgs: + if m.which() == 'liveParameters': + params['steerRatio'] = m.liveParameters.steerRatio + params['angleOffsetAverageDeg'] = m.liveParameters.angleOffsetAverageDeg + break + + for m in msgs: + if m.which() == 'carState': + last_carstate = m + break + + print(params) + learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) + msgs = [m for m in tqdm(msgs) if m.which() in ('liveLocationKalman', 'carState', 'liveParameters')] + msgs = sorted(msgs, key=lambda m: m.logMonoTime) + + ts = [] + ts_log = [] + results = [] + results_log = [] + for m in tqdm(msgs): + if m.which() == 'carState': + last_carstate = m + + elif m.which() == 'liveLocationKalman': + t = last_carstate.logMonoTime / 1e9 + learner.handle_log(t, 'carState', last_carstate.carState) + + t = m.logMonoTime / 1e9 + learner.handle_log(t, 'liveLocationKalman', m.liveLocationKalman) + + x = learner.kf.x + sr = float(x[States.STEER_RATIO]) + st = float(x[States.STIFFNESS]) + ao_avg = math.degrees(x[States.ANGLE_OFFSET]) + ao = ao_avg + math.degrees(x[States.ANGLE_OFFSET_FAST]) + r = [sr, st, ao_avg, ao] + if any(math.isnan(v) for v in r): + print("NaN", t) + + ts.append(t) + results.append(r) + + elif m.which() == 'liveParameters': + t = m.logMonoTime / 1e9 + mm = m.liveParameters + + r = [mm.steerRatio, mm.stiffnessFactor, mm.angleOffsetAverageDeg, mm.angleOffsetDeg] + if any(math.isnan(v) for v in r): + print("NaN in log", t) + ts_log.append(t) + results_log.append(r) + + results = np.asarray(results) + results_log = np.asarray(results_log) + + if PLOT: + import matplotlib.pyplot as plt + plt.figure() + + plt.subplot(3, 2, 1) + plt.plot(ts, results[:, 0], label='Steer Ratio') + plt.grid() + plt.ylim([0, 20]) + plt.legend() + + plt.subplot(3, 2, 3) + plt.plot(ts, results[:, 1], label='Stiffness') + plt.ylim([0, 2]) + plt.grid() + plt.legend() + + plt.subplot(3, 2, 5) + plt.plot(ts, results[:, 2], label='Angle offset (average)') + plt.plot(ts, results[:, 3], label='Angle offset (instant)') + plt.ylim([-5, 5]) + plt.grid() + plt.legend() + + plt.subplot(3, 2, 2) + plt.plot(ts_log, results_log[:, 0], label='Steer Ratio') + plt.grid() + plt.ylim([0, 20]) + plt.legend() + + plt.subplot(3, 2, 4) + plt.plot(ts_log, results_log[:, 1], label='Stiffness') + plt.ylim([0, 2]) + plt.grid() + plt.legend() + + plt.subplot(3, 2, 6) + plt.plot(ts_log, results_log[:, 2], label='Angle offset (average)') + plt.plot(ts_log, results_log[:, 3], label='Angle offset (instant)') + plt.ylim([-5, 5]) + plt.grid() + plt.legend() + plt.show() + diff --git a/selfdrive/debug/internal/test_paramsd.py b/selfdrive/debug/internal/test_paramsd.py new file mode 100755 index 00000000000000..3d8e422c352fcb --- /dev/null +++ b/selfdrive/debug/internal/test_paramsd.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 +# pylint: skip-file +# type: ignore + +import numpy as np +import math +from tqdm import tqdm +from typing import cast + +import seaborn as sns +import matplotlib.pyplot as plt + + +from selfdrive.car.honda.interface import CarInterface +from selfdrive.car.honda.values import CAR +from selfdrive.controls.lib.vehicle_model import VehicleModel, create_dyn_state_matrices +from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States + +T_SIM = 5 * 60 # s +DT = 0.01 + + +CP = CarInterface.get_params(CAR.CIVIC) +VM = VehicleModel(CP) + +x, y = 0, 0 # m, m +psi = math.radians(0) # rad + +# The state is x = [v, r]^T +# with v lateral speed [m/s], and r rotational speed [rad/s] +state = np.array([[0.0], [0.0]]) + + +ts = np.arange(0, T_SIM, DT) +speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25 + +angle_offsets = math.radians(1.0) * np.ones_like(ts) +angle_offsets[ts > 60] = 0 +steering_angles = cast(np.ndarray, np.radians(5 * np.cos(2 * np.pi * ts / 100.))) + +xs = [] +ys = [] +psis = [] +yaw_rates = [] +speed_ys = [] + + +kf_states = [] +kf_ps = [] + +kf = CarKalman() + +for i, t in tqdm(list(enumerate(ts))): + u = speeds[i] + sa = steering_angles[i] + ao = angle_offsets[i] + + A, B = create_dyn_state_matrices(u, VM) + + state += DT * (A.dot(state) + B.dot(sa + ao)) + + x += u * math.cos(psi) * DT + y += (float(state[0]) * math.sin(psi) + u * math.sin(psi)) * DT + psi += float(state[1]) * DT + + kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [float(state[1])]) + kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[u, float(state[0])]]) + kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [sa]) + kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) + kf.predict(t) + + speed_ys.append(float(state[0])) + yaw_rates.append(float(state[1])) + kf_states.append(kf.x.copy()) + kf_ps.append(kf.P.copy()) + + xs.append(x) + ys.append(y) + psis.append(psi) + + +xs = np.asarray(xs) +ys = np.asarray(ys) +psis = np.asarray(psis) +speed_ys = np.asarray(speed_ys) +kf_states = np.asarray(kf_states) +kf_ps = np.asarray(kf_ps) + + +palette = sns.color_palette() + +def plot_with_bands(ts, state, label, ax, idx=1, converter=None): + mean = kf_states[:, state].flatten() + stds = np.sqrt(kf_ps[:, state, state].flatten()) + + if converter is not None: + mean = converter(mean) + stds = converter(stds) + + sns.lineplot(ts, mean, label=label, ax=ax) + ax.fill_between(ts, mean - stds, mean + stds, alpha=.2, color=palette[idx]) + + +print(kf.x) + +sns.set_context("paper") +f, axes = plt.subplots(6, 1) + +sns.lineplot(ts, np.degrees(steering_angles), label='Steering Angle [deg]', ax=axes[0]) +plot_with_bands(ts, States.STEER_ANGLE, 'Steering Angle kf [deg]', axes[0], converter=np.degrees) + +sns.lineplot(ts, np.degrees(yaw_rates), label='Yaw Rate [deg]', ax=axes[1]) +plot_with_bands(ts, States.YAW_RATE, 'Yaw Rate kf [deg]', axes[1], converter=np.degrees) + +sns.lineplot(ts, np.ones_like(ts) * VM.sR, label='Steer ratio [-]', ax=axes[2]) +plot_with_bands(ts, States.STEER_RATIO, 'Steer ratio kf [-]', axes[2]) +axes[2].set_ylim([10, 20]) + + +sns.lineplot(ts, np.ones_like(ts), label='Tire stiffness[-]', ax=axes[3]) +plot_with_bands(ts, States.STIFFNESS, 'Tire stiffness kf [-]', axes[3]) +axes[3].set_ylim([0.8, 1.2]) + + +sns.lineplot(ts, np.degrees(angle_offsets), label='Angle offset [deg]', ax=axes[4]) +plot_with_bands(ts, States.ANGLE_OFFSET, 'Angle offset kf deg', axes[4], converter=np.degrees) +plot_with_bands(ts, States.ANGLE_OFFSET_FAST, 'Fast Angle offset kf deg', axes[4], converter=np.degrees, idx=2) + +axes[4].set_ylim([-2, 2]) + +sns.lineplot(ts, speeds, ax=axes[5]) + +plt.show() diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index ee0524ec9d4d06..c35afc474bea3a 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -1,10 +1,11 @@ #!/usr/bin/env python3 import argparse -import numpy as np import capnp from collections import defaultdict from cereal.messaging import SubMaster +from common.numpy_fast import mean +from typing import Optional, Dict def cputime_total(ct): return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq @@ -41,15 +42,15 @@ def proc_name(proc): total_times = [0.]*8 busy_times = [0.]*8 - prev_proclog: capnp._DynamicStructReader | None = None - prev_proclog_t: int | None = None + prev_proclog: Optional[capnp._DynamicStructReader] = None + prev_proclog_t: Optional[int] = None while True: sm.update() if sm.updated['deviceState']: t = sm['deviceState'] - last_temp = np.mean(t.cpuTempC) + last_temp = mean(t.cpuTempC) last_mem = t.memoryUsagePercent if sm.updated['procLog']: @@ -72,10 +73,10 @@ def proc_name(proc): total_times = total_times_new[:] busy_times = busy_times_new[:] - print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C") + print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C") if args.cpu and prev_proclog is not None and prev_proclog_t is not None: - procs: dict[str, float] = defaultdict(float) + procs: Dict[str, float] = defaultdict(float) dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9 for proc in m.procs: try: diff --git a/selfdrive/debug/max_lat_accel.py b/selfdrive/debug/max_lat_accel.py deleted file mode 100755 index dc44e8ac40f14b..00000000000000 --- a/selfdrive/debug/max_lat_accel.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import numpy as np -import matplotlib.pyplot as plt -from functools import partial -from tqdm import tqdm -from typing import NamedTuple -from openpilot.tools.lib.logreader import LogReader -from openpilot.selfdrive.locationd.models.pose_kf import EARTH_G - -RLOG_MIN_LAT_ACTIVE = 50 -RLOG_MIN_STEERING_UNPRESSED = 50 -RLOG_MIN_REQUESTING_MAX = 25 # sample many times after reaching max torque - -QLOG_DECIMATION = 10 - - -class Event(NamedTuple): - lateral_accel: float - speed: float - roll: float - timestamp: float # relative to start of route (s) - - -def find_events(lr: LogReader, extrapolate: bool = False, qlog: bool = False) -> list[Event]: - min_lat_active = RLOG_MIN_LAT_ACTIVE // QLOG_DECIMATION if qlog else RLOG_MIN_LAT_ACTIVE - min_steering_unpressed = RLOG_MIN_STEERING_UNPRESSED // QLOG_DECIMATION if qlog else RLOG_MIN_STEERING_UNPRESSED - min_requesting_max = RLOG_MIN_REQUESTING_MAX // QLOG_DECIMATION if qlog else RLOG_MIN_REQUESTING_MAX - - # if we test with driver torque safety, max torque can be slightly noisy - steer_threshold = 0.7 if extrapolate else 0.95 - - events = [] - - # state tracking - steering_unpressed = 0 # frames - requesting_max = 0 # frames - lat_active = 0 # frames - - # current state - curvature = 0 - v_ego = 0 - roll = 0 - out_torque = 0 - - start_ts = 0 - for msg in lr: - if msg.which() == 'carControl': - if start_ts == 0: - start_ts = msg.logMonoTime - - lat_active = lat_active + 1 if msg.carControl.latActive else 0 - - elif msg.which() == 'carOutput': - out_torque = msg.carOutput.actuatorsOutput.torque - requesting_max = requesting_max + 1 if abs(out_torque) > steer_threshold else 0 - - elif msg.which() == 'carState': - steering_unpressed = steering_unpressed + 1 if not msg.carState.steeringPressed else 0 - v_ego = msg.carState.vEgo - - elif msg.which() == 'controlsState': - curvature = msg.controlsState.curvature - - elif msg.which() == 'liveParameters': - roll = msg.liveParameters.roll - - if lat_active > min_lat_active and steering_unpressed > min_steering_unpressed and requesting_max > min_requesting_max: - # TODO: record max lat accel at the end of the event, need to use the past lat accel as overriding can happen before we detect it - requesting_max = 0 - - factor = 1 / abs(out_torque) - current_lateral_accel = (curvature * v_ego ** 2 * factor) - roll * EARTH_G - events.append(Event(current_lateral_accel, v_ego, roll, round((msg.logMonoTime - start_ts) * 1e-9, 2))) - print(events[-1]) - - return events - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Find max lateral acceleration events", - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - - parser.add_argument("route", nargs='+') - parser.add_argument("-e", "--extrapolate", action="store_true", help="Extrapolates max lateral acceleration events linearly. " + - "This option can be far less accurate.") - args = parser.parse_args() - - events = [] - for route in tqdm(args.route): - try: - lr = LogReader(route, sort_by_time=True) - except Exception: - print(f'Skipping {route}') - continue - - qlog = route.endswith('/q') - if qlog: - print('WARNING: Treating route as qlog!') - - print('Finding events...') - events += lr.run_across_segments(8, partial(find_events, extrapolate=args.extrapolate, qlog=qlog), disable_tqdm=True) - - print() - print(f'Found {len(events)} events') - - perc_left_accel = -np.percentile([-ev.lateral_accel for ev in events if ev.lateral_accel < 0] or [0], 90) - perc_right_accel = np.percentile([ev.lateral_accel for ev in events if ev.lateral_accel > 0] or [0], 90) - - CP = lr.first('carParams') - - plt.ion() - plt.clf() - plt.suptitle(f'{CP.carFingerprint} - Max lateral acceleration events') - plt.title(', '.join(args.route)) - plt.scatter([ev.speed for ev in events], [ev.lateral_accel for ev in events], label='max lateral accel events') - - plt.plot([0, 35], [3, 3], c='r', label='ISO 11270 - 3 m/s^2') - plt.plot([0, 35], [-3, -3], c='r') - - plt.plot([0, 35], [perc_left_accel, perc_left_accel], c='g', linestyle='--', label='90th percentile left lateral accel') - plt.plot([0, 35], [perc_right_accel, perc_right_accel], c='#ff7f0e', linestyle='--', label='90th percentile right lateral accel') - plt.text(0.4, float(perc_left_accel + 0.4), f'{perc_left_accel:.2f} m/s^2', verticalalignment='center', fontsize=12) - plt.text(0.4, float(perc_right_accel - 0.4), f'{perc_right_accel:.2f} m/s^2', verticalalignment='center', fontsize=12) - - plt.xlim(0, 35) - plt.ylim(-5, 5) - plt.xlabel('speed (m/s)') - plt.ylabel('lateral acceleration (m/s^2)') - plt.legend() - plt.show(block=True) diff --git a/selfdrive/debug/print_docs_diff.py b/selfdrive/debug/print_docs_diff.py index 388acf3af58020..b80428645873d3 100755 --- a/selfdrive/debug/print_docs_diff.py +++ b/selfdrive/debug/print_docs_diff.py @@ -4,20 +4,17 @@ import difflib import pickle -from opendbc.car.docs import get_all_car_docs -from opendbc.car.docs_definitions import Column +from selfdrive.car.docs import get_all_car_info +from selfdrive.car.docs_definitions import Column FOOTNOTE_TAG = "{}" -STAR_ICON = '' -VIDEO_ICON = '' + \ - '' +STAR_ICON = '' COLUMNS = "|" + "|".join([column.value for column in Column]) + "|" COLUMN_HEADER = "|---|---|---|{}|".format("|".join([":---:"] * (len(Column) - 3))) ARROW_SYMBOL = "➡️" -def load_base_car_docs(path): +def load_base_car_info(path): with open(path, "rb") as f: return pickle.load(f) @@ -42,8 +39,8 @@ def match_cars(base_cars, new_cars): def build_column_diff(base_car, new_car): row_builder = [] for column in Column: - base_column = base_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) - new_column = new_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + base_column = base_car.get_column(column, STAR_ICON, FOOTNOTE_TAG) + new_column = new_car.get_column(column, STAR_ICON, FOOTNOTE_TAG) if base_column != new_column: row_builder.append(f"{base_column} {ARROW_SYMBOL} {new_column}") @@ -57,31 +54,31 @@ def format_row(builder): return "|" + "|".join(builder) + "|" -def print_car_docs_diff(path): - base_car_docs = defaultdict(list) - new_car_docs = defaultdict(list) +def print_car_info_diff(path): + base_car_info = defaultdict(list) + new_car_info = defaultdict(list) - for car in load_base_car_docs(path): - base_car_docs[car.car_fingerprint].append(car) - for car in get_all_car_docs(): - new_car_docs[car.car_fingerprint].append(car) + for car in load_base_car_info(path): + base_car_info[car.car_fingerprint].append(car) + for car in get_all_car_info(): + new_car_info[car.car_fingerprint].append(car) # Add new platforms to base cars so we can detect additions and removals in one pass - base_car_docs.update({car: [] for car in new_car_docs if car not in base_car_docs}) + base_car_info.update({car: [] for car in new_car_info if car not in base_car_info}) changes = defaultdict(list) - for base_car_model, base_cars in base_car_docs.items(): + for base_car_model, base_cars in base_car_info.items(): # Match car info changes, and get additions and removals - new_cars = new_car_docs[base_car_model] + new_cars = new_car_info[base_car_model] car_changes, car_additions, car_removals = match_cars(base_cars, new_cars) # Removals - for car_docs in car_removals: - changes["removals"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + for car_info in car_removals: + changes["removals"].append(format_row([car_info.get_column(column, STAR_ICON, FOOTNOTE_TAG) for column in Column])) # Additions - for car_docs in car_additions: - changes["additions"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + for car_info in car_additions: + changes["additions"].append(format_row([car_info.get_column(column, STAR_ICON, FOOTNOTE_TAG) for column in Column])) for new_car, base_car in car_changes: # Column changes @@ -101,8 +98,7 @@ def print_car_docs_diff(path): if any(len(c) for c in changes.values()): markdown_builder = ["### ⚠️ This PR makes changes to [CARS.md](../blob/master/docs/CARS.md) ⚠️"] - for title, category in (("## 🔀 Column Changes", "column"), ("## ❌ Removed", "removals"), - ("## ➕ Added", "additions"), ("## 📖 Detail Sentence Changes", "detail")): + for title, category in (("## 🔀 Column Changes", "column"), ("## ❌ Removed", "removals"), ("## ➕ Added", "additions"), ("## 📖 Detail Sentence Changes", "detail")): if len(changes[category]): markdown_builder.append(title) if category not in ("detail",): @@ -117,4 +113,4 @@ def print_car_docs_diff(path): parser = argparse.ArgumentParser() parser.add_argument("--path", required=True) args = parser.parse_args() - print_car_docs_diff(args.path) + print_car_info_diff(args.path) diff --git a/selfdrive/debug/print_flags.py b/selfdrive/debug/print_flags.py deleted file mode 100755 index a50d929f54a5de..00000000000000 --- a/selfdrive/debug/print_flags.py +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env python3 -from opendbc.car.values import BRANDS - -for brand in BRANDS: - all_flags = set() - for platform in brand: - if platform.config.flags != 0: - all_flags |= set(platform.config.flags) - - if len(all_flags): - print(brand.__module__.split('.')[-2].upper() + ':') - for flag in sorted(all_flags): - print(f' {flag.name:<24}:', {platform.name for platform in brand.with_flags(flag)}) - print() diff --git a/tools/profiling/clpeak/.gitignore b/selfdrive/debug/profiling/clpeak/.gitignore similarity index 100% rename from tools/profiling/clpeak/.gitignore rename to selfdrive/debug/profiling/clpeak/.gitignore diff --git a/selfdrive/debug/profiling/clpeak/build.sh b/selfdrive/debug/profiling/clpeak/build.sh new file mode 100755 index 00000000000000..1dcb05cbbc8508 --- /dev/null +++ b/selfdrive/debug/profiling/clpeak/build.sh @@ -0,0 +1,22 @@ +#!/usr/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd $DIR + +if [ ! -d "$DIR/clpeak" ]; then + git clone https://github.com/krrishnarraj/clpeak.git + + cd clpeak + git fetch + git checkout ec2d3e70e1abc7738b81f9277c7af79d89b2133b + git reset --hard origin/master + git submodule update --init --recursive --remote + + git apply ../run_continuously.patch +fi + +cd clpeak +mkdir build || true +cd build +cmake .. +cmake --build . diff --git a/selfdrive/debug/profiling/clpeak/clpeak3 b/selfdrive/debug/profiling/clpeak/clpeak3 new file mode 100755 index 00000000000000..4b3c4c130e7a78 Binary files /dev/null and b/selfdrive/debug/profiling/clpeak/clpeak3 differ diff --git a/tools/profiling/clpeak/no_print.patch b/selfdrive/debug/profiling/clpeak/no_print.patch similarity index 100% rename from tools/profiling/clpeak/no_print.patch rename to selfdrive/debug/profiling/clpeak/no_print.patch diff --git a/tools/profiling/clpeak/run_continuously.patch b/selfdrive/debug/profiling/clpeak/run_continuously.patch similarity index 100% rename from tools/profiling/clpeak/run_continuously.patch rename to selfdrive/debug/profiling/clpeak/run_continuously.patch diff --git a/selfdrive/debug/profiling/ftrace.sh b/selfdrive/debug/profiling/ftrace.sh new file mode 100755 index 00000000000000..fe91a3c0d905b1 --- /dev/null +++ b/selfdrive/debug/profiling/ftrace.sh @@ -0,0 +1,23 @@ +#!/usr/bin/bash +set -e + +cd /sys/kernel/tracing + +echo 1 > tracing_on +echo boot > trace_clock +echo 1000 > buffer_size_kb + +# /sys/kernel/tracing/available_events +echo 1 > events/irq/enable +echo 1 > events/sched/enable +echo 1 > events/kgsl/enable +echo 1 > events/camera/enable +echo 1 > events/workqueue/enable + +echo > trace +sleep 5 +echo 0 > tracing_on + +cp trace /tmp/trace +chown comma: /tmp/trace +echo /tmp/trace diff --git a/tools/profiling/palanteer/.gitignore b/selfdrive/debug/profiling/palanteer/.gitignore similarity index 100% rename from tools/profiling/palanteer/.gitignore rename to selfdrive/debug/profiling/palanteer/.gitignore diff --git a/selfdrive/debug/profiling/palanteer/setup.sh b/selfdrive/debug/profiling/palanteer/setup.sh new file mode 100755 index 00000000000000..e912a9367fa234 --- /dev/null +++ b/selfdrive/debug/profiling/palanteer/setup.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +cd $DIR + +if [ ! -d palanteer ]; then + git clone https://github.com/dfeneyrou/palanteer + pip install wheel + sudo apt install libunwind-dev libdw-dev +fi + +cd palanteer +git pull + +mkdir -p build +cd build +cmake .. -DCMAKE_BUILD_TYPE=Release +make -j$(nproc) + +pip install --force-reinstall python/dist/palanteer*.whl + +cp bin/palanteer $DIR/viewer diff --git a/tools/profiling/perfetto/.gitignore b/selfdrive/debug/profiling/perfetto/.gitignore similarity index 100% rename from tools/profiling/perfetto/.gitignore rename to selfdrive/debug/profiling/perfetto/.gitignore diff --git a/selfdrive/debug/profiling/perfetto/build.sh b/selfdrive/debug/profiling/perfetto/build.sh new file mode 100755 index 00000000000000..448c1d04aeb856 --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/build.sh @@ -0,0 +1,11 @@ +#!/usr/bin/bash + +if [ ! -d perfetto ]; then + git clone https://android.googlesource.com/platform/external/perfetto/ +fi + +cd perfetto + +tools/install-build-deps --linux-arm +tools/gn gen --args='is_debug=false target_os="linux" target_cpu="arm64"' out/linux +tools/ninja -C out/linux tracebox traced traced_probes perfetto diff --git a/tools/profiling/perfetto/copy.sh b/selfdrive/debug/profiling/perfetto/copy.sh similarity index 87% rename from tools/profiling/perfetto/copy.sh rename to selfdrive/debug/profiling/perfetto/copy.sh index b91d49a80b65a8..8deca9a6e73910 100755 --- a/tools/profiling/perfetto/copy.sh +++ b/selfdrive/debug/profiling/perfetto/copy.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash DEST=tici:/data/openpilot/selfdrive/debug/profiling/perfetto diff --git a/tools/profiling/perfetto/record.sh b/selfdrive/debug/profiling/perfetto/record.sh similarity index 89% rename from tools/profiling/perfetto/record.sh rename to selfdrive/debug/profiling/perfetto/record.sh index 9715a7e7e97cc0..99f2168771cc88 100755 --- a/tools/profiling/perfetto/record.sh +++ b/selfdrive/debug/profiling/perfetto/record.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" cd $DIR diff --git a/tools/profiling/perfetto/server.sh b/selfdrive/debug/profiling/perfetto/server.sh similarity index 84% rename from tools/profiling/perfetto/server.sh rename to selfdrive/debug/profiling/perfetto/server.sh index 103ad9bd848db0..19958c653bb761 100755 --- a/tools/profiling/perfetto/server.sh +++ b/selfdrive/debug/profiling/perfetto/server.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash curl -LO https://get.perfetto.dev/trace_processor chmod +x ./trace_processor diff --git a/tools/profiling/perfetto/traces.sh b/selfdrive/debug/profiling/perfetto/traces.sh similarity index 86% rename from tools/profiling/perfetto/traces.sh rename to selfdrive/debug/profiling/perfetto/traces.sh index 17ca89da72bc3e..489b042377c33c 100755 --- a/tools/profiling/perfetto/traces.sh +++ b/selfdrive/debug/profiling/perfetto/traces.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash DEST=tici:/data/openpilot/selfdrive/debug/profiling/perfetto diff --git a/tools/profiling/py-spy/profile.sh b/selfdrive/debug/profiling/py-spy/profile.sh similarity index 100% rename from tools/profiling/py-spy/profile.sh rename to selfdrive/debug/profiling/py-spy/profile.sh diff --git a/selfdrive/debug/profiling/simpleperf/bin/android/arm64/simpleperf b/selfdrive/debug/profiling/simpleperf/bin/android/arm64/simpleperf new file mode 100755 index 00000000000000..4df1f31fcdadbf Binary files /dev/null and b/selfdrive/debug/profiling/simpleperf/bin/android/arm64/simpleperf differ diff --git a/selfdrive/debug/profiling/simpleperf/eon_perf.sh b/selfdrive/debug/profiling/simpleperf/eon_perf.sh new file mode 100755 index 00000000000000..385af26ba36329 --- /dev/null +++ b/selfdrive/debug/profiling/simpleperf/eon_perf.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -e + +cd "$( dirname "${BASH_SOURCE[0]}" )" + +ssh "$1" '$HOME/one/external/simpleperf/bin/android/arm64/simpleperf record --call-graph fp -a --duration 10 -o /tmp/perf.data' +scp "$1":/tmp/perf.data /tmp/perf.data +python2 report_html.py -i /tmp/perf.data -o /tmp/report.html diff --git a/selfdrive/debug/profiling/simpleperf/get.txt b/selfdrive/debug/profiling/simpleperf/get.txt new file mode 100644 index 00000000000000..8e57e6e6cec33e --- /dev/null +++ b/selfdrive/debug/profiling/simpleperf/get.txt @@ -0,0 +1,3 @@ +git clone https://android.googlesource.com/platform/prebuilts/simpleperf +git reset --hard 311a9d2cd27841498fc90a0b26a755deb47e7ebd +cp -r report_html.* simpleperf_report_lib.py utils.py inferno lib ~/one/external/simpleperf/ diff --git a/selfdrive/debug/profiling/snapdragon/.gitignore b/selfdrive/debug/profiling/snapdragon/.gitignore new file mode 100644 index 00000000000000..5d3033efb3db39 --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/.gitignore @@ -0,0 +1 @@ +SnapdragonProfiler/ diff --git a/selfdrive/debug/profiling/snapdragon/README b/selfdrive/debug/profiling/snapdragon/README new file mode 100644 index 00000000000000..ee826b413a635d --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/README @@ -0,0 +1,7 @@ +snapdragon profiler +-------- + +* download from https://developer.qualcomm.com/software/snapdragon-profiler +* unzip to selfdrive/debug/profiling/snapdragon/SnapdragonProfiler +* run ./setup-agnos.sh +* run selfdrive/debug/adb.sh on device diff --git a/tools/profiling/snapdragon/setup-agnos.sh b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh similarity index 86% rename from tools/profiling/snapdragon/setup-agnos.sh rename to selfdrive/debug/profiling/snapdragon/setup-agnos.sh index 9a781bf3ce995e..f036ca211172ef 100755 --- a/tools/profiling/snapdragon/setup-agnos.sh +++ b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/bin/bash # TODO: there's probably a better way to do this diff --git a/selfdrive/debug/qlog_size.py b/selfdrive/debug/qlog_size.py deleted file mode 100755 index 2b54cfeebf7297..00000000000000 --- a/selfdrive/debug/qlog_size.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import zstandard as zstd -from collections import defaultdict - -import matplotlib.pyplot as plt - -from cereal.services import SERVICE_LIST -from openpilot.common.utils import LOG_COMPRESSION_LEVEL -from openpilot.tools.lib.logreader import LogReader -from tqdm import tqdm - -MIN_SIZE = 0.5 # Percent size of total to show as separate entry - - -def make_pie(msgs, typ): - msgs_by_type = defaultdict(list) - for m in msgs: - msgs_by_type[m.which()].append(m.as_builder().to_bytes()) - - total = len(zstd.compress(b"".join([m.as_builder().to_bytes() for m in msgs]), LOG_COMPRESSION_LEVEL)) - uncompressed_total = len(b"".join([m.as_builder().to_bytes() for m in msgs])) - - length_by_type = {k: len(b"".join(v)) for k, v in msgs_by_type.items()} - # calculate compressed size by calculating diff when removed from the segment - compressed_length_by_type = {} - for k in tqdm(msgs_by_type.keys(), desc="Compressing"): - compressed_length_by_type[k] = total - len(zstd.compress(b"".join([m.as_builder().to_bytes() for m in msgs if m.which() != k]), LOG_COMPRESSION_LEVEL)) - - sizes = sorted(compressed_length_by_type.items(), key=lambda kv: kv[1]) - - print("name - comp. size (uncomp. size)") - for (name, sz) in sizes: - print(f"{name:<22} - {sz / 1024:.2f} kB ({length_by_type[name] / 1024:.2f} kB)") - print() - print(f"{typ} - Real total {total / 1024:.2f} kB") - print(f"{typ} - Breakdown total {sum(compressed_length_by_type.values()) / 1024:.2f} kB") - print(f"{typ} - Uncompressed total {uncompressed_total / 1024 / 1024:.2f} MB") - - sizes_large = [(k, sz) for (k, sz) in sizes if sz >= total * MIN_SIZE / 100] - sizes_large += [('other', sum(sz for (_, sz) in sizes if sz < total * MIN_SIZE / 100))] - - labels, sizes = zip(*sizes_large, strict=True) - - plt.figure() - plt.title(f"{typ}") - plt.pie(sizes, labels=labels, autopct='%1.1f%%') - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='View log size breakdown by message type') - parser.add_argument('route', help='route to use') - parser.add_argument('--as-qlog', action='store_true', help='decimate rlog using latest decimation factors') - args = parser.parse_args() - - msgs = list(LogReader(args.route)) - - if args.as_qlog: - new_msgs = [] - msg_cnts: dict[str, int] = defaultdict(int) - for msg in msgs: - msg_which = msg.which() - if msg.which() in ("initData", "sentinel"): - new_msgs.append(msg) - continue - - if msg_which not in SERVICE_LIST: - continue - - decimation = SERVICE_LIST[msg_which].decimation - if decimation is not None and msg_cnts[msg_which] % decimation == 0: - new_msgs.append(msg) - msg_cnts[msg_which] += 1 - - msgs = new_msgs - - make_pie(msgs, 'qlog') - plt.show() diff --git a/selfdrive/debug/read_dtc_status.py b/selfdrive/debug/read_dtc_status.py index f9dad535771f6c..9ad5563975f70e 100755 --- a/selfdrive/debug/read_dtc_status.py +++ b/selfdrive/debug/read_dtc_status.py @@ -2,10 +2,9 @@ import sys import argparse from subprocess import check_output, CalledProcessError -from opendbc.car.carlog import carlog -from opendbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names -from opendbc.car.structs import CarParams from panda import Panda +from panda.python.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE +from panda.python.uds import get_dtc_num_as_str, get_dtc_status_names parser = argparse.ArgumentParser(description="read DTC status") parser.add_argument("addr", type=lambda x: int(x,0)) @@ -13,20 +12,17 @@ parser.add_argument('--debug', action='store_true') args = parser.parse_args() -if args.debug: - carlog.setLevel('DEBUG') - try: - check_output(["pidof", "pandad"]) - print("pandad is running, please kill openpilot before running this script! (aborted)") + check_output(["pidof", "boardd"]) + print("boardd is running, please kill openpilot before running this script! (aborted)") sys.exit(1) except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (pandad not running) + if e.returncode != 1: # 1 == no process found (boardd not running) raise e panda = Panda() -panda.set_safety_mode(CarParams.SafetyModel.elm327) -uds_client = UdsClient(panda, args.addr, bus=args.bus) +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) print("extended diagnostic session ...") uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) print("read diagnostic codes ...") diff --git a/selfdrive/debug/run_process_on_route.py b/selfdrive/debug/run_process_on_route.py index d6743fedcb4095..63b0733bba1cfc 100755 --- a/selfdrive/debug/run_process_on_route.py +++ b/selfdrive/debug/run_process_on_route.py @@ -1,32 +1,31 @@ #!/usr/bin/env python3 -import argparse -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, replay_process -from openpilot.selfdrive.test.process_replay.test_processes import EXCLUDED_PROCS -from openpilot.tools.lib.logreader import LogReader, save_log +import argparse -ALLOW_PROCS = {c.proc_name for c in CONFIGS} +from selfdrive.test.process_replay.compare_logs import save_log +from selfdrive.test.process_replay.process_replay import CONFIGS, replay_process +from tools.lib.logreader import MultiLogIterator +from tools.lib.route import Route if __name__ == "__main__": parser = argparse.ArgumentParser(description="Run process on route and create new logs", formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument("route", help="The route name to use") - parser.add_argument("--fingerprint", help="The fingerprint to use") - parser.add_argument("--whitelist-procs", nargs='*', default=ALLOW_PROCS, help="Whitelist given processes (e.g. controlsd)") - parser.add_argument("--blacklist-procs", nargs='*', default=EXCLUDED_PROCS, help="Blacklist given processes (e.g. controlsd)") + parser.add_argument("process", help="The process to run") args = parser.parse_args() - allowed_procs = set(args.whitelist_procs) - set(args.blacklist_procs) - cfgs = [c for c in CONFIGS if c.proc_name in allowed_procs] + cfg = [c for c in CONFIGS if c.proc_name == args.process][0] + + route = Route(args.route) + lr = MultiLogIterator(route.log_paths()) + inputs = list(lr) - inputs = list(LogReader(args.route)) - outputs = replay_process(cfgs, inputs, fingerprint=args.fingerprint) + outputs = replay_process(cfg, inputs) # Remove message generated by the process under test and merge in the new messages produces = {o.which() for o in outputs} inputs = [i for i in inputs if i.which() not in produces] - outputs = sorted(inputs + outputs, key=lambda x: x.logMonoTime) + outputs = sorted(inputs + outputs, key=lambda x: x.logMonoTime) # type: ignore - fn = f"{args.route.replace('/', '_')}_{'_'.join(allowed_procs)}.zst" - print(f"Saving log to {fn}") + fn = f"{args.route}_{args.process}.bz2" save_log(fn, outputs) diff --git a/selfdrive/debug/set_car_params.py b/selfdrive/debug/set_car_params.py index aec30b4d74649f..24258db9f20523 100755 --- a/selfdrive/debug/set_car_params.py +++ b/selfdrive/debug/set_car_params.py @@ -2,9 +2,9 @@ import sys from cereal import car -from openpilot.common.params import Params -from openpilot.tools.lib.route import Route -from openpilot.tools.lib.logreader import LogReader +from common.params import Params +from tools.lib.route import Route +from tools.lib.logreader import LogReader if __name__ == "__main__": CP = None @@ -15,7 +15,7 @@ else: CP = car.CarParams.new_message() CP.openpilotLongitudinalControl = True - CP.alphaLongitudinalAvailable = False + CP.experimentalLongitudinalAvailable = False cp_bytes = CP.to_bytes() for p in ("CarParams", "CarParamsCache", "CarParamsPersistent"): diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py new file mode 100755 index 00000000000000..d5199b2a9e3f3e --- /dev/null +++ b/selfdrive/debug/show_matching_cars.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars +import cereal.messaging as messaging + + +# rav4 2019 and corolla tss2 +fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8} + +candidate_cars = all_legacy_fingerprint_cars() + + +for addr, l in fingerprint.items(): + dat = messaging.new_message('can', 1) + + msg = dat.can[0] + msg.address = addr + msg.dat = " " * l + + candidate_cars = eliminate_incompatible_cars(msg, candidate_cars) + print(candidate_cars) diff --git a/selfdrive/debug/test_car_model.py b/selfdrive/debug/test_car_model.py new file mode 100755 index 00000000000000..4de5b267628810 --- /dev/null +++ b/selfdrive/debug/test_car_model.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +import argparse +import sys +from typing import List, Tuple +import unittest + +from selfdrive.car.tests.routes import CarTestRoute +from selfdrive.car.tests.test_models import TestCarModel + + +def create_test_models_suite(routes: List[Tuple[str, CarTestRoute]], ci=False) -> unittest.TestSuite: + test_suite = unittest.TestSuite() + for car_model, test_route in routes: + # create new test case and discover tests + test_case_args = {"car_model": car_model, "test_route": test_route, "ci": ci} + CarModelTestCase = type("CarModelTestCase", (TestCarModel,), test_case_args) + test_suite.addTest(unittest.TestLoader().loadTestsFromTestCase(CarModelTestCase)) + return test_suite + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Test any route against common issues with a new car port. " + + "Uses selfdrive/car/tests/test_models.py") + parser.add_argument("route", help="Specify route to run tests on") + parser.add_argument("--car", help="Specify car model for test route") + parser.add_argument("--segment", type=int, nargs="?", help="Specify segment of route to test") + parser.add_argument("--ci", action="store_true", help="Attempt to get logs using openpilotci, need to specify car") + args = parser.parse_args() + if len(sys.argv) == 1: + parser.print_help() + sys.exit() + + test_route = CarTestRoute(args.route, args.car, segment=args.segment) + test_suite = create_test_models_suite([(args.car, test_route)], ci=args.ci) + + unittest.TextTestRunner().run(test_suite) diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index ab539e4feba49f..595e25e8c3242f 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -1,22 +1,29 @@ #!/usr/bin/env python3 +# type: ignore from collections import defaultdict import argparse import os import traceback from tqdm import tqdm -from opendbc.car.car_helpers import interface_names -from opendbc.car.fingerprints import MIGRATION -from opendbc.car.fw_versions import VERSIONS, match_fw_to_car -from openpilot.tools.lib.logreader import LogReader, ReadMode -from openpilot.tools.lib.route import SegmentRange +from tools.lib.logreader import LogReader +from tools.lib.route import Route +from selfdrive.car.interfaces import get_interface_attr +from selfdrive.car.car_helpers import interface_names +from selfdrive.car.fw_versions import match_fw_to_car NO_API = "NO_API" in os.environ +VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) SUPPORTED_BRANDS = VERSIONS.keys() SUPPORTED_CARS = [brand for brand in SUPPORTED_BRANDS for brand in interface_names[brand]] UNKNOWN_BRAND = "unknown" +try: + from xx.pipeline.c.CarState import migration +except ImportError: + migration = {} + if __name__ == "__main__": parser = argparse.ArgumentParser(description='Run FW fingerprint on Qlog of route or list of routes') parser.add_argument('route', help='Route or file with list of routes') @@ -39,35 +46,41 @@ dongles = [] for route in tqdm(routes): - sr = SegmentRange(route) - dongle_id = sr.dongle_id + route = route.rstrip() + dongle_id, time = route.split('|') if dongle_id in dongles: continue - if sr.slice == '' and sr.selector is None: - route += '/0' + if NO_API: + qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2" + else: + route = Route(route) + qlog_path = next((p for p in route.qlog_paths() if p is not None), None) - lr = LogReader(route, default_mode=ReadMode.QLOG) + if qlog_path is None: + continue try: + lr = LogReader(qlog_path) dongles.append(dongle_id) CP = None for msg in lr: if msg.which() == "pandaStates": - if msg.pandaStates[0].pandaType in ('unknown', 'whitePanda', 'greyPanda', 'pedal'): + if msg.pandaStates[0].pandaType not in ('uno', 'blackPanda', 'dos'): print("wrong panda type") break elif msg.which() == "carParams": CP = msg.carParams - car_fw = [fw for fw in CP.carFw if not fw.logging] + car_fw = CP.carFw if len(car_fw) == 0: - print("WARNING: no fw") + print("no fw") + break live_fingerprint = CP.carFingerprint - live_fingerprint = MIGRATION.get(live_fingerprint, live_fingerprint) + live_fingerprint = migration.get(live_fingerprint, live_fingerprint) if args.car is not None: live_fingerprint = args.car @@ -76,8 +89,8 @@ print("not in supported cars") break - _, exact_matches = match_fw_to_car(car_fw, CP.carVin, allow_exact=True, allow_fuzzy=False) - _, fuzzy_matches = match_fw_to_car(car_fw, CP.carVin, allow_exact=False, allow_fuzzy=True) + _, exact_matches = match_fw_to_car(car_fw, allow_exact=True, allow_fuzzy=False) + _, fuzzy_matches = match_fw_to_car(car_fw, allow_exact=False, allow_fuzzy=True) if (len(exact_matches) == 1) and (list(exact_matches)[0] == live_fingerprint): good_exact += 1 @@ -87,20 +100,21 @@ if len(fuzzy_matches) == 1: if list(fuzzy_matches)[0] != live_fingerprint: wrong_fuzzy += 1 + print(f"{dongle_id}|{time}") print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) else: good_fuzzy += 1 break + print(f"{dongle_id}|{time}") print("Old style:", live_fingerprint, "Vin", CP.carVin) print("New style (exact):", exact_matches) print("New style (fuzzy):", fuzzy_matches) - padding = max([len(fw.brand or UNKNOWN_BRAND) for fw in car_fw] + [0]) + padding = max([len(fw.brand or UNKNOWN_BRAND) for fw in car_fw]) for version in sorted(car_fw, key=lambda fw: fw.brand): subaddr = None if version.subAddress == 0 else hex(version.subAddress) - print(f" Brand: {version.brand or UNKNOWN_BRAND:{padding}}, bus: {version.bus} - " + - f"(Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}],") + print(f" Brand: {version.brand or UNKNOWN_BRAND:{padding}}, bus: {version.bus} - (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}],") print("Mismatches") found = False diff --git a/selfdrive/debug/touch_replay.py b/selfdrive/debug/touch_replay.py deleted file mode 100755 index 6e5ecbbe5b59e8..00000000000000 --- a/selfdrive/debug/touch_replay.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python3 -import argparse - -import numpy as np -import matplotlib.pyplot as plt - -from openpilot.tools.lib.logreader import LogReader - -if __name__ == '__main__': - parser = argparse.ArgumentParser() - parser.add_argument('--width', default=2160, type=int) - parser.add_argument('--height', default=1080, type=int) - parser.add_argument('--route', default='rlog', type=str) - args = parser.parse_args() - - w = args.width - h = args.height - route = args.route - - fingers = [[-1, -1]] * 5 - touch_points = [] - current_slot = 0 - - lr = list(LogReader(route)) - for msg in lr: - if msg.which() == 'touch': - for event in msg.touch: - if event.type == 3 and event.code == 47: - current_slot = event.value - elif event.type == 3 and event.code == 57 and event.value == -1: - fingers[current_slot] = [-1, -1] - elif event.type == 3 and event.code == 53: - fingers[current_slot][1] = event.value - if fingers[current_slot][0] != -1: - touch_points.append(fingers[current_slot].copy()) - elif event.type == 3 and event.code == 54: - fingers[current_slot][0] = w - event.value - if fingers[current_slot][1] != -1: - touch_points.append(fingers[current_slot].copy()) - - if not touch_points: - print(f'No touch events found for {route}') - quit() - - unique_points, counts = np.unique(touch_points, axis=0, return_counts=True) - - plt.figure(figsize=(10, 3)) - plt.scatter(unique_points[:, 0], unique_points[:, 1], c=counts, s=counts * 20, edgecolors='red') - plt.colorbar() - plt.title(f'Touches for {route}') - plt.xlim(0, w) - plt.ylim(0, h) - plt.grid(True) - plt.show() diff --git a/selfdrive/debug/car/toyota_eps_factor.py b/selfdrive/debug/toyota_eps_factor.py similarity index 84% rename from selfdrive/debug/car/toyota_eps_factor.py rename to selfdrive/debug/toyota_eps_factor.py index f35a86e1ad18b0..0a459bb7199782 100755 --- a/selfdrive/debug/car/toyota_eps_factor.py +++ b/selfdrive/debug/toyota_eps_factor.py @@ -2,10 +2,11 @@ import sys import numpy as np import matplotlib.pyplot as plt -from sklearn import linear_model -from opendbc.car.toyota.values import STEER_THRESHOLD +from sklearn import linear_model # pylint: disable=import-error +from selfdrive.car.toyota.values import STEER_THRESHOLD -from openpilot.tools.lib.logreader import LogReader +from tools.lib.route import Route +from tools.lib.logreader import MultiLogIterator MIN_SAMPLES = 30 * 100 @@ -57,6 +58,7 @@ def get_eps_factor(lr, plot=False): if __name__ == "__main__": - lr = LogReader(sys.argv[1]) + r = Route(sys.argv[1]) + lr = MultiLogIterator(r.log_paths()) n = get_eps_factor(lr, plot="--plot" in sys.argv) print("EPS torque factor: ", n) diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index 8e75769a85ec64..93d901f7c978ba 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -2,15 +2,14 @@ import time from cereal import car, log, messaging -from openpilot.common.params import Params -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.hardware import HARDWARE +from common.params import Params +from selfdrive.manager.process_config import managed_processes if __name__ == "__main__": - CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10) + CP = car.CarParams(notCar=True) Params().put("CarParams", CP.to_bytes()) - procs = ['camerad', 'ui', 'modeld', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd'] + procs = ['camerad', 'ui', 'modeld', 'calibrationd'] for p in procs: managed_processes[p].start() @@ -18,7 +17,6 @@ msgs = {s: messaging.new_message(s) for s in ['controlsState', 'deviceState', 'carParams']} msgs['deviceState'].deviceState.started = True - msgs['deviceState'].deviceState.deviceType = HARDWARE.get_device_type() msgs['carParams'].carParams.openpilotLongitudinalControl = True msgs['pandaStates'] = messaging.new_message('pandaStates', 1) diff --git a/selfdrive/debug/car/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py similarity index 84% rename from selfdrive/debug/car/vw_mqb_config.py rename to selfdrive/debug/vw_mqb_config.py index 13ee7786d9b411..c1068bf067797d 100755 --- a/selfdrive/debug/car/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -3,12 +3,9 @@ import argparse import struct from enum import IntEnum -from opendbc.car.carlog import carlog -from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ - DATA_IDENTIFIER_TYPE, ACCESS_TYPE -from opendbc.car.structs import CarParams from panda import Panda -from datetime import date +from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ + DATA_IDENTIFIER_TYPE, ACCESS_TYPE # TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks class VOLKSWAGEN_DATA_IDENTIFIER_TYPE(IntEnum): @@ -35,12 +32,10 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): parser.add_argument("action", choices={"show", "enable", "disable"}, help="show or modify current EPS HCA config") args = parser.parse_args() - if args.debug: - carlog.setLevel('DEBUG') - panda = Panda() - panda.set_safety_mode(CarParams.SafetyModel.elm327) - uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, 1, timeout=0.2) + panda.set_safety_mode(Panda.SAFETY_ELM327) + bus = 1 if panda.has_obd() else 0 + uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2, debug=args.debug) try: uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) @@ -54,8 +49,8 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): sw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER).decode("utf-8") sw_ver = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER).decode("utf-8") component = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.SYSTEM_NAME_OR_ENGINE_TYPE).decode("utf-8") - odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8").rstrip('\x00') - current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) + odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8") + current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) # type: ignore coding_text = current_coding.hex() print("\nEPS diagnostic data\n") @@ -75,14 +70,14 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_length = len(current_coding) - # EPS_MQB_ZFLS - if odx_file in ("EV_SteerAssisMQB", "EV_SteerAssisMNB"): - coding_variant = "ZFLS" + # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) + if odx_file == "EV_SteerAssisMQB\x00": + coding_variant = "ZF" coding_byte = 0 coding_bit = 4 - # MQB_PP_APA, MQB_VWBS_GEN2 - elif odx_file in ("EV_SteerAssisVWBSMQBA", "EV_SteerAssisVWBSMQBGen2"): + # APA racks (MQB_PP_APA) have a different coding layout + elif odx_file == "EV_SteerAssisVWBSMQBA\x00\x00\x00\x00": coding_variant = "APA" coding_byte = 3 coding_bit = 0 @@ -116,8 +111,8 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant in ("ZFLS", "APA")) - # ZFLS EPS config coding length can be anywhere from 1 to 4 bytes, but the + assert(coding_variant in ("ZF", "APA")) + # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the # bit we care about is always in the same place in the first byte if args.action == "enable": new_byte = current_coding_array[coding_byte] | (1 << coding_bit) @@ -126,12 +121,11 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): new_coding = current_coding[0:coding_byte] + new_byte.to_bytes(1, "little") + current_coding[coding_byte+1:] try: - seed = uds_client.security_access(ACCESS_TYPE_LEVEL_1.REQUEST_SEED) + seed = uds_client.security_access(ACCESS_TYPE_LEVEL_1.REQUEST_SEED) # type: ignore key = struct.unpack("!I", seed)[0] + 28183 # yeah, it's like that - uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) + uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) # type: ignore except (NegativeResponseError, MessageTimeoutError): print("Security access failed!") - print("Open the hood and retry (disables the \"diagnostic firewall\" on newer vehicles)") quit() try: @@ -141,22 +135,19 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): # last two bytes, but not the VZ/importer or tester serial number # Can't seem to read it back, but we can read the calibration tester, # so fib a little and say that same tester did the programming - current_date = date.today() - formatted_date = current_date.strftime('%y-%m-%d') - year, month, day = (int(part) for part in formatted_date.split('-')) - prog_date = bytes([year, month, day]) + # TODO: encode the actual current date + prog_date = b'\x22\x02\x08' uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.PROGRAMMING_DATE, prog_date) tester_num = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.CALIBRATION_REPAIR_SHOP_CODE_OR_CALIBRATION_EQUIPMENT_SERIAL_NUMBER) uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.REPAIR_SHOP_CODE_OR_TESTER_SERIAL_NUMBER, tester_num) - uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding) + uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding) # type: ignore except (NegativeResponseError, MessageTimeoutError): print("Writing new configuration failed!") - print("Make sure the comma processes are stopped: tmux kill-session -t comma") quit() try: # Read back result just to make 100% sure everything worked - current_coding_text = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING).hex() + current_coding_text = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING).hex() # type: ignore print(f" New coding: {current_coding_text}") except (NegativeResponseError, MessageTimeoutError): print("Reading back updated coding failed!") diff --git a/selfdrive/hardware b/selfdrive/hardware new file mode 120000 index 00000000000000..02a42c502ffc1b --- /dev/null +++ b/selfdrive/hardware @@ -0,0 +1 @@ +../system/hardware/ \ No newline at end of file diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 1a8c72388ad8fc..5268902785092c 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,2 +1,5 @@ +ubloxd +ubloxd_test params_learner paramsd +locationd diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index e8eeff7e0cc102..4b7fba19b6c933 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,21 +1,22 @@ -Import('env', 'rednose') +Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations') -# build ekf models -rednose_gen_dir = 'models/generated' -rednose_gen_deps = [ - "models/constants.py", -] -pose_ekf = env.RednoseCompileFilter( - target='pose', - filter_gen_script='models/pose_kf.py', - output_dir=rednose_gen_dir, - extra_gen_artifacts=[], - 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, -) +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] + +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) + +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) + +ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") +locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] +lenv = env.Clone() +lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}' +locationd = lenv.Program("locationd", locationd_sources, LIBS=loc_libs + transformations) +lenv.Depends(locationd, libkf) + +if File("liblocationd.cc").exists(): + liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) + lenv.Depends(liblocationd, libkf) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 036f5822d25589..9e6536f9b5a69b 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -6,48 +6,46 @@ and the image input into the neural network is not corrected for roll. ''' +import gc import os import capnp import numpy as np -from typing import NoReturn +from typing import List, NoReturn, Optional -from cereal import log, car +from cereal import log import cereal.messaging as messaging -from openpilot.system.hardware import HARDWARE -from openpilot.common.constants import CV -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process -from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot -from openpilot.common.swaglog import cloudlog +from common.conversions import Conversions as CV +from common.params import Params, put_nonblocking +from common.realtime import set_realtime_priority +from common.transformations.orientation import rot_from_euler, euler_from_rot +from system.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) MAX_YAW_RATE_FILTER = np.radians(2) # per second -MAX_HEIGHT_STD = np.exp(-3.5) - # This is at model frequency, blocks needed for efficiency -SMOOTH_CYCLES = 10 +SMOOTH_CYCLES = 400 BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability -MAX_ALLOWED_YAW_SPREAD = np.radians(2) -MAX_ALLOWED_PITCH_SPREAD = np.radians(4) +MAX_ALLOWED_SPREAD = np.radians(2) RPY_INIT = np.array([0.0,0.0,0.0]) -WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0]) -HEIGHT_INIT = np.array([1.22]) - -# These values are needed to accommodate the model frame in the narrow cam -if HARDWARE.get_device_type() == 'mici': - PITCH_LIMITS = np.array([-0.143101, 0.22235988]) -else: - PITCH_LIMITS = np.array([-0.09074112085129739, 0.17]) + +# These values are needed to accommodate biggest modelframe +PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657]) YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235]) DEBUG = os.getenv("DEBUG") is not None +class Calibration: + UNCALIBRATED = 0 + CALIBRATED = 1 + INVALID = 2 + + def is_calibration_valid(rpy: np.ndarray) -> bool: - return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) + return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore def sanity_clip(rpy: np.ndarray) -> np.ndarray: @@ -57,8 +55,6 @@ def sanity_clip(rpy: np.ndarray) -> np.ndarray: np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005), np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)]) -def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray: - return (idx*prev_mean + (block_size - idx) * new_val) / block_size class Calibrator: def __init__(self, param_put: bool = False): @@ -67,55 +63,35 @@ def __init__(self, param_put: bool = False): self.not_car = False # Read saved calibration - self.params = Params() - calibration_params = self.params.get("CalibrationParams") + params = Params() + calibration_params = params.get("CalibrationParams") + self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT - wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT - height = HEIGHT_INIT valid_blocks = 0 - self.cal_status = log.LiveCalibrationData.Status.uncalibrated if param_put and calibration_params: try: - with log.Event.from_bytes(calibration_params) as msg: - rpy_init = np.array(msg.liveCalibration.rpyCalib) - valid_blocks = msg.liveCalibration.validBlocks - wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler) - height = np.array(msg.liveCalibration.height) + msg = log.Event.from_bytes(calibration_params) + rpy_init = np.array(msg.liveCalibration.rpyCalib) + valid_blocks = msg.liveCalibration.validBlocks except Exception: cloudlog.exception("Error reading cached CalibrationParams") - self.reset(rpy_init, valid_blocks, wide_from_device_euler, height) + self.reset(rpy_init, valid_blocks) self.update_status() - def reset(self, rpy_init: np.ndarray = RPY_INIT, - valid_blocks: int = 0, - wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, - height_init: np.ndarray = HEIGHT_INIT, - smooth_from: np.ndarray | None = None) -> None: + def reset(self, rpy_init: np.ndarray = RPY_INIT, valid_blocks: int = 0, smooth_from: Optional[np.ndarray] = None) -> None: if not np.isfinite(rpy_init).all(): self.rpy = RPY_INIT.copy() else: self.rpy = rpy_init.copy() - if not np.isfinite(height_init).all() or len(height_init) != 1: - self.height = HEIGHT_INIT.copy() - else: - self.height = height_init.copy() - - if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3: - self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy() - else: - self.wide_from_device_euler = wide_from_device_euler_init.copy() - if not np.isfinite(valid_blocks) or valid_blocks < 0: self.valid_blocks = 0 else: self.valid_blocks = valid_blocks self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) - self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1)) - self.heights = np.tile(self.height, (INPUTS_WANTED, 1)) self.idx = 0 self.block_idx = 0 @@ -128,7 +104,7 @@ def reset(self, rpy_init: np.ndarray = RPY_INIT, self.old_rpy = smooth_from self.old_rpy_weight = 1.0 - def get_valid_idxs(self) -> list[int]: + def get_valid_idxs(self) -> List[int]: # exclude current block_idx from validity window before_current = list(range(self.block_idx)) after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) @@ -137,8 +113,6 @@ def get_valid_idxs(self) -> list[int]: def update_status(self) -> None: valid_idxs = self.get_valid_idxs() if valid_idxs: - self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0) - self.height = np.mean(self.heights[valid_idxs], axis=0) rpys = self.rpys[valid_idxs] self.rpy = np.mean(rpys, axis=0) max_rpy_calib = np.array(np.max(rpys, axis=0)) @@ -148,26 +122,20 @@ def update_status(self) -> None: self.calib_spread = np.zeros(3) if self.valid_blocks < INPUTS_NEEDED: - if self.cal_status == log.LiveCalibrationData.Status.recalibrating: - self.cal_status = log.LiveCalibrationData.Status.recalibrating - else: - self.cal_status = log.LiveCalibrationData.Status.uncalibrated + self.cal_status = Calibration.UNCALIBRATED elif is_calibration_valid(self.rpy): - self.cal_status = log.LiveCalibrationData.Status.calibrated + self.cal_status = Calibration.CALIBRATED else: - self.cal_status = log.LiveCalibrationData.Status.invalid + self.cal_status = Calibration.INVALID # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. - # TODO: add height spread check with smooth transition too - spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD - if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated: - self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) - self.cal_status = log.LiveCalibrationData.Status.recalibrating + if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: + self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy) write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: - self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes()) + put_nonblocking("CalibrationParams", self.get_msg().to_bytes()) def handle_v_ego(self, v_ego: float) -> None: self.v_ego = v_ego @@ -178,24 +146,16 @@ def get_smooth_rpy(self) -> np.ndarray: else: return self.rpy - def handle_cam_odom(self, trans: list[float], - rot: list[float], - wide_from_device_euler: list[float], - trans_std: list[float], - road_transform_trans: list[float], - road_transform_trans_std: list[float]) -> np.ndarray | None: - self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) + def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]: + self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) - angle_std_threshold = MAX_VEL_ANGLE_STD - height_std_threshold = MAX_HEIGHT_STD - rpy_certain = np.arctan2(trans_std[1], trans[0]) < angle_std_threshold - if len(road_transform_trans_std) == 3: - height_certain = road_transform_trans_std[2] < height_std_threshold + if self.wide_camera: + angle_std_threshold = 4*MAX_VEL_ANGLE_STD else: - height_certain = True - - certain_if_calib = (rpy_certain and height_certain) or (self.valid_blocks < INPUTS_NEEDED) + angle_std_threshold = MAX_VEL_ANGLE_STD + certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or + (self.valid_blocks < INPUTS_NEEDED)) if not (straight_and_fast and certain_if_calib): return None @@ -205,21 +165,7 @@ def handle_cam_odom(self, trans: list[float], new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy))) new_rpy = sanity_clip(new_rpy) - if len(wide_from_device_euler) == 3: - new_wide_from_device_euler = np.array(wide_from_device_euler) - else: - new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT - - if (len(road_transform_trans) == 3): - new_height = np.array([road_transform_trans[2]]) - else: - new_height = HEIGHT_INIT - - self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE)) - self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx], - new_wide_from_device_euler, self.idx, float(BLOCK_SIZE)) - self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE)) - + self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) self.idx = (self.idx + 1) % BLOCK_SIZE if self.idx == 0: self.block_idx += 1 @@ -230,65 +176,65 @@ def handle_cam_odom(self, trans: list[float], return new_rpy - def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder: + def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder: smooth_rpy = self.get_smooth_rpy() msg = messaging.new_message('liveCalibration') - msg.valid = valid - liveCalibration = msg.liveCalibration + liveCalibration.validBlocks = self.valid_blocks liveCalibration.calStatus = self.cal_status liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalibSpread = self.calib_spread.tolist() - liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() - liveCalibration.height = self.height.tolist() if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED - liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated + liveCalibration.calStatus = Calibration.CALIBRATED liveCalibration.calPerc = 100. liveCalibration.rpyCalib = [0, 0, 0] liveCalibration.rpyCalibSpread = self.calib_spread.tolist() return msg - def send_data(self, pm: messaging.PubMaster, valid: bool) -> None: - pm.send('liveCalibration', self.get_msg(valid)) + def send_data(self, pm: messaging.PubMaster) -> None: + pm.send('liveCalibration', self.get_msg()) -def main() -> NoReturn: - config_realtime_process([0, 1, 2, 3], 5) +def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn: + gc.disable() + set_realtime_priority(1) - pm = messaging.PubMaster(['liveCalibration']) - sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry') + if sm is None: + sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry']) - params_reader = Params() - CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) + if pm is None: + pm = messaging.PubMaster(['liveCalibration']) calibrator = Calibrator(param_put=True) - calibrator.not_car = CP.notCar while 1: timeout = 0 if sm.frame == -1 else 100 sm.update(timeout) + calibrator.not_car = sm['carParams'].notCar + if sm.updated['cameraOdometry']: calibrator.handle_v_ego(sm['carState'].vEgo) new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, sm['cameraOdometry'].rot, - sm['cameraOdometry'].wideFromDeviceEuler, - sm['cameraOdometry'].transStd, - sm['cameraOdometry'].roadTransformTrans, - sm['cameraOdometry'].roadTransformTransStd) + sm['cameraOdometry'].transStd) if DEBUG and new_rpy is not None: print('got new rpy', new_rpy) # 4Hz driven by cameraOdometry if sm.frame % 5 == 0: - calibrator.send_data(pm, sm.all_checks()) + calibrator.send_data(pm) + + +def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn: + calibrationd_thread(sm, pm) if __name__ == "__main__": diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp new file mode 100644 index 00000000000000..9b020735bb0c6b --- /dev/null +++ b/selfdrive/locationd/generated/gps.cpp @@ -0,0 +1,325 @@ +// 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_magic = m__io->read_bytes(1); + if (!(magic() == std::string("\x8B", 1))) { + throw kaitai::validation_not_equal_error(std::string("\x8B", 1), magic(), _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/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h new file mode 100644 index 00000000000000..293e2e4a05974a --- /dev/null +++ b/selfdrive/locationd/generated/gps.h @@ -0,0 +1,359 @@ +#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_magic; + uint64_t m_tlm; + bool m_integrity_status; + bool m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + std::string magic() const { return m_magic; } + 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/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp new file mode 100644 index 00000000000000..5e743e1ee7e95d --- /dev/null +++ b/selfdrive/locationd/generated/ubx.cpp @@ -0,0 +1,340 @@ +// 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 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_measurements = 0; + m__raw_measurements = 0; + m__io__raw_measurements = 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); + int l_measurements = num_meas(); + m__raw_measurements = new std::vector(); + m__raw_measurements->reserve(l_measurements); + m__io__raw_measurements = new std::vector(); + m__io__raw_measurements->reserve(l_measurements); + m_measurements = new std::vector(); + m_measurements->reserve(l_measurements); + for (int i = 0; i < l_measurements; i++) { + m__raw_measurements->push_back(m__io->read_bytes(32)); + kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); + m__io__raw_measurements->push_back(io__raw_measurements); + m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root)); + } +} + +ubx_t::rxm_rawx_t::~rxm_rawx_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::_clean_up() { + if (m__raw_measurements) { + delete m__raw_measurements; m__raw_measurements = 0; + } + if (m__io__raw_measurements) { + for (std::vector::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + delete *it; + } + delete m__io__raw_measurements; m__io__raw_measurements = 0; + } + if (m_measurements) { + for (std::vector::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + delete *it; + } + delete m_measurements; m_measurements = 0; + } +} + +ubx_t::rxm_rawx_t::meas_t::meas_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::meas_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::meas_t::~meas_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::meas_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); + int l_body = num_words(); + m_body = new std::vector(); + m_body->reserve(l_body); + 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_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/selfdrive/locationd/generated/ubx.h b/selfdrive/locationd/generated/ubx.h new file mode 100644 index 00000000000000..6be4ce8c4b7c66 --- /dev/null +++ b/selfdrive/locationd/generated/ubx.h @@ -0,0 +1,410 @@ +#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_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 meas_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 meas_t : public kaitai::kstruct { + + public: + + meas_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: + ~meas_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_measurements; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_measurements; + std::vector* m__io__raw_measurements; + + 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* measurements() const { return m_measurements; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_measurements() const { return m__raw_measurements; } + std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + }; + + 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_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/selfdrive/locationd/gps.ksy b/selfdrive/locationd/gps.ksy new file mode 100644 index 00000000000000..6f5cde316b8b27 --- /dev/null +++ b/selfdrive/locationd/gps.ksy @@ -0,0 +1,189 @@ +# https://www.gps.gov/technical/icwg/IS-GPS-200E.pdf +meta: + id: gps + endian: be + bit-endian: be +seq: + - id: tlm + type: tlm + - id: how + type: how + - id: body + type: + switch-on: how.subframe_id + cases: + 1: subframe_1 + 2: subframe_2 + 3: subframe_3 + 4: subframe_4 +types: + tlm: + seq: + - id: magic + contents: [0x8b] + - id: tlm + type: b14 + - id: integrity_status + type: b1 + - id: reserved + type: b1 + how: + seq: + - id: tow_count + type: b17 + - id: alert + type: b1 + - id: anti_spoof + type: b1 + - id: subframe_id + type: b3 + - id: reserved + type: b2 + subframe_1: + seq: + # Word 3 + - id: week_no + type: b10 + - id: code + type: b2 + - id: sv_accuracy + type: b4 + - id: sv_health + type: b6 + - id: iodc_msb + type: b2 + # Word 4 + - id: l2_p_data_flag + type: b1 + - id: reserved1 + type: b23 + # Word 5 + - id: reserved2 + type: b24 + # Word 6 + - id: reserved3 + type: b24 + # Word 7 + - id: reserved4 + type: b16 + - id: t_gd + type: s1 + # Word 8 + - id: iodc_lsb + type: u1 + - id: t_oc + type: u2 + # Word 9 + - id: af_2 + type: s1 + - id: af_1 + type: s2 + # Word 10 + - id: af_0_sign + type: b1 + - id: af_0_value + type: b21 + - id: reserved5 + type: b2 + instances: + af_0: + value: 'af_0_sign ? (af_0_value - (1 << 21)) : af_0_value' + subframe_2: + seq: + # Word 3 + - id: iode + type: u1 + - id: c_rs + type: s2 + # Word 4 & 5 + - id: delta_n + type: s2 + - id: m_0 + type: s4 + # Word 6 & 7 + - id: c_uc + type: s2 + - id: e + type: s4 + # Word 8 & 9 + - id: c_us + type: s2 + - id: sqrt_a + type: u4 + # Word 10 + - id: t_oe + type: u2 + - id: fit_interval_flag + type: b1 + - id: aoda + type: b5 + - id: reserved + type: b2 + subframe_3: + seq: + # Word 3 & 4 + - id: c_ic + type: s2 + - id: omega_0 + type: s4 + # Word 5 & 6 + - id: c_is + type: s2 + - id: i_0 + type: s4 + # Word 7 & 8 + - id: c_rc + type: s2 + - id: omega + type: s4 + # Word 9 + - id: omega_dot_sign + type: b1 + - id: omega_dot_value + type: b23 + # Word 10 + - id: iode + type: u1 + - id: idot_sign + type: b1 + - id: idot_value + type: b13 + - id: reserved + type: b2 + instances: + omega_dot: + value: 'omega_dot_sign ? (omega_dot_value - (1 << 23)) : omega_dot_value' + idot: + value: 'idot_sign ? (idot_value - (1 << 13)) : idot_value' + subframe_4: + seq: + # Word 3 + - id: data_id + type: b2 + - id: page_id + type: b6 + - id: body + type: + switch-on: page_id + cases: + 56: ionosphere_data + types: + ionosphere_data: + seq: + - id: a0 + type: s1 + - id: a1 + type: s1 + - id: a2 + type: s1 + - id: a3 + type: s1 + - id: b0 + type: s1 + - id: b1 + type: s1 + - id: b2 + type: s1 + - id: b3 + type: s1 + diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py deleted file mode 100644 index 73c4d8bf352552..00000000000000 --- a/selfdrive/locationd/helpers.py +++ /dev/null @@ -1,183 +0,0 @@ -import numpy as np -from typing import Any -from functools import cache - -from cereal import log -from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot - - -@cache -def fft_next_good_size(n: int) -> int: - """ - smallest composite of 2, 3, 5, 7, 11 that is >= n - inspired by pocketfft - """ - if n <= 6: - return n - best, f2 = 2 * n, 1 - while f2 < best: - f23 = f2 - while f23 < best: - f235 = f23 - while f235 < best: - f2357 = f235 - while f2357 < best: - f235711 = f2357 - while f235711 < best: - best = f235711 if f235711 >= n else best - f235711 *= 11 - f2357 *= 7 - f235 *= 5 - f23 *= 3 - f2 *= 2 - return best - - -def parabolic_peak_interp(R, max_index): - if max_index == 0 or max_index == len(R) - 1: - return max_index - - y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1] - offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1) - - return max_index + offset - - -def rotate_cov(rot_matrix, cov_in): - return rot_matrix @ cov_in @ rot_matrix.T - - -def rotate_std(rot_matrix, std_in): - return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2)))) - - -class NPQueue: - def __init__(self, maxlen: int, rowsize: int) -> None: - self.maxlen = maxlen - self.arr = np.empty((0, rowsize)) - - def __len__(self) -> int: - return len(self.arr) - - def append(self, pt: list[float]) -> None: - if len(self.arr) < self.maxlen: - self.arr = np.append(self.arr, [pt], axis=0) - else: - self.arr[:-1] = self.arr[1:] - self.arr[-1] = pt - - -class PointBuckets: - def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: - self.x_bounds = x_bounds - self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} - self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) - self.min_points_total = min_points_total - - def __len__(self) -> int: - return sum([len(v) for v in self.buckets.values()]) - - def is_valid(self) -> bool: - individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) - total_points_valid = self.__len__() >= self.min_points_total - return individual_buckets_valid and total_points_valid - - def get_valid_percent(self) -> int: - total_points_perc = min(self.__len__() / self.min_points_total * 100, 100) - individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in - zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100) - return int((total_points_perc + individual_buckets_perc) / 2) - - def is_calculable(self) -> bool: - return all(len(v) > 0 for v in self.buckets.values()) - - def add_point(self, x: float, y: float) -> None: - raise NotImplementedError - - def get_points(self, num_points: int | None = None) -> Any: - points = np.vstack([x.arr for x in self.buckets.values()]) - if num_points is None: - return points - return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] - - def load_points(self, points: list[list[float]]) -> None: - for point in points: - self.add_point(*point) - - -class ParameterEstimator: - """ Base class for parameter estimators """ - def reset(self) -> None: - raise NotImplementedError - - def handle_log(self, t: int, which: str, msg: log.Event) -> None: - raise NotImplementedError - - def get_msg(self, valid: bool, with_points: bool) -> log.Event: - raise NotImplementedError - - -class Measurement: - x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2])) - x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2])) - roll, pitch, yaw = x, y, z - roll_std, pitch_std, yaw_std = x_std, y_std, z_std - - def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray): - self.xyz: np.ndarray = xyz - self.xyz_std: np.ndarray = xyz_std - - @classmethod - def from_measurement_xyz(cls, measurement: log.LivePose.XYZMeasurement) -> 'Measurement': - return cls( - xyz=np.array([measurement.x, measurement.y, measurement.z]), - xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd]) - ) - - -class Pose: - def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement): - self.orientation = orientation - self.velocity = velocity - self.acceleration = acceleration - self.angular_velocity = angular_velocity - - @classmethod - def from_live_pose(cls, live_pose: log.LivePose) -> 'Pose': - return Pose( - orientation=Measurement.from_measurement_xyz(live_pose.orientationNED), - velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice), - acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice), - angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice) - ) - - -class PoseCalibrator: - def __init__(self): - self.calib_valid = False - self.calib_from_device = np.eye(3) - - def _transform_calib_from_device(self, meas: Measurement): - new_xyz = self.calib_from_device @ meas.xyz - new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std) - return Measurement(new_xyz, new_xyz_std) - - def _ned_from_calib(self, orientation: Measurement): - ned_from_device = rot_from_euler(orientation.xyz) - ned_from_calib = ned_from_device @ self.calib_from_device.T - ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan)) - return ned_from_calib_euler_meas - - def build_calibrated_pose(self, pose: Pose) -> Pose: - ned_from_calib_euler = self._ned_from_calib(pose.orientation) - angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity) - acceleration_calib = self._transform_calib_from_device(pose.acceleration) - velocity_calib = self._transform_calib_from_device(pose.velocity) - - return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib) - - def feed_live_calib(self, live_calib: log.LiveCalibrationData): - calib_rpy = np.array(live_calib.rpyCalib) - device_from_calib = rot_from_euler(calib_rpy) - self.calib_from_device = device_from_calib.T - self.calib_valid = live_calib.calStatus == log.LiveCalibrationData.Status.calibrated diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py deleted file mode 100755 index d7834f7f1fd417..00000000000000 --- a/selfdrive/locationd/lagd.py +++ /dev/null @@ -1,394 +0,0 @@ -#!/usr/bin/env python3 -import os -import numpy as np -import capnp -from collections import deque -from functools import partial - -import cereal.messaging as messaging -from cereal import car, log -from cereal.services import SERVICE_LIST -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp - -BLOCK_SIZE = 100 -BLOCK_NUM = 50 -BLOCK_NUM_NEEDED = 5 -MOVING_WINDOW_SEC = 60.0 -MIN_OKAY_WINDOW_SEC = 25.0 -MIN_RECOVERY_BUFFER_SEC = 2.0 -MIN_VEGO = 15.0 -MIN_ABS_YAW_RATE = 0.0 -MAX_YAW_RATE_SANITY_CHECK = 1.0 -MIN_NCC = 0.95 -MAX_LAG = 1.0 -MAX_LAG_STD = 0.1 -MAX_LAT_ACCEL = 2.0 -MAX_LAT_ACCEL_DIFF = 0.6 -MIN_CONFIDENCE = 0.7 -CORR_BORDER_OFFSET = 5 -LAG_CANDIDATE_CORR_THRESHOLD = 0.9 - - -def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int): - """ - References: - D. Padfield. "Masked FFT registration". In Proc. Computer Vision and - Pattern Recognition, pp. 2918-2925 (2010). - :DOI:`10.1109/CVPR.2010.5540032` - """ - - eps = np.finfo(np.float64).eps - expected_sig = np.asarray(expected_sig, dtype=np.float64) - actual_sig = np.asarray(actual_sig, dtype=np.float64) - - expected_sig[~mask] = 0.0 - actual_sig[~mask] = 0.0 - - rotated_expected_sig = expected_sig[::-1] - rotated_mask = mask[::-1] - - fft = partial(np.fft.fft, n=n) - - actual_sig_fft = fft(actual_sig) - rotated_expected_sig_fft = fft(rotated_expected_sig) - actual_mask_fft = fft(mask.astype(np.float64)) - rotated_mask_fft = fft(rotated_mask.astype(np.float64)) - - number_overlap_masked_samples = np.fft.ifft(rotated_mask_fft * actual_mask_fft).real - number_overlap_masked_samples[:] = np.round(number_overlap_masked_samples) - number_overlap_masked_samples[:] = np.fmax(number_overlap_masked_samples, eps) - masked_correlated_actual_fft = np.fft.ifft(rotated_mask_fft * actual_sig_fft).real - masked_correlated_expected_fft = np.fft.ifft(actual_mask_fft * rotated_expected_sig_fft).real - - numerator = np.fft.ifft(rotated_expected_sig_fft * actual_sig_fft).real - numerator -= masked_correlated_actual_fft * masked_correlated_expected_fft / number_overlap_masked_samples - - actual_squared_fft = fft(actual_sig ** 2) - actual_sig_denom = np.fft.ifft(rotated_mask_fft * actual_squared_fft).real - actual_sig_denom -= masked_correlated_actual_fft ** 2 / number_overlap_masked_samples - actual_sig_denom[:] = np.fmax(actual_sig_denom, 0.0) - - rotated_expected_squared_fft = fft(rotated_expected_sig ** 2) - expected_sig_denom = np.fft.ifft(actual_mask_fft * rotated_expected_squared_fft).real - expected_sig_denom -= masked_correlated_expected_fft ** 2 / number_overlap_masked_samples - expected_sig_denom[:] = np.fmax(expected_sig_denom, 0.0) - - denom = np.sqrt(actual_sig_denom * expected_sig_denom) - - # zero-out samples with very small denominators - tol = 1e3 * eps * np.max(np.abs(denom), keepdims=True) - nonzero_indices = denom > tol - - ncc = np.zeros_like(denom, dtype=np.float64) - ncc[nonzero_indices] = numerator[nonzero_indices] / denom[nonzero_indices] - np.clip(ncc, -1, 1, out=ncc) - - return ncc - - -class Points: - def __init__(self, num_points: int): - self.times = deque[float]([0.0] * num_points, maxlen=num_points) - self.okay = deque[bool]([False] * num_points, maxlen=num_points) - self.desired = deque[float]([0.0] * num_points, maxlen=num_points) - self.actual = deque[float]([0.0] * num_points, maxlen=num_points) - - @property - def num_points(self): - return len(self.desired) - - @property - def num_okay(self): - return np.count_nonzero(self.okay) - - def update(self, t: float, desired: float, actual: float, okay: bool): - self.times.append(t) - self.okay.append(okay) - self.desired.append(desired) - self.actual.append(actual) - - def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay) - - -class BlockAverage: - def __init__(self, num_blocks: int, block_size: int, valid_blocks: int, initial_value: float): - self.num_blocks = num_blocks - self.block_size = block_size - self.block_idx = valid_blocks % num_blocks - self.idx = 0 - - self.values = np.tile(initial_value, (num_blocks, 1)) - self.valid_blocks = valid_blocks - - def update(self, value: float): - self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + value) / (self.idx + 1) - self.idx = (self.idx + 1) % self.block_size - if self.idx == 0: - self.block_idx = (self.block_idx + 1) % self.num_blocks - self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks) - - def get(self) -> tuple[float, float, float, float]: - valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx] - valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else []) - - if len(valid_block_idx) > 0: - valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item()) - valid_std = float(np.std(self.values[valid_block_idx], axis=0).item()) - else: - valid_mean, valid_std = float('nan'), float('nan') - - if len(valid_and_current_idx) > 0: - current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item()) - current_std = float(np.std(self.values[valid_and_current_idx], axis=0).item()) - else: - current_mean, current_std = float('nan'), float('nan') - - return valid_mean, valid_std, current_mean, current_std - - -class LateralLagEstimator: - inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"} - - def __init__(self, CP: car.CarParams, dt: float, - block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE, - window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC, - min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC, - max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE): - self.dt = dt - self.window_sec = window_sec - self.okay_window_sec = okay_window_sec - self.min_recovery_buffer_sec = min_recovery_buffer_sec - self.initial_lag = CP.steerActuatorDelay + 0.2 - self.block_size = block_size - self.block_count = block_count - self.min_valid_block_count = min_valid_block_count - self.min_vego = min_vego - self.min_yr = min_yr - self.min_ncc = min_ncc - self.min_confidence = min_confidence - self.max_lat_accel = max_lat_accel - self.max_lat_accel_diff = max_lat_accel_diff - - self.t = 0.0 - self.lat_active = False - self.steering_pressed = False - self.steering_saturated = False - self.desired_curvature = 0.0 - self.v_ego = 0.0 - self.yaw_rate = 0.0 - self.yaw_rate_std = 0.0 - self.pose_valid = False - - self.last_lat_inactive_t = 0.0 - self.last_steering_pressed_t = 0.0 - self.last_steering_saturated_t = 0.0 - self.last_pose_invalid_t = 0.0 - self.last_estimate_t = 0.0 - - self.calibrator = PoseCalibrator() - - self.reset(self.initial_lag, 0) - - def reset(self, initial_lag: float, valid_blocks: int): - window_len = int(self.window_sec / self.dt) - self.points = Points(window_len) - self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) - - def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: - msg = messaging.new_message('liveDelay') - - msg.valid = valid - - liveDelay = msg.liveDelay - - valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() - if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): - if valid_std > MAX_LAG_STD: - liveDelay.status = log.LiveDelayData.Status.invalid - else: - liveDelay.status = log.LiveDelayData.Status.estimated - else: - liveDelay.status = log.LiveDelayData.Status.unestimated - - if liveDelay.status == log.LiveDelayData.Status.estimated: - liveDelay.lateralDelay = valid_mean_lag - else: - liveDelay.lateralDelay = self.initial_lag - - if not np.isnan(current_mean_lag) and not np.isnan(current_std): - liveDelay.lateralDelayEstimate = current_mean_lag - liveDelay.lateralDelayEstimateStd = current_std - else: - liveDelay.lateralDelayEstimate = self.initial_lag - liveDelay.lateralDelayEstimateStd = 0.0 - - liveDelay.validBlocks = self.block_avg.valid_blocks - liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // - (self.min_valid_block_count * self.block_size), 100) - if debug: - liveDelay.points = self.block_avg.values.flatten().tolist() - - return msg - - def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): - if which == "carControl": - self.lat_active = msg.latActive - elif which == "carState": - self.steering_pressed = msg.steeringPressed - self.v_ego = msg.vEgo - elif which == "controlsState": - self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated - self.desired_curvature = msg.desiredCurvature - elif which == "liveCalibration": - self.calibrator.feed_live_calib(msg) - elif which == "livePose": - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - self.yaw_rate = calibrated_pose.angular_velocity.yaw - self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std - self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK - self.t = t - - def points_enough(self): - return self.points.num_points >= int(self.okay_window_sec / self.dt) - - def points_valid(self): - return self.points.num_okay >= int(self.okay_window_sec / self.dt) - - def update_points(self): - la_desired = self.desired_curvature * self.v_ego * self.v_ego - la_actual_pose = self.yaw_rate * self.v_ego - - fast = self.v_ego > self.min_vego - turning = np.abs(self.yaw_rate) >= self.min_yr - sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK - la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff - calib_valid = self.calibrator.calib_valid - - if not self.lat_active: - self.last_lat_inactive_t = self.t - if self.steering_pressed: - self.last_steering_pressed_t = self.t - if self.steering_saturated: - self.last_steering_saturated_t = self.t - if not sensors_valid or not la_valid: - self.last_pose_invalid_t = self.t - - has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated, !sensors/la_valid - self.t - last_t >= self.min_recovery_buffer_sec - for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t, self.last_pose_invalid_t] - ) - okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \ - fast and turning and has_recovered and calib_valid and sensors_valid and la_valid - - self.points.update(self.t, la_desired, la_actual_pose, okay) - - def update_estimate(self): - if not self.points_enough(): - return - - times, desired, actual, okay = self.points.get() - # check if there are any new valid data points since the last update - is_valid = self.points_valid() - if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t: - new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t) - is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:])) - - delay, corr, confidence = self.actuator_delay(desired, actual, okay, self.dt, MAX_LAG) - if corr < self.min_ncc or confidence < self.min_confidence or not is_valid: - return - - self.block_avg.update(delay) - self.last_estimate_t = self.t - - @staticmethod - def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, dt: float, max_lag: float) -> tuple[float, float, float]: - assert len(expected_sig) == len(actual_sig) - max_lag_samples = int(max_lag / dt) - padded_size = fft_next_good_size(len(expected_sig) + max_lag_samples) - - ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size) - - # only consider lags from 0 to max_lag - roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + max_lag_samples] - extended_roi = np.s_[roi.start - CORR_BORDER_OFFSET: roi.stop + CORR_BORDER_OFFSET] - roi_ncc = ncc[roi] - extended_roi_ncc = ncc[extended_roi] - - max_corr_index = np.argmax(roi_ncc) - corr = roi_ncc[max_corr_index] - lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt - - # to estimate lag confidence, gather all high-correlation candidates and see how spread they are - # if e.g. 0.8 and 0.4 are both viable, this is an ambiguous case - ncc_thresh = (roi_ncc.max() - roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + roi_ncc.min() - good_lag_candidate_mask = extended_roi_ncc >= ncc_thresh - good_lag_candidate_edges = np.diff(good_lag_candidate_mask.astype(int), prepend=0, append=0) - starts, ends = np.where(good_lag_candidate_edges == 1)[0], np.where(good_lag_candidate_edges == -1)[0] - 1 - run_idx = np.searchsorted(starts, max_corr_index + CORR_BORDER_OFFSET, side='right') - 1 - width = ends[run_idx] - starts[run_idx] + 1 - confidence = np.clip(1 - width * dt, 0, 1) - - return lag, corr, confidence - - -def retrieve_initial_lag(params: Params, CP: car.CarParams): - last_lag_data = params.get("LiveDelay") - last_carparams_data = params.get("CarParamsPrevRoute") - - if last_lag_data is not None: - try: - with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: - ld = last_lag_msg.liveDelay - if last_CP.carFingerprint != CP.carFingerprint: - raise Exception("Car model mismatch") - - lag, valid_blocks, status = ld.lateralDelayEstimate, ld.validBlocks, ld.status - assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" - assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid" - return lag, valid_blocks - except Exception as e: - cloudlog.error(f"Failed to retrieve initial lag: {e}") - params.remove("LiveDelay") - - return None - - -def main(): - config_realtime_process([0, 1, 2, 3], 5) - - DEBUG = bool(int(os.getenv("DEBUG", "0"))) - - pm = messaging.PubMaster(['liveDelay']) - sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') - - params = Params() - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) - if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: - lag, valid_blocks = initial_lag_params - lag_learner.reset(lag, valid_blocks) - - while True: - sm.update() - if sm.all_checks(): - for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): - if sm.updated[which]: - t = sm.logMonoTime[which] * 1e-9 - lag_learner.handle_log(t, which, sm[which]) - lag_learner.update_points() - - # 4Hz driven by livePose - if sm.frame % 5 == 0: - lag_learner.update_estimate() - lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) - lag_msg_dat = lag_msg.to_bytes() - pm.send('liveDelay', lag_msg_dat) - - if sm.frame % 1200 == 0: # cache every 60 seconds - params.put_nonblocking("LiveDelay", lag_msg_dat) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py new file mode 100755 index 00000000000000..c7f2d2ceac1f8e --- /dev/null +++ b/selfdrive/locationd/laikad.py @@ -0,0 +1,362 @@ +#!/usr/bin/env python3 +import json +import math +import os +import time +from collections import defaultdict +from concurrent.futures import Future, ProcessPoolExecutor +from datetime import datetime +from enum import IntEnum +from typing import List, Optional + +import numpy as np + +from cereal import log, messaging +from common.params import Params, put_nonblocking +from laika import AstroDog +from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.downloader import DownloadFailed +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem +from laika.gps_time import GPSTime +from laika.helpers import ConstellationId +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom +from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun +from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind +from selfdrive.locationd.models.gnss_kf import GNSSKalman +from selfdrive.locationd.models.gnss_kf import States as GStates +from system.swaglog import cloudlog + +MAX_TIME_GAP = 10 +EPHEMERIS_CACHE = 'LaikadEphemeris' +DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/" +CACHE_VERSION = 0.1 +POS_FIX_RESIDUAL_THRESHOLD = 100.0 + + +class Laikad: + def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False, + valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), + save_ephemeris=False, use_qcom=False): + """ + valid_const: GNSS constellation which can be used + auto_fetch_orbits: If true fetch orbits from internet when needed + auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere. + valid_ephem_types: Valid ephemeris types to be used by AstroDog + save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. + """ + self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER) + self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom) + + self.auto_fetch_orbits = auto_fetch_orbits + self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None + self.orbit_fetch_future: Optional[Future] = None + + self.last_fetch_orbits_t = None + self.got_first_gnss_msg = False + self.last_cached_t = None + self.save_ephemeris = save_ephemeris + self.load_cache() + + self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} + self.last_pos_fix = [] + self.last_pos_residual = [] + self.last_pos_fix_t = None + self.use_qcom = use_qcom + + def load_cache(self): + if not self.save_ephemeris: + return + + cache = Params().get(EPHEMERIS_CACHE) + if not cache: + return + + try: + cache = json.loads(cache, object_hook=deserialize_hook) + self.astro_dog.add_orbits(cache['orbits']) + self.astro_dog.add_navs(cache['nav']) + self.last_fetch_orbits_t = cache['last_fetch_orbits_t'] + except json.decoder.JSONDecodeError: + cloudlog.exception("Error parsing cache") + timestamp = self.last_fetch_orbits_t.as_datetime() if self.last_fetch_orbits_t is not None else 'Nan' + cloudlog.debug( + f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and orbits ({sum([len(v) for v in cache['orbits']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['orbits'].keys())} {list(cache['nav'].keys())} " + + f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.orbit_fetched_times._ranges]}") + + def cache_ephemeris(self, t: GPSTime): + if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): + put_nonblocking(EPHEMERIS_CACHE, json.dumps( + {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav}, + cls=CacheSerializer)) + cloudlog.debug("Cache saved") + self.last_cached_t = t + + def get_est_pos(self, t, processed_measurements): + if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2: + min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 5 + pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + if len(pos_fix) > 0: + self.last_pos_fix_t = t + residual_median = np.median(np.abs(pos_fix_residual)) + if np.median(np.abs(pos_fix_residual)) < POS_FIX_RESIDUAL_THRESHOLD: + cloudlog.debug(f"Pos fix is within threshold with median: {residual_median.round()}") + self.last_pos_fix = pos_fix[:3] + self.last_pos_residual = pos_fix_residual + else: + cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}") + return self.last_pos_fix + + def is_good_report(self, gnss_msg): + if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: + constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) + # TODO support GLONASS + return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] + elif gnss_msg.which == 'measurementReport' and not self.use_qcom: + return True + else: + return False + + def read_report(self, gnss_msg): + if self.use_qcom: + report = gnss_msg.drMeasurementReport + week = report.gpsWeek + tow = report.gpsMilliseconds / 1000.0 + new_meas = read_raw_qcom(report) + else: + report = gnss_msg.measurementReport + week = report.gpsWeek + tow = report.rcvTow + new_meas = read_raw_ublox(report) + return week, tow, new_meas + + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): + if self.is_good_report(gnss_msg): + week, tow, new_meas = self.read_report(gnss_msg) + + t = gnss_mono_time * 1e-9 + if week > 0: + self.got_first_gnss_msg = True + latest_msg_t = GPSTime(week, tow) + if self.auto_fetch_orbits: + self.fetch_orbits(latest_msg_t, block) + + # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites + new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] + + processed_measurements = process_measurements(new_meas, self.astro_dog) + est_pos = self.get_est_pos(t, processed_measurements) + + corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else [] + if gnss_mono_time % 10 == 0: + cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") + + self.update_localizer(est_pos, t, corrected_measurements) + kf_valid = all(self.kf_valid(t)) + ecef_pos = self.gnss_kf.x[GStates.ECEF_POS] + ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY] + + p = self.gnss_kf.P.diagonal() + pos_std = np.sqrt(p[GStates.ECEF_POS]) + vel_std = np.sqrt(p[GStates.ECEF_VELOCITY]) + + meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] + dat = messaging.new_message("gnssMeasurements") + measurement_msg = log.LiveLocationKalman.Measurement.new_message + dat.gnssMeasurements = { + "gpsWeek": week, + "gpsTimeOfWeek": tow, + "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), + "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), + "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), + "ubloxMonoTime": gnss_mono_time, + "correctedMeasurements": meas_msgs + } + return dat + # TODO this only works on GLONASS, qcom needs live ephemeris parsing too + elif gnss_msg.which == 'ephemeris': + ephem = convert_ublox_ephem(gnss_msg.ephemeris) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) + #elif gnss_msg.which == 'ionoData': + # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. + + def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): + # Check time and outputs are valid + valid = self.kf_valid(t) + if not all(valid): + if not valid[0]: # Filter not initialized + pass + elif not valid[1]: + cloudlog.error("Time gap of over 10s detected, gnss kalman reset") + elif not valid[2]: + cloudlog.error("Gnss kalman filter state is nan") + if len(est_pos) > 0: + cloudlog.info(f"Reset kalman filter with {est_pos}") + self.init_gnss_localizer(est_pos) + else: + return + if len(measurements) > 0: + kf_add_observations(self.gnss_kf, t, measurements) + else: + # Ensure gnss filter is updated even with no new measurements + self.gnss_kf.predict(t) + + def kf_valid(self, t: float) -> List[bool]: + filter_time = self.gnss_kf.filter.get_filter_time() + return [not math.isnan(filter_time), + abs(t - filter_time) < MAX_TIME_GAP, + all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))] + + def init_gnss_localizer(self, est_pos): + x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) + x_initial[GStates.ECEF_POS] = est_pos + p_initial_diag[GStates.ECEF_POS] = 1000 ** 2 + self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) + + def fetch_orbits(self, t: GPSTime, block): + # Download new orbits if 1 hour of orbits data left + if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or abs(t - self.last_fetch_orbits_t) > SECS_IN_MIN): + astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir + ret = None + + if block: # Used for testing purposes + ret = get_orbit_data(t, *astro_dog_vars) + elif self.orbit_fetch_future is None: + self.orbit_fetch_executor = ProcessPoolExecutor(max_workers=1) + self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars) + elif self.orbit_fetch_future.done(): + ret = self.orbit_fetch_future.result() + self.orbit_fetch_executor = self.orbit_fetch_future = None + + if ret is not None: + if ret[0] is None: + self.last_fetch_orbits_t = ret[2] + else: + self.astro_dog.orbits, self.astro_dog.orbit_fetched_times, self.last_fetch_orbits_t = ret + self.cache_ephemeris(t=t) + + +def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir): + astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir) + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + try: + astro_dog.get_orbit_data(t, only_predictions=True) + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s") + cloudlog.debug(f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}" + + f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}") + return astro_dog.orbits, astro_dog.orbit_fetched_times, t + except (DownloadFailed, RuntimeError, ValueError, IOError) as e: + cloudlog.warning(f"No orbit data found or parsing failure: {e}") + return None, None, t + + +def create_measurement_msg(meas: GNSSMeasurement): + c = log.GnssMeasurements.CorrectedMeasurement.new_message() + c.constellationId = meas.constellation_id.value + c.svId = meas.sv_id + c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0 + c.pseudorange = float(meas.observables_final['C1C']) + c.pseudorangeStd = float(meas.observables_std['C1C']) + c.pseudorangeRate = float(meas.observables_final['D1C']) + c.pseudorangeRateStd = float(meas.observables_std['D1C']) + c.satPos = meas.sat_pos_final.tolist() + c.satVel = meas.sat_vel.tolist() + c.satVel = meas.sat_vel.tolist() + ephem = meas.sat_ephemeris + assert ephem is not None + if ephem.eph_type == EphemerisType.NAV: + source_type = EphemerisSourceType.nav + week, time_of_week = -1, -1 + else: + assert ephem.file_epoch is not None + week = ephem.file_epoch.week + time_of_week = ephem.file_epoch.tow + file_src = ephem.file_source + if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z' + source_type = EphemerisSourceType.nasaUltraRapid + elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3' + source_type = EphemerisSourceType.glonassIacUltraRapid + else: + raise Exception(f"Didn't expect file source {file_src}") + + c.ephemerisSource.type = source_type.value + c.ephemerisSource.gpsWeek = week + c.ephemerisSource.gpsTimeOfWeek = int(time_of_week) + return c + + +def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]): + ekf_data = defaultdict(list) + for m in measurements: + m_arr = m.as_array() + if m.constellation_id == ConstellationId.GPS: + ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr) + elif m.constellation_id == ConstellationId.GLONASS: + ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr) + ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS] + ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS] + for kind, data in ekf_data.items(): + if len(data) > 0: + gnss_kf.predict_and_observe(t, kind, data) + + +class CacheSerializer(json.JSONEncoder): + + def default(self, o): + if isinstance(o, Ephemeris): + return o.to_json() + if isinstance(o, GPSTime): + return o.__dict__ + if isinstance(o, np.ndarray): + return o.tolist() + return json.JSONEncoder.default(self, o) + + +def deserialize_hook(dct): + if 'ephemeris' in dct: + return Ephemeris.from_json(dct) + if 'week' in dct: + return GPSTime(dct['week'], dct['tow']) + return dct + + +class EphemerisSourceType(IntEnum): + nav = 0 + nasaUltraRapid = 1 + glonassIacUltraRapid = 2 + + +def main(sm=None, pm=None): + use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps") + if use_qcom: + raw_gnss_socket = "qcomGnss" + else: + raw_gnss_socket = "ubloxGnss" + + if sm is None: + sm = messaging.SubMaster([raw_gnss_socket, 'clocks']) + if pm is None: + pm = messaging.PubMaster(['gnssMeasurements']) + + replay = "REPLAY" in os.environ + use_internet = "LAIKAD_NO_INTERNET" not in os.environ + laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom) + + while True: + sm.update() + + if sm.updated[raw_gnss_socket]: + gnss_msg = sm[raw_gnss_socket] + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) + if msg is not None: + pm.send('gnssMeasurements', msg) + if not laikad.got_first_gnss_msg and sm.updated['clocks']: + clocks_msg = sm['clocks'] + t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9)) + if laikad.auto_fetch_orbits: + laikad.fetch_orbits(t, block=replay) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/laikad_helpers.py b/selfdrive/locationd/laikad_helpers.py new file mode 100644 index 00000000000000..f13e8e73bb22a7 --- /dev/null +++ b/selfdrive/locationd/laikad_helpers.py @@ -0,0 +1,89 @@ +import numpy as np +import sympy + +from laika.constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT +from laika.helpers import ConstellationId + + +def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6): + ''' + Calculates gps fix using gauss newton method + To solve the problem a minimal of 4 measurements are required. + If Glonass is included 5 are required to solve for the additional free variable. + returns: + 0 -> list with positions + ''' + if x0 is None: + x0 = [0, 0, 0, 0, 0] + n = len(measurements) + if n < min_measurements: + return [], [] + + Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) + x = gauss_newton(Fx_pos, x0) + residual, _ = Fx_pos(x, weight=1.0) + return x.tolist(), residual.tolist() + + +def pr_residual(measurements, posfix_functions, signal='C1C'): + def Fx_pos(inp, weight=None): + vals, gradients = [], [] + + for meas in measurements: + pr = meas.observables[signal] + pr += meas.sat_clock_err * SPEED_OF_LIGHT + + w = (1 / meas.observables_std[signal]) if weight is None else weight + + val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w) + vals.append(val) + gradients.append(gradient) + return np.asarray(vals), np.asarray(gradients) + + return Fx_pos + + +def gauss_newton(fun, b, xtol=1e-8, max_n=25): + for _ in range(max_n): + # Compute function and jacobian on current estimate + r, J = fun(b) + + # Update estimate + delta = np.linalg.pinv(J) @ r + b -= delta + + # Check step size for stopping condition + if np.linalg.norm(delta) < xtol: + break + return b + + +def get_posfix_sympy_fun(constellation): + # Unknowns + x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z') + bc = sympy.Symbol('bc') + bg = sympy.Symbol('bg') + var = [x, y, z, bc, bg] + + # Knowns + pr = sympy.Symbol('pr') + sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z') + weight = sympy.Symbol('weight') + + theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT + val = sympy.sqrt( + (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 + + (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 + + (sat_z - z) ** 2 + ) + + if constellation == ConstellationId.GLONASS: + res = weight * (val - (pr - bc - bg)) + elif constellation == ConstellationId.GPS: + res = weight * (val - (pr - bc)) + else: + raise NotImplementedError(f"Constellation {constellation} not supported") + + res = [res] + [sympy.diff(res, v) for v in var] + + return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res, modules=["numpy"]) diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc new file mode 100755 index 00000000000000..49404668a442c1 --- /dev/null +++ b/selfdrive/locationd/liblocationd.cc @@ -0,0 +1,29 @@ +#include "locationd.h" + +extern "C" { + typedef Localizer* Localizer_t; + + Localizer *localizer_init() { + return new Localizer(); + } + + void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, + char *buff, size_t buff_size) { + MessageBuilder msg_builder; + kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, msgValid).asChars(); + assert(buff_size >= arr.size()); + memcpy(buff, arr.begin(), arr.size()); + } + + void localizer_handle_msg_bytes(Localizer *localizer, const char *data, size_t size) { + localizer->handle_msg_bytes(data, size); + } + + void get_filter_internals(Localizer *localizer, double *state_buff, double *std_buff){ + Eigen::VectorXd state = localizer->get_state(); + memcpy(state_buff, state.data(), sizeof(double) * state.size()); + Eigen::VectorXd stdev = localizer->get_stdev(); + memcpy(std_buff, stdev.data(), sizeof(double) * stdev.size()); + } + +} diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc new file mode 100755 index 00000000000000..2fb3e0081d8378 --- /dev/null +++ b/selfdrive/locationd/locationd.cc @@ -0,0 +1,554 @@ +#include +#include + +#include + +#include "locationd.h" + +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 + +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() { + 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] }); +} + +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); + + 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); + 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(); +} + +void Localizer::handle_sensors(double current_time, const capnp::List::Reader& log) { + // TODO does not yet account for double sensor readings in the log + for (int i = 0; i < log.size(); i++) { + const cereal::SensorEventData::Reader& sensor_reading = log[i]; + + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (sensor_reading.getTimestamp() == 0) { + continue; + } + + double sensor_time = 1e-9 * sensor_reading.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"); + return; + } + + // TODO: handle messages from two IMUs at the same time + if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::BMX055) { + continue; + } + + // Gyro Uncalibrated + if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = sensor_reading.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ROTATION_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + } + } + + // Accelerometer + if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = sensor_reading.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 }); + } + } + } +} + +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); + MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov(); + 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) { + // ignore the message if the fix is invalid + bool gps_invalid_flag = (log.getFlags() % 2 == 0); + bool gps_unreasonable = (Vector2d(log.getAccuracy(), 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 (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){ + this->determine_gps_mode(current_time); + return; + } + + // Process message + this->last_gps_fix = current_time; + 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; + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 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(current_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->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_car_state(double current_time, const cereal::CarState::Reader& log) { + this->car_speed = std::abs(log.getVEgo()); + if (log.getStandstill()) { + 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 ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + 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)) { + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + 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 }); +} + +void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + 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)) { + 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() == 1; + } +} + +void Localizer::reset_kalman(double current_time) { + VectorXd init_x = this->kf->get_initial_x(); + 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; + } + 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->isGpsOK()) { + this->reset_tracker *= .99995; + } else { + this->reset_tracker = 0.0; + } +} + +void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, 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(); + 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, VectorXd init_x, 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.isSensorEvents()) { + this->handle_sensors(t, log.getSensorEvents()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation()); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal()); + } 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::isGpsOK() { + return this->kf->get_filter_time() - this->last_gps_fix < 1.0; +} + +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); + } + } +} + +int Localizer::locationd_thread() { + const char* gps_location_socket; + if (util::file_exists("/persist/comma/use-quectel-rawgps")) { + gps_location_socket = "gpsLocation"; + } else { + gps_location_socket = "gpsLocationExternal"; + } + const std::initializer_list service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"}; + PubMaster pm({"liveLocationKalman"}); + + // TODO: remove carParams once we're always sending at 100Hz + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + + uint64_t cnt = 0; + bool filterInitialized = false; + + while (!do_exit) { + sm.update(); + if (filterInitialized){ + 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(); + } + + // 100Hz publish for notcars, 20Hz for cars + const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry"; + if (sm.updated(trigger_msg)) { + bool inputsOK = sm.allAliveAndValid(); + bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); + bool gpsOK = this->isGpsOK(); + + 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)); + + std::thread([] (const std::string gpsjson) { + Params().put("LastGPSPosition", gpsjson); + }, lastGPSPosJSON).detach(); + } + cnt++; + } + } + return 0; +} + +int main() { + util::set_realtime_priority(5); + + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h new file mode 100755 index 00000000000000..7c0cb6b7fe252e --- /dev/null +++ b/selfdrive/locationd/locationd.h @@ -0,0 +1,75 @@ +#pragma once + +#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 "selfdrive/sensord/sensors/constants.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 +#include "selfdrive/locationd/models/live_kf.h" + +#define POSENET_STD_HIST_HALF 20 + +class Localizer { +public: + Localizer(); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R); + void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P); + void finite_check(double current_time = NAN); + void time_check(double current_time = NAN); + void update_reset_tracker(); + bool isGpsOK(); + void determine_gps_mode(double current_time); + + 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_sensors(double current_time, const capnp::List::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::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 last_gps_fix = 0; + double reset_tracker = 0.0; + bool device_fell = false; + bool gps_mode = false; +}; diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py deleted file mode 100755 index f6a0935ed97c9a..00000000000000 --- a/selfdrive/locationd/locationd.py +++ /dev/null @@ -1,332 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import capnp -import numpy as np -from enum import Enum -from collections import defaultdict - -from cereal import log, messaging -from cereal.services import SERVICE_LIST -from openpilot.common.transformations.orientation import rot_from_euler -from openpilot.common.realtime import config_realtime_process -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.helpers import rotate_std -from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States -from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR - -ACCEL_SANITY_CHECK = 100.0 # m/s^2 -ROTATION_SANITY_CHECK = 10.0 # rad/s -TRANS_SANITY_CHECK = 200.0 # m/s -CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg) -MIN_STD_SANITY_CHECK = 1e-5 # m or rad -MAX_FILTER_REWIND_TIME = 0.8 # s -MAX_SENSOR_TIME_DIFF = 0.1 # s -YAWRATE_CROSS_ERR_CHECK_FACTOR = 30 -INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored -INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one -POSENET_STD_INITIAL_VALUE = 10.0 -POSENET_STD_HIST_HALF = 20 - - -def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency): - return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency)) - - -def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool): - assert len(values) == len(stds) == 3 - measurement.x, measurement.y, measurement.z = map(float, values) - measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds) - measurement.valid = valid - - -class HandleLogResult(Enum): - SUCCESS = 0 - TIMING_INVALID = 1 - INPUT_INVALID = 2 - SENSOR_SOURCE_INVALID = 3 - - -class LocationEstimator: - def __init__(self, debug: bool): - self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME) - - self.debug = debug - - self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2)) - self.car_speed = 0.0 - self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std - self.device_from_calib = np.eye(3) - - obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION] - self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} - self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} - - def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P): - self.kf.init_state(x_initial, covs=P_initial, filter_time=t) - - def _validate_sensor_source(self, source: log.SensorEventData.SensorSource): - # some segments have two IMUs, ignore the second one - return source != log.SensorEventData.SensorSource.bmx055 - - def _validate_sensor_time(self, sensor_time: float, t: float): - # ignore empty readings - if sensor_time == 0: - return False - - # sensor time and log time should be close - sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF - if sensor_time_invalid: - cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time") - return not sensor_time_invalid - - def _validate_timestamp(self, t: float): - kf_t = self.kf.t - invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME - if invalid: - cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter") - return not invalid - - def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray): - all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all() - if not all_finite: - cloudlog.error("Non-finite values detected, kalman reset") - self.reset(t) - - def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult: - new_x, new_P = None, None - if which == "accelerometer" and msg.which() == "acceleration": - sensor_time = msg.timestamp * 1e-9 - - if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): - return HandleLogResult.TIMING_INVALID - - if not self._validate_sensor_source(msg.source): - return HandleLogResult.SENSOR_SOURCE_INVALID - - v = msg.acceleration.v - meas = np.array([-v[2], -v[1], -v[0]]) - if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK: - return HandleLogResult.INPUT_INVALID - - acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas) - if acc_res is not None: - _, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res - self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err) - self.observations[ObservationKind.PHONE_ACCEL] = meas - - elif which == "gyroscope" and msg.which() == "gyroUncalibrated": - sensor_time = msg.timestamp * 1e-9 - - if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): - return HandleLogResult.TIMING_INVALID - - if not self._validate_sensor_source(msg.source): - return HandleLogResult.SENSOR_SOURCE_INVALID - - v = msg.gyroUncalibrated.v - meas = np.array([-v[2], -v[1], -v[0]]) - - gyro_bias = self.kf.x[States.GYRO_BIAS] - gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0]) - gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1] - gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold - - if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid: - return HandleLogResult.INPUT_INVALID - - gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas) - if gyro_res is not None: - _, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res - self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err) - self.observations[ObservationKind.PHONE_GYRO] = meas - - elif which == "carState": - self.car_speed = abs(msg.vEgo) - - elif which == "liveCalibration": - # Note that we use this message during calibration - if len(msg.rpyCalib) > 0: - calib = np.array(msg.rpyCalib) - if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK: - return HandleLogResult.INPUT_INVALID - - self.device_from_calib = rot_from_euler(calib) - - elif which == "cameraOdometry": - if not self._validate_timestamp(t): - return HandleLogResult.TIMING_INVALID - - rot_device = np.matmul(self.device_from_calib, np.array(msg.rot)) - trans_device = np.matmul(self.device_from_calib, np.array(msg.trans)) - - if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK: - return HandleLogResult.INPUT_INVALID - - rot_calib_std = np.array(msg.rotStd) - trans_calib_std = np.array(msg.transStd) - - if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK: - return HandleLogResult.INPUT_INVALID - - if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK: - return HandleLogResult.INPUT_INVALID - - self.posenet_stds = np.roll(self.posenet_stds, -1) - self.posenet_stds[-1] = trans_calib_std[0] - - # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise - rot_calib_std *= 10 - trans_calib_std *= 2 - - rot_device_std = rotate_std(self.device_from_calib, rot_calib_std) - trans_device_std = rotate_std(self.device_from_calib, trans_calib_std) - rot_device_noise = rot_device_std ** 2 - trans_device_noise = trans_device_std ** 2 - - cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)])) - cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)])) - self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) - if cam_odo_rot_res is not None: - _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res - self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err) - self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device - if cam_odo_trans_res is not None: - _, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res - self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err) - self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device - - if new_x is not None and new_P is not None: - self._finite_check(t, new_x, new_P) - return HandleLogResult.SUCCESS - - def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool): - state, cov = self.kf.x, self.kf.P - std = np.sqrt(np.diag(cov)) - - orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION] - velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY] - angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY] - acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION] - - msg = messaging.new_message("livePose") - msg.valid = filter_valid - - livePose = msg.livePose - init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid) - init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid) - init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) - init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) - if self.debug: - livePose.debugFilterState.value = state.tolist() - livePose.debugFilterState.std = std.tolist() - livePose.debugFilterState.valid = filter_valid - livePose.debugFilterState.observations = [ - {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()} - for k in self.observations.keys() - ] - - old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF]) - new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:]) - std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0 - - livePose.inputsOK = inputs_valid - livePose.posenetOK = not std_spike or self.car_speed <= 5.0 - livePose.sensorsOK = sensors_valid - - return msg - - -def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation): - cur_time = time.monotonic() - for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]: - if len(msgs) > 0: - sensor_valid[which] = msgs[-1].valid - sensor_recv_time[which] = cur_time - - if not simulation: - sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1 - else: - sensor_alive[which] = len(msgs) > 0 - - return all(sensor_alive.values()) and all(sensor_valid.values()) - - -def main(): - config_realtime_process([0, 1, 2, 3], 5) - - DEBUG = bool(int(os.getenv("DEBUG", "0"))) - SIMULATION = bool(int(os.getenv("SIMULATION", "0"))) - - pm = messaging.PubMaster(['livePose']) - sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry') - # separate sensor sockets for efficiency - sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']] - sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float) - - params = Params() - - estimator = LocationEstimator(DEBUG) - - filter_initialized = False - critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"] - observation_input_invalid = defaultdict(int) - - input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services} - input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services} - input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services} - - initial_pose_data = params.get("LocationFilterInitialState") - if initial_pose_data is not None: - with log.Event.from_bytes(initial_pose_data) as lp_msg: - filter_state = lp_msg.livePose.debugFilterState - x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x - P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P - estimator.reset(None, x_initial, P_initial) - - while True: - sm.update() - - acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) - - if filter_initialized: - msgs = [] - for msg in acc_msgs + gyro_msgs: - t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which()) - msgs.append((t, valid, which, data)) - for which, updated in sm.updated.items(): - if not updated: - continue - t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which] - msgs.append((t, valid, which, data)) - - for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]): - if valid: - t = log_mono_time * 1e-9 - res = estimator.handle_log(t, which, msg) - if which not in critcal_services: - continue - - if res == HandleLogResult.TIMING_INVALID: - cloudlog.warning(f"Observation {which} ignored due to failed timing check") - observation_input_invalid[which] += 1 - elif res == HandleLogResult.INPUT_INVALID: - cloudlog.warning(f"Observation {which} ignored due to failed sanity check") - observation_input_invalid[which] += 1 - elif res == HandleLogResult.SUCCESS: - observation_input_invalid[which] *= input_invalid_decay[which] - else: - filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) - - if sm.updated["cameraOdometry"]: - critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services) - inputs_valid = sm.all_valid() and critical_service_inputs_valid - sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) - - msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) - pm.send("livePose", msg) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 27cc4ef9c93fc5..3faf4f8d4e2563 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 import math import sys -from typing import Any +from typing import Any, Dict import numpy as np -from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY -from openpilot.selfdrive.locationd.models.constants import ObservationKind -from openpilot.common.swaglog import cloudlog +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from selfdrive.locationd.models.constants import ObservationKind +from system.swaglog import cloudlog from rednose.helpers.kalmanfilter import KalmanFilter @@ -15,7 +15,7 @@ import sympy as sp from rednose.helpers.ekf_sym import gen_code else: - from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module, import-error i = 0 @@ -28,7 +28,7 @@ def _slice(n): return s -class States: +class States(): # Vehicle model params STIFFNESS = _slice(1) # [-] STEER_RATIO = _slice(1) # [-] @@ -70,7 +70,7 @@ class CarKalman(KalmanFilter): ]) P_initial = Q.copy() - obs_noise: dict[int, Any] = { + 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), @@ -93,9 +93,8 @@ 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 + # vehicle models comes from The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars + # Model used is in 6.15 with formula from 6.198 # globals global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] @@ -161,18 +160,18 @@ def generate_code(generated_dir): 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) + def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None): # pylint: disable=super-init-not-called + dim_state = self.initial_x.shape[0] + dim_state_err = self.P_initial.shape[0] + x_init = self.initial_x + x_init[States.STEER_RATIO] = steer_ratio + x_init[States.STIFFNESS] = stiffness_factor + x_init[States.ANGLE_OFFSET] = angle_offset + + if P_initial is not None: + self.P_initial = P_initial + # init filter + self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog) if __name__ == "__main__": diff --git a/selfdrive/locationd/models/gnss_helpers.py b/selfdrive/locationd/models/gnss_helpers.py new file mode 100644 index 00000000000000..b6c1771ec60e72 --- /dev/null +++ b/selfdrive/locationd/models/gnss_helpers.py @@ -0,0 +1,19 @@ +import numpy as np +from laika.raw_gnss import GNSSMeasurement + +def parse_prr(m): + sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS], + m[GNSSMeasurement.SAT_VEL])) + R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2) + z_i = m[GNSSMeasurement.PRR] + return z_i, R_i, sat_pos_vel_i + +def parse_pr(m): + pseudorange = m[GNSSMeasurement.PR] + pseudorange_stdev = m[GNSSMeasurement.PR_STD] + sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS], + np.array([m[GNSSMeasurement.GLONASS_FREQ]]))) + z_i = np.atleast_1d(pseudorange) + R_i = np.atleast_2d(pseudorange_stdev**2) + return z_i, R_i, sat_pos_freq_i + diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py new file mode 100755 index 00000000000000..0d661dc3218d34 --- /dev/null +++ b/selfdrive/locationd/models/gnss_kf.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +import sys +from typing import List + +import numpy as np + +from selfdrive.locationd.models.constants import ObservationKind +from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr + +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 # pylint: disable=no-name-in-module,import-error + from rednose.helpers.ekf_sym import EKF_sym # pylint: disable=no-name-in-module,import-error + + +class States(): + ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters + ECEF_VELOCITY = slice(3, 6) + CLOCK_BIAS = slice(6, 7) # clock bias in light-meters, + CLOCK_DRIFT = slice(7, 8) # clock drift in light-meters/s, + CLOCK_ACCELERATION = slice(8, 9) # clock acceleration in light-meters/s**2 + GLONASS_BIAS = slice(9, 10) # clock drift in light-meters/s, + GLONASS_FREQ_SLOPE = slice(10, 11) # GLONASS bias in m expressed as bias + freq_num*freq_slope + + +class GNSSKalman(): + name = 'gnss' + + x_initial = np.array([-2712700.6008, -4281600.6679, 3859300.1830, + 0, 0, 0, + 0, 0, 0, + 0, 0]) + + # state covariance + P_initial = np.diag([1e16, 1e16, 1e16, + 10**2, 10**2, 10**2, + 1e14, (100)**2, (0.2)**2, + (10)**2, (1)**2]) + + maha_test_kinds: List[int] = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] + + @staticmethod + def generate_code(generated_dir): + dim_state = GNSSKalman.x_initial.shape[0] + name = GNSSKalman.name + maha_test_kinds = GNSSKalman.maha_test_kinds + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x, y, z = state[0:3, :] + v = state[3:6, :] + vx, vy, vz = v + cb, cd, ca = state[6:9, :] + glonass_bias, glonass_freq_slope = state[9:11, :] + + dt = sp.Symbol('dt') + + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[:3, :] = v + state_dot[6, 0] = cd + state_dot[7, 0] = ca + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + + # extra args + sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) + sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) + # sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) + # orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1) + + # expand extra args + sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym + sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] + # los_x, los_y, los_z = sat_los_sym + # orb_x, orb_y, orb_z = orb_epos_sym + + h_pseudorange_sym = sp.Matrix([ + sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2 + ) + cb + ]) + + h_pseudorange_glonass_sym = sp.Matrix([ + sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2 + ) + cb + glonass_bias + glonass_freq_slope * glonass_freq + ]) + + los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) + los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) + h_pseudorange_rate_sym = sp.Matrix([los_vector[0] * (sat_vx - vx) + + los_vector[1] * (sat_vy - vy) + + los_vector[2] * (sat_vz - vz) + + cd]) + + obs_eqs = [[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym], + [h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym]] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds) + + def __init__(self, generated_dir, cython=False, erratic_clock=False): + # process noise + clock_error_drift = 100.0 if erratic_clock else 0.1 + self.Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 3**2, 3**2, 3**2, + (clock_error_drift)**2, (0)**2, (0.005)**2, + .1**2, (.01)**2]) + + self.dim_state = self.x_initial.shape[0] + + # init filter + filter_cls = EKF_sym_pyx if cython else EKF_sym + self.filter = filter_cls(generated_dir, self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, + self.dim_state, maha_test_kinds=self.maha_test_kinds) + self.init_state(GNSSKalman.x_initial, covs=GNSSKalman.P_initial) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + if kind == ObservationKind.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS: + r = self.predict_and_update_pseudorange(data, t, kind) + elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: + r = self.predict_and_update_pseudorange_rate(data, t, kind) + return r + + def predict_and_update_pseudorange(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + sat_pos_freq = np.zeros((len(meas), 4)) + z = np.zeros((len(meas), 1)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_freq_i = parse_pr(m) + sat_pos_freq[i, :] = sat_pos_freq_i + z[i, :] = z_i + R[i, :, :] = R_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq) + + def predict_and_update_pseudorange_rate(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + z = np.zeros((len(meas), 1)) + sat_pos_vel = np.zeros((len(meas), 6)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_vel_i = parse_prr(m) + sat_pos_vel[i] = sat_pos_vel_i + R[i, :, :] = R_i + z[i, :] = z_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + GNSSKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc new file mode 100755 index 00000000000000..5ff0f2699555b1 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,122 @@ +#include "live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(Eigen::VectorXd& vec) { + return Eigen::Map(vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(MatrixXdr& mat) { + return Eigen::Map(mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(std::vector& vec_vec) { + std::vector> res; + for (Eigen::VectorXd& vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(std::vector& mat_vec) { + std::vector> res; + for (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.2); +} + +void LiveKalman::init_state(VectorXd& state, 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(VectorXd& state, MatrixXdr& covs, double filter_time) { + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(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, 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); +} + +Eigen::VectorXd LiveKalman::get_initial_x() { + return this->initial_x; +} + +MatrixXdr LiveKalman::get_initial_P() { + return this->initial_P; +} + +MatrixXdr LiveKalman::get_fake_gps_pos_cov() { + return this->fake_gps_pos_cov; +} + +MatrixXdr LiveKalman::get_fake_gps_vel_cov() { + return this->fake_gps_vel_cov; +} + +MatrixXdr LiveKalman::get_reset_orientation_P() { + return this->reset_orientation_P; +} + +MatrixXdr LiveKalman::H(VectorXd in) { + assert(in.size() == 6); + Matrix res; + this->filter->get_extra_routine("H")(in.data(), res.data()); + return res; +} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h new file mode 100755 index 00000000000000..06ec3854cb3d62 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,64 @@ +#pragma once + +#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(Eigen::VectorXd& vec); +Eigen::Map get_mapmat(MatrixXdr& mat); +std::vector> get_vec_mapvec(std::vector& vec_vec); +std::vector> get_vec_mapmat(std::vector& mat_vec); + +class LiveKalman { +public: + LiveKalman(); + + void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time); + void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time); + void init_state(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, 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); + + Eigen::VectorXd get_initial_x(); + MatrixXdr get_initial_P(); + MatrixXdr get_fake_gps_pos_cov(); + MatrixXdr get_fake_gps_vel_cov(); + MatrixXdr get_reset_orientation_P(); + + MatrixXdr H(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/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py new file mode 100755 index 00000000000000..023479d10e1809 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +import sys +import os +import numpy as np + +from 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: type(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: type(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/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py new file mode 100755 index 00000000000000..4c947422b15cc9 --- /dev/null +++ b/selfdrive/locationd/models/loc_kf.py @@ -0,0 +1,562 @@ +#!/usr/bin/env python3 + +import sys + +import numpy as np +import sympy as sp + +from rednose.helpers.ekf_sym import EKF_sym, gen_code +from rednose.helpers.lst_sq_computer import LstSqComputer +from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate + +from selfdrive.locationd.models.constants import ObservationKind +from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr + +EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) + +class States(): + ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters + ECEF_ORIENTATION = slice(3, 7) # quat for orientation 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 + CLOCK_BIAS = slice(13, 14) # clock bias in light-meters, + CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s, + GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases + ODO_SCALE_UNUSED = slice(18, 19) # odometer scale + ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2 + FOCAL_SCALE_UNUSED = slice(22, 23) # focal length scale + IMU_FROM_DEVICE_EULER = slice(23, 26) # imu offset angles in radians + GLONASS_BIAS = slice(26, 27) # GLONASS bias in m expressed as bias + freq_num*freq_slope + GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope + CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2, + ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer + ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer + # TODO the offset is likely a translation of the sensor, not a rotation of the camera + WIDE_FROM_DEVICE_EULER = slice(33, 36) # wide camera offset angles in radians (tici only) + # We currently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_FROM_DEVICE_EULER). + # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE + + # 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) + CLOCK_BIAS_ERR = slice(12, 13) + CLOCK_DRIFT_ERR = slice(13, 14) + GYRO_BIAS_ERR = slice(14, 17) + ODO_SCALE_ERR_UNUSED = slice(17, 18) + ACCELERATION_ERR = slice(18, 21) + FOCAL_SCALE_ERR_UNUSED = slice(21, 22) + IMU_FROM_DEVICE_EULER_ERR = slice(22, 25) + GLONASS_BIAS_ERR = slice(25, 26) + GLONASS_FREQ_SLOPE_ERR = slice(26, 27) + CLOCK_ACCELERATION_ERR = slice(27, 28) + ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29) + ACCELEROMETER_BIAS_ERR = slice(29, 32) + WIDE_FROM_DEVICE_EULER_ERR = slice(32, 35) + + +class LocKalman(): + name = "loc" + x_initial = np.array([0, 0, 0, + 1, 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, + 0, 0, 0, + 1, + 0, 0, 0, + 1, + 0, 0, 0, + 0, 0, + 0, + 1, + 0, 0, 0, + 0, 0, 0], dtype=np.float64) + + # state covariance + P_initial = np.diag([1e16, 1e16, 1e16, + 10**2, 10**2, 10**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + 1e14, (100)**2, + 0.05**2, 0.05**2, 0.05**2, + 0.02**2, + 2**2, 2**2, 2**2, + 0.01**2, + 0.01**2, 0.01**2, 0.01**2, + 10**2, 1**2, + 0.2**2, + 0.05**2, + 0.05**2, 0.05**2, 0.05**2, + 0.01**2, 0.01**2, 0.01**2]) + + + # measurements that need to pass mahalanobis distance outlier rejector + maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] + dim_augment = 7 + dim_augment_err = 6 + + @staticmethod + def generate_code(generated_dir, N=4): + dim_augment = LocKalman.dim_augment + dim_augment_err = LocKalman.dim_augment_err + + dim_main = LocKalman.x_initial.shape[0] + dim_main_err = LocKalman.P_initial.shape[0] + dim_state = dim_main + dim_augment * N + dim_state_err = dim_main_err + dim_augment_err * N + maha_test_kinds = LocKalman.maha_test_kinds + + name = f"{LocKalman.name}_{N}" + + # make functions and jacobians with sympy + # state variables + 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 + cb = state[States.CLOCK_BIAS, :] + cd = state[States.CLOCK_DRIFT, :] + roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] + acceleration = state[States.ACCELERATION, :] + imu_from_device_euler = state[States.IMU_FROM_DEVICE_EULER, :] + imu_from_device_euler[0, 0] = 0 # not observable enough + imu_from_device_euler[2, 0] = 0 # not observable enough + glonass_bias = state[States.GLONASS_BIAS, :] + glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :] + ca = state[States.CLOCK_ACCELERATION, :] + accel_bias = state[States.ACCELEROMETER_BIAS, :] + wide_from_device_euler = state[States.WIDE_FROM_DEVICE_EULER, :] + wide_from_device_euler[0, 0] = 0 # not observable enough + + 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 + state_dot[States.CLOCK_BIAS, :] = cd + state_dot[States.CLOCK_DRIFT, :] = ca + + # 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, :] + cd_err = state_err[States.CLOCK_DRIFT_ERR, :] + acceleration_err = state_err[States.ACCELERATION_ERR, :] + ca_err = state_err[States.CLOCK_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) + state_err_dot[States.CLOCK_BIAS_ERR, :] = cd_err + state_err_dot[States.CLOCK_DRIFT_ERR, :] = ca_err + f_err_sym = state_err + dt * state_err_dot + + # convenient indexing + # q idxs are for quats and p idxs are for other + q_idxs = [[3, dim_augment]] + [[dim_main + n * dim_augment + 3, dim_main + (n + 1) * dim_augment] for n in range(N)] + q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n * dim_augment_err + 3, dim_main_err + (n + 1) * dim_augment_err] for n in range(N)] + p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n * dim_augment, dim_main + n * dim_augment + 3] for n in range(N)] + p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n * dim_augment_err, dim_main_err + n * dim_augment_err + 3] for n in range(N)] + + # Observation matrix modifier + H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:] + + # 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))) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + delta_quat = sp.Matrix(np.ones(4)) + delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :]) + err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) + + inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] + inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) + + 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 + # + + # extra args + sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) + sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) + # sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) + + # expand extra args + sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym + sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] + + h_pseudorange_sym = sp.Matrix([ + sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2 + ) + cb[0] + ]) + + h_pseudorange_glonass_sym = sp.Matrix([ + sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2 + ) + cb[0] + glonass_bias[0] + glonass_freq_slope[0] * glonass_freq + ]) + + los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) + los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) + h_pseudorange_rate_sym = sp.Matrix([los_vector[0] * (sat_vx - vx) + + los_vector[1] * (sat_vy - vy) + + los_vector[2] * (sat_vz - vz) + + cd[0]]) + + imu_from_device = euler_rotate(*imu_from_device_euler) + h_gyro_sym = imu_from_device * sp.Matrix([vroll + roll_bias, + vpitch + pitch_bias, + vyaw + yaw_bias]) + + pos = sp.Matrix([x, y, z]) + # add 1 for stability, prevent division by 0 + gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos) + h_acc_sym = imu_from_device * (gravity + acceleration + accel_bias) + h_acc_stationary_sym = acceleration + h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) + 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_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym], + [h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym], + [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]] + + wide_from_device = euler_rotate(*wide_from_device_euler) + # MSCKF configuration + if N > 0: + # experimentally found this is correct value for imx298 with 910 focal length + # this is a variable so it can change with focus, but we disregard that for now + # TODO: this isn't correct for tici + focal_scale = 1.01 + # Add observation functions for orb feature tracks + track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1) + track_x, track_y, track_z = track_epos_sym + h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1))) + h_track_wide_cam_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1))) + + track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) + track_pos_rot_sym = quat_rot.T * track_pos_sym + track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym + h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), + focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) + h_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), + focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) + + h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1))) + h_msckf_test_sym[-3:, :] = track_pos_sym + + for n in range(N): + idx = dim_main + n * dim_augment + # err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used? + x, y, z = state[idx:idx + 3] + q = state[idx + 3:idx + 7] + quat_rot = quat_rotate(*q) + track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) + track_pos_rot_sym = quat_rot.T * track_pos_sym + track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym + h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), + focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) + h_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), + focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) + h_msckf_test_sym[n * 3:n * 3 + 3, :] = track_pos_sym + + obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym]) + obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym]) + obs_eqs.append([h_track_wide_cam_sym, ObservationKind.ORB_FEATURES_WIDE, track_epos_sym]) + obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym]) + msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE]] + else: + msckf_params = None + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds) + + def __init__(self, generated_dir, N=4, erratic_clock=False): + name = f"{self.name}_{N}" + + + # process noise + clock_error_drift = 100.0 if erratic_clock else 0.1 + self.Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 0.0**2, 0.0**2, 0.0**2, + 0.0**2, 0.0**2, 0.0**2, + 0.1**2, 0.1**2, 0.1**2, + (clock_error_drift)**2, (0)**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.02 / 100)**2, + 3**2, 3**2, 3**2, + 0.001**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2, + (.1)**2, (.01)**2, + 0.005**2, + (0.02 / 100)**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + + + self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), + ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.diag([0.0025**2, 0.0025**2, 0.0025**2]), + ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]), + ObservationKind.NO_ACCEL: np.diag([0.0025**2, 0.0025**2, 0.0025**2])} + + # MSCKF stuff + self.N = N + self.dim_main = LocKalman.x_initial.shape[0] + self.dim_main_err = LocKalman.P_initial.shape[0] + self.dim_state = self.dim_main + self.dim_augment * self.N + self.dim_state_err = self.dim_main_err + self.dim_augment_err * self.N + + if self.N > 0: + x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q) # lgtm[py/mismatched-multiple-assignment] pylint: disable=unbalanced-tuple-unpacking + self.computer = LstSqComputer(generated_dir, N) + + self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)] + + # init filter + self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err, + N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs) + + @property + def x(self): + return self.filter.state() + + @property + def t(self): + return self.filter.get_filter_time() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=True) + + def pad_augmented(self, x, P, Q=None): + if x.shape[0] == self.dim_main and self.N > 0: + x = np.pad(x, (0, self.N * self.dim_augment), mode='constant') + x[self.dim_main + 3::7] = 1 + if P.shape[0] == self.dim_main_err and self.N > 0: + P = np.pad(P, [(0, self.N * self.dim_augment_err), (0, self.N * self.dim_augment_err)], mode='constant') + P[self.dim_main_err:, self.dim_main_err:] = 10e20 * np.eye(self.dim_augment_err * self.N) + if Q is None: + return x, P + else: + Q = np.pad(Q, [(0, self.N * self.dim_augment_err), (0, self.N * self.dim_augment_err)], mode='constant') + return x, P, Q + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + state, P = self.pad_augmented(state, P) + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + if kind == ObservationKind.CAMERA_ODO_TRANSLATION: + r = self.predict_and_update_odo_trans(data, t, kind) + elif kind == ObservationKind.CAMERA_ODO_ROTATION: + r = self.predict_and_update_odo_rot(data, t, kind) + elif kind == ObservationKind.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS: + r = self.predict_and_update_pseudorange(data, t, kind) + elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: + r = self.predict_and_update_pseudorange_rate(data, t, kind) + elif kind == ObservationKind.ORB_FEATURES or kind == ObservationKind.ORB_FEATURES_WIDE: + r = self.predict_and_update_orb_features(data, t, kind) + elif kind == ObservationKind.MSCKF_TEST: + r = self.predict_and_update_msckf_test(data, t, kind) + else: + r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + # Normalize quats + quat_norm = np.linalg.norm(self.filter.state()[3:7]) + # Should not continue if the quats behave this weirdly + if not 0.1 < quat_norm < 10: + raise RuntimeError("Sir! The filter's gone all wobbly!") + return r + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + def predict_and_update_pseudorange(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + sat_pos_freq = np.zeros((len(meas), 4)) + z = np.zeros((len(meas), 1)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_freq_i = parse_pr(m) + sat_pos_freq[i, :] = sat_pos_freq_i + z[i, :] = z_i + R[i, :, :] = R_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq) + + def predict_and_update_pseudorange_rate(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + z = np.zeros((len(meas), 1)) + sat_pos_vel = np.zeros((len(meas), 6)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_vel_i = parse_prr(m) + sat_pos_vel[i] = sat_pos_vel_i + R[i, :, :] = R_i + z[i, :] = z_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel) + + def predict_and_update_odo_trans(self, trans, t, kind): + z = trans[:, :3] + R = np.zeros((len(trans), 3, 3)) + for i, _ in enumerate(z): + R[i, :, :] = np.diag(trans[i, 3:]**2) + return self.filter.predict_and_update_batch(t, kind, z, R) + + def predict_and_update_odo_rot(self, rot, t, kind): + z = rot[:, :3] + R = np.zeros((len(rot), 3, 3)) + for i, _ in enumerate(z): + R[i, :, :] = np.diag(rot[i, 3:]**2) + return self.filter.predict_and_update_batch(t, kind, z, R) + + def predict_and_update_orb_features(self, tracks, t, kind): + k = 2 * (self.N + 1) + R = np.zeros((len(tracks), k, k)) + z = np.zeros((len(tracks), k)) + ecef_pos = np.zeros((len(tracks), 3)) + ecef_pos[:] = np.nan + poses = self.x[self.dim_main:].reshape((-1, 7)) + times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0] + if kind==ObservationKind.ORB_FEATURES: + pt_std = 0.005 + else: + pt_std = 0.02 + if times.any(): + assert np.allclose(times[0, :-1], self.filter.get_augment_times(), atol=1e-7, rtol=0.0) + for i, track in enumerate(tracks): + img_positions = track.reshape((self.N + 1, 4))[:, 2:] + + # TODO not perfect as last pose not used + # img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i]) + + ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1]) + z[i] = img_positions.flatten() + R[i, :, :] = np.diag([pt_std**2] * (k)) + + good_idxs = np.all(np.isfinite(ecef_pos), axis=1) + + # This code relies on wide and narrow orb features being captured at the same time, + # and wide features to be processed first. + ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], + augment=kind==ObservationKind.ORB_FEATURES) + if ret is None: + return + + # have to do some weird stuff here to keep + # to have the observations input from mesh3d + # consistent with the outputs of the filter + # Probably should be replaced, not sure how. + y_full = np.zeros((z.shape[0], z.shape[1] - 3)) + if sum(good_idxs) > 0: + y_full[good_idxs] = np.array(ret[6]) + ret = ret[:6] + (y_full, z, ecef_pos) + return ret + + def predict_and_update_msckf_test(self, test_data, t, kind): + assert self.N > 0 + z = test_data + R = np.zeros((len(test_data), len(z[0]), len(z[0]))) + ecef_pos = [self.x[:3]] + for i, _ in enumerate(z): + R[i, :, :] = np.diag([0.1**2] * len(z[0])) + ret = self.filter.predict_and_update_batch(t, kind, z, R, ecef_pos) + self.filter.augment() + return ret + + def maha_test_pseudorange(self, x, P, meas, kind, maha_thresh=.3): + bools = [] + for m in meas: + z, R, sat_pos_freq = parse_pr(m) + bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_freq, maha_thresh=maha_thresh)) + return np.array(bools) + + def maha_test_pseudorange_rate(self, x, P, meas, kind, maha_thresh=.999): + bools = [] + for m in meas: + z, R, sat_pos_vel = parse_prr(m) + bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_vel, maha_thresh=maha_thresh)) + return np.array(bools) + + +if __name__ == "__main__": + N = int(sys.argv[1].split("_")[-1]) + generated_dir = sys.argv[2] + LocKalman.generate_code(generated_dir, N=N) diff --git a/selfdrive/locationd/models/pose_kf.py b/selfdrive/locationd/models/pose_kf.py deleted file mode 100755 index 020e51ad6e50a4..00000000000000 --- a/selfdrive/locationd/models/pose_kf.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import numpy as np - -from openpilot.selfdrive.locationd.models.constants import ObservationKind - -from rednose.helpers.kalmanfilter import KalmanFilter - -if __name__=="__main__": - import sympy as sp - from rednose.helpers.ekf_sym import gen_code - from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler -else: - from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx - -EARTH_G = 9.81 - - -class States: - NED_ORIENTATION = slice(0, 3) # roll, pitch, yaw in rad - DEVICE_VELOCITY = slice(3, 6) # ned velocity in m/s - ANGULAR_VELOCITY = slice(6, 9) # roll, pitch and yaw rates in rad/s - GYRO_BIAS = slice(9, 12) # roll, pitch and yaw gyroscope biases in rad/s - ACCELERATION = slice(12, 15) # acceleration in device frame in m/s**2 - ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2 - - -class PoseKalman(KalmanFilter): - name = "pose" - - # state - initial_x = np.array([0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0]) - # state covariance - initial_P = np.diag([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]) - - # process noise - Q = np.diag([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 = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), - ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), - ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]), - ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])} - - @staticmethod - def generate_code(generated_dir): - name = PoseKalman.name - dim_state = PoseKalman.initial_x.shape[0] - dim_state_err = PoseKalman.initial_P.shape[0] - - state_sym = sp.MatrixSymbol('state', dim_state, 1) - state = sp.Matrix(state_sym) - roll, pitch, yaw = state[States.NED_ORIENTATION, :] - velocity = state[States.DEVICE_VELOCITY, :] - angular_velocity = state[States.ANGULAR_VELOCITY, :] - vroll, vpitch, vyaw = angular_velocity - gyro_bias = state[States.GYRO_BIAS, :] - acceleration = state[States.ACCELERATION, :] - acc_bias = state[States.ACCEL_BIAS, :] - - dt = sp.Symbol('dt') - - ned_from_device = euler_rotate(roll, pitch, yaw) - device_from_ned = ned_from_device.T - - state_dot = sp.Matrix(np.zeros((dim_state, 1))) - state_dot[States.DEVICE_VELOCITY, :] = acceleration - - f_sym = state + dt * state_dot - device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw) - ned_from_device_t1 = ned_from_device * device_from_device_t1 - f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1) - - centripetal_acceleration = angular_velocity.cross(velocity) - gravity = sp.Matrix([0, 0, -EARTH_G]) - h_gyro_sym = angular_velocity + gyro_bias - h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias - h_phone_rot_sym = angular_velocity - h_relative_motion_sym = velocity - obs_eqs = [ - [h_gyro_sym, ObservationKind.PHONE_GYRO, None], - [h_acc_sym, ObservationKind.PHONE_ACCEL, None], - [h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None], - [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], - ] - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err) - - def __init__(self, generated_dir, max_rewind_age): - dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0] - self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, - dim_state, dim_state_err, max_rewind_age=max_rewind_age) - - -if __name__ == "__main__": - generated_dir = sys.argv[2] - PoseKalman.generate_code(generated_dir) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index fd03d3d09368f7..86672b046036d7 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -1,85 +1,54 @@ #!/usr/bin/env python3 -import os +import math +import json import numpy as np -import capnp import cereal.messaging as messaging -from cereal import car, log -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, DT_MDL -from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States -from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose -from openpilot.common.swaglog import cloudlog +from cereal import car +from common.params import Params, put_nonblocking +from common.realtime import config_realtime_process, DT_MDL +from common.numpy_fast import clip +from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States +from selfdrive.locationd.models.constants import GENERATED_DIR +from system.swaglog import cloudlog + MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits -ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10) -ROLL_LOWERED_MAX = np.radians(8) -ROLL_STD_MAX = np.radians(1.5) +ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 -OFFSET_MAX = 10.0 -OFFSET_LOWERED_MAX = 8.0 -MIN_ACTIVE_SPEED = 1.0 -LOW_ACTIVE_SPEED = 10.0 - - -class VehicleParamsLearner: - def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): - self.kf = CarKalman(GENERATED_DIR) - - self.x_initial = CarKalman.initial_x.copy() - self.x_initial[States.STEER_RATIO] = steer_ratio - self.x_initial[States.STIFFNESS] = stiffness_factor - self.x_initial[States.ANGLE_OFFSET] = angle_offset - self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial - - self.kf.set_globals( - mass=CP.mass, - rotational_inertia=CP.rotationalInertia, - center_to_front=CP.centerToFront, - center_to_rear=CP.wheelbase - CP.centerToFront, - stiffness_front=CP.tireStiffnessFront, - stiffness_rear=CP.tireStiffnessRear - ) - - self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio - - self.calibrator = PoseCalibrator() - - self.observed_speed = 0.0 - self.observed_yaw_rate = 0.0 - self.observed_roll = 0.0 - - self.avg_offset_valid = True - self.total_offset_valid = True - self.roll_valid = True - - self.reset(None) - - def reset(self, t: float | None): - self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t) - - self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False - self.avg_angle_offset = self.angle_offset - - def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): - if which == 'livePose': - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - - yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std - yaw_rate_valid = msg.angularVelocityDevice.valid - yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s - yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s - if not yaw_rate_valid: - # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating - yaw_rate, yaw_rate_std = 0.0, np.radians(10.0) - self.observed_yaw_rate = yaw_rate - - localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std - localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std - roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK + + +class ParamsLearner: + def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None): + self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial) + + self.kf.filter.set_global("mass", CP.mass) + self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) + self.kf.filter.set_global("center_to_front", CP.centerToFront) + self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront) + self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront) + self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) + + self.active = False + + self.speed = 0.0 + self.yaw_rate = 0.0 + self.yaw_rate_std = 0.0 + self.roll = 0.0 + self.steering_pressed = False + self.steering_angle = 0.0 + + self.valid = True + + def handle_log(self, t, which, msg): + if which == 'liveLocationKalman': + self.yaw_rate = msg.angularVelocityCalibrated.value[2] + self.yaw_rate_std = msg.angularVelocityCalibrated.std[2] + + localizer_roll = msg.orientationNED.value[0] + localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] + roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX if roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? @@ -88,193 +57,107 @@ def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): # This is done to bound the road roll estimate when localizer values are invalid roll = 0.0 roll_std = np.radians(10.0) - self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA) + self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + + yaw_rate_valid = msg.angularVelocityCalibrated.valid + yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s + yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s if self.active: if msg.posenetOK: - self.kf.predict_and_observe(t, - ObservationKind.ROAD_FRAME_YAW_RATE, - np.array([[-self.observed_yaw_rate]]), - np.array([np.atleast_2d(yaw_rate_std**2)])) + + if yaw_rate_valid: + self.kf.predict_and_observe(t, + ObservationKind.ROAD_FRAME_YAW_RATE, + np.array([[-self.yaw_rate]]), + np.array([np.atleast_2d(self.yaw_rate_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ROAD_ROLL, - np.array([[self.observed_roll]]), + np.array([[self.roll]]), np.array([np.atleast_2d(roll_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) # We observe the current stiffness and steer ratio (with a high observation noise) to bound # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the # states in longer routes (especially straight stretches). - stiffness = float(self.kf.x[States.STIFFNESS].item()) - steer_ratio = float(self.kf.x[States.STEER_RATIO].item()) + stiffness = float(self.kf.x[States.STIFFNESS]) + steer_ratio = float(self.kf.x[States.STEER_RATIO]) self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) - elif which == 'liveCalibration': - self.calibrator.feed_live_calib(msg) - elif which == 'carState': - steering_angle = msg.steeringAngleDeg + self.steering_angle = msg.steeringAngleDeg + self.steering_pressed = msg.steeringPressed + self.speed = msg.vEgo - in_linear_region = abs(steering_angle) < 45 - self.observed_speed = msg.vEgo - self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region + in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed + self.active = self.speed > 5 and in_linear_region if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]])) - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]])) if not self.active: # Reset time when stopped so uncertainty doesn't grow self.kf.filter.set_filter_time(t) self.kf.filter.reset_rewind() - def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: - x = self.kf.x - P = np.sqrt(self.kf.P.diagonal()) - if not np.all(np.isfinite(x)): - cloudlog.error("NaN in liveParameters estimate. Resetting to default values") - self.reset(self.kf.t) - x = self.kf.x - - self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()), - self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA) - self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()), - self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA) - self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) - roll_std = float(P[States.ROAD_ROLL].item()) - if self.active and self.observed_speed > LOW_ACTIVE_SPEED: - # Account for the opposite signs of the yaw rates - # At low speeds, bumping into a curb can cause the yaw rate to be very high - sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) - else: - sensors_valid = True - self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX) - - msg = messaging.new_message('liveParameters') - - msg.valid = valid - - liveParameters = msg.liveParameters - liveParameters.posenetValid = True - liveParameters.sensorValid = sensors_valid - liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) - liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) - liveParameters.roll = float(self.roll) - liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) - liveParameters.angleOffsetDeg = float(self.angle_offset) - liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr - liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 - liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid) - liveParameters.angleOffsetValid = bool(self.total_offset_valid) - liveParameters.valid = all(( - liveParameters.angleOffsetAverageValid, - liveParameters.angleOffsetValid , - self.roll_valid, - roll_std < ROLL_STD_MAX, - liveParameters.stiffnessFactorValid, - liveParameters.steerRatioValid, - )) - liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) - liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) - liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) - liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item()) - if debug: - liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message() - liveParameters.debugFilterState.value = x.tolist() - liveParameters.debugFilterState.std = P.tolist() - - return msg - - -def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float): - if current_valid: - current_valid = abs(val) < threshold - else: - current_valid = abs(val) < lowered_threshold - return current_valid - - -# TODO: Remove this function after few releases (added in 0.9.9) -def migrate_cached_vehicle_params_if_needed(params: Params): - last_parameters_data_old = params.get("LiveParameters") - last_parameters_data = params.get("LiveParametersV2") - if last_parameters_data_old is None or last_parameters_data is not None: - return - - try: - last_parameters_msg = messaging.new_message('liveParameters') - last_parameters_msg.liveParameters.valid = True - last_parameters_msg.liveParameters.steerRatio = last_parameters_data_old['steerRatio'] - last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor'] - last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg'] - params.put("LiveParametersV2", last_parameters_msg.to_bytes()) - except Exception as e: - cloudlog.error(f"Failed to perform parameter migration: {e}") - params.remove("LiveParameters") - - -def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool): - last_parameters_data = params.get("LiveParametersV2") - last_carparams_data = params.get("CarParamsPrevRoute") - - steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None - - retrieve_success = False - if last_parameters_data is not None and last_carparams_data is not None: - try: - with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: - lp = last_lp_msg.liveParameters - # Check if car model matches - if last_CP.carFingerprint != CP.carFingerprint: - raise Exception("Car model mismatch") - - # Check if starting values are sane - min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio - steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr - if not steer_ratio_sane: - raise Exception(f"Invalid starting values found {lp}") - - initial_filter_std = np.array(lp.debugFilterState.std) - if debug and len(initial_filter_std) != 0: - p_initial = np.diag(initial_filter_std) - - steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg - retrieve_success = True - except Exception as e: - cloudlog.error(f"Failed to retrieve initial values: {e}") - params.remove("LiveParametersV2") - if not replay: - # When driving in wet conditions the stiffness can go down, and then be too low on the next drive - # Without a way to detect this we have to reset the stiffness every drive - stiffness_factor = 1.0 - - if not retrieve_success: - cloudlog.info("Parameter learner resetting to default values") - - return steer_ratio, stiffness_factor, angle_offset_deg, p_initial +def main(sm=None, pm=None): + config_realtime_process([0, 1, 2, 3], 5) + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) + if pm is None: + pm = messaging.PubMaster(['liveParameters']) -def main(): - config_realtime_process([0, 1, 2, 3], 5) + params_reader = Params() + # wait for stats about the car to come in from controls + cloudlog.info("paramsd is waiting for CarParams") + CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) + cloudlog.info("paramsd got CarParams") - DEBUG = bool(int(os.getenv("DEBUG", "0"))) - REPLAY = bool(int(os.getenv("REPLAY", "0"))) + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio - pm = messaging.PubMaster(['liveParameters']) - sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose') + params = params_reader.get("LiveParameters") - params = Params() - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + # Check if car model matches + if params is not None: + params = json.loads(params) + if params.get('carFingerprint', None) != CP.carFingerprint: + cloudlog.info("Parameter learner found parameters for wrong car.") + params = None - migrate_cached_vehicle_params_if_needed(params) + # Check if starting values are sane + if params is not None: + try: + angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 + steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr + params_sane = angle_offset_sane and steer_ratio_sane + if not params_sane: + cloudlog.info(f"Invalid starting values found {params}") + params = None + except Exception as e: + cloudlog.info(f"Error reading params {params}: {str(e)}") + params = None + + # TODO: cache the params with the capnp struct + if params is None: + params = { + 'carFingerprint': CP.carFingerprint, + 'steerRatio': CP.steerRatio, + 'stiffnessFactor': 1.0, + 'angleOffsetAverageDeg': 0.0, + } + cloudlog.info("Parameter learner resetting to default values") - steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG) - learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) + # When driving in wet conditions the stiffness can go down, and then be too low on the next drive + # Without a way to detect this we have to reset the stiffness every drive + params['stiffnessFactor'] = 1.0 + learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) + angle_offset_average = params['angleOffsetAverageDeg'] + angle_offset = angle_offset_average while True: sm.update() @@ -284,14 +167,52 @@ def main(): t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) - if sm.updated['livePose']: - msg = learner.get_msg(sm.all_checks(), debug=DEBUG) + if sm.updated['liveLocationKalman']: + x = learner.kf.x + P = np.sqrt(learner.kf.P.diagonal()) + if not all(map(math.isfinite, x)): + cloudlog.error("NaN in liveParameters estimate. Resetting to default values") + learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) + x = learner.kf.x - msg_dat = msg.to_bytes() - if sm.frame % 1200 == 0: # once a minute - params.put_nonblocking("LiveParametersV2", msg_dat) + angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) + angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) + # Account for the opposite signs of the yaw rates + sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) + + msg = messaging.new_message('liveParameters') + + liveParameters = msg.liveParameters + liveParameters.posenetValid = True + liveParameters.sensorValid = sensors_valid + liveParameters.steerRatio = float(x[States.STEER_RATIO]) + liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) + liveParameters.roll = float(x[States.ROAD_ROLL]) + liveParameters.angleOffsetAverageDeg = angle_offset_average + liveParameters.angleOffsetDeg = angle_offset + liveParameters.valid = all(( + abs(liveParameters.angleOffsetAverageDeg) < 10.0, + abs(liveParameters.angleOffsetDeg) < 10.0, + 0.2 <= liveParameters.stiffnessFactor <= 5.0, + min_sr <= liveParameters.steerRatio <= max_sr, + )) + liveParameters.steerRatioStd = float(P[States.STEER_RATIO]) + liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS]) + liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET]) + liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST]) + + msg.valid = sm.all_checks() - pm.send('liveParameters', msg_dat) + if sm.frame % 1200 == 0: # once a minute + params = { + 'carFingerprint': CP.carFingerprint, + 'steerRatio': liveParameters.steerRatio, + 'stiffnessFactor': liveParameters.stiffnessFactor, + 'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg, + } + put_nonblocking("LiveParameters", json.dumps(params)) + + pm.send('liveParameters', msg) if __name__ == "__main__": diff --git a/selfdrive/locationd/test/_test_locationd_lib.py b/selfdrive/locationd/test/_test_locationd_lib.py new file mode 100755 index 00000000000000..8a0ed3ef05923e --- /dev/null +++ b/selfdrive/locationd/test/_test_locationd_lib.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +"""This test can't be run together with other locationd tests. +cffi.dlopen breaks the list of registered filters.""" +import os +import random +import unittest + +from cffi import FFI + +import cereal.messaging as messaging +from cereal import log + +SENSOR_DECIMATION = 1 +VISION_DECIMATION = 1 + +LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so')) + + +class TestLocationdLib(unittest.TestCase): + def setUp(self): + header = '''typedef ...* Localizer_t; +Localizer_t localizer_init(); +void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); +void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' + + self.ffi = FFI() + self.ffi.cdef(header) + self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) + + self.localizer = self.lib.localizer_init() + + self.buff_size = 2048 + self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') + + def localizer_handle_msg(self, msg_builder): + bytstr = msg_builder.to_bytes() + self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr)) + + def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True): + self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size) + return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8) + + def test_liblocalizer(self): + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = random.randint(1, 10) + msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)] + + self.localizer_handle_msg(msg) + liveloc = self.localizer_get_msg() + self.assertTrue(liveloc is not None) + + @unittest.skip("temporarily disabled due to false positives") + def test_device_fell(self): + msg = messaging.new_message('sensorEvents', 1) + msg.sensorEvents[0].sensor = 1 + msg.sensorEvents[0].timestamp = msg.logMonoTime + msg.sensorEvents[0].type = 1 + msg.sensorEvents[0].init('acceleration') + msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0] # zero with gravity + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertTrue(ret.liveLocationKalman.deviceStable) + + msg = messaging.new_message('sensorEvents', 1) + msg.sensorEvents[0].sensor = 1 + msg.sensorEvents[0].timestamp = msg.logMonoTime + msg.sensorEvents[0].type = 1 + msg.sensorEvents[0].init('acceleration') + msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2 + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertFalse(ret.liveLocationKalman.deviceStable) + + def test_posenet_spike(self): + for _ in range(SENSOR_DECIMATION): + msg = messaging.new_message('carState') + msg.carState.vEgo = 6.0 # more than 5 m/s + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertTrue(ret.liveLocationKalman.posenetOK) + + for _ in range(20 * VISION_DECIMATION): # size of hist_old + msg = messaging.new_message('cameraOdometry') + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [2.0, 0.1, 0.1] + self.localizer_handle_msg(msg) + + for _ in range(20 * VISION_DECIMATION): # size of hist_new + msg = messaging.new_message('cameraOdometry') + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger + self.localizer_handle_msg(msg) + + ret = self.localizer_get_msg() + self.assertFalse(ret.liveLocationKalman.posenetOK) + +if __name__ == "__main__": + unittest.main() + diff --git a/selfdrive/locationd/test/print_gps_stats.py b/selfdrive/locationd/test/print_gps_stats.py new file mode 100755 index 00000000000000..cfddb7f8b8021b --- /dev/null +++ b/selfdrive/locationd/test/print_gps_stats.py @@ -0,0 +1,20 @@ +#!/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("Sats: %d Accuracy: %.2f m cnos" % (ug.measurementReport.numMeas, gle.accuracy), sorted(cnos)) + except Exception: + pass + sm.update() + time.sleep(0.1) diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py old mode 100644 new mode 100755 index df61b6a7c7e3ef..3612f48276a632 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -1,112 +1,26 @@ +#!/usr/bin/env python3 import random +import unittest import numpy as np import cereal.messaging as messaging -from cereal import log -from openpilot.common.params import Params -from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \ - MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD +from common.params import Params +from selfdrive.locationd.calibrationd import Calibrator -def process_messages(c, cam_odo_calib, cycles, - cam_odo_speed=MIN_SPEED_FILTER + 1, - carstate_speed=MIN_SPEED_FILTER + 1, - cam_odo_yr=0.0, - cam_odo_speed_std=1e-3, - cam_odo_height_std=1e-3): - old_rpy_weight_prev = 0.0 - for _ in range(cycles): - assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3) - old_rpy_weight_prev = c.old_rpy_weight - c.handle_v_ego(carstate_speed) - c.handle_cam_odom([cam_odo_speed, - np.sin(cam_odo_calib[2]) * cam_odo_speed, - -np.sin(cam_odo_calib[1]) * cam_odo_speed], - [0.0, 0.0, cam_odo_yr], - [0.0, 0.0, 0.0], - [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std], - [0.0, 0.0, HEIGHT_INIT.item()], - [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std]) - -class TestCalibrationd: +class TestCalibrationd(unittest.TestCase): def test_read_saved_params(self): msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = random.randint(1, 10) msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)] - msg.liveCalibration.height = [random.random() for _ in range(1)] Params().put("CalibrationParams", msg.to_bytes()) c = Calibrator(param_put=True) np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) - np.testing.assert_allclose(msg.liveCalibration.height, c.height) - assert msg.liveCalibration.validBlocks == c.valid_blocks - - - def test_calibration_basics(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED) - assert c.valid_blocks == INPUTS_WANTED - np.testing.assert_allclose(c.rpy, np.zeros(3)) - np.testing.assert_allclose(c.height, HEIGHT_INIT) - c.reset() - - - def test_calibration_low_speed_reject(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1) - assert c.valid_blocks == 0 - np.testing.assert_allclose(c.rpy, np.zeros(3)) - np.testing.assert_allclose(c.height, HEIGHT_INIT) - - - def test_calibration_yaw_rate_reject(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER) - assert c.valid_blocks == 0 - np.testing.assert_allclose(c.rpy, np.zeros(3)) - np.testing.assert_allclose(c.height, HEIGHT_INIT) - - - def test_calibration_speed_std_reject(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3) - assert c.valid_blocks == INPUTS_NEEDED - np.testing.assert_allclose(c.rpy, np.zeros(3)) - - - def test_calibration_speed_std_height_reject(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3) - assert c.valid_blocks == INPUTS_NEEDED - np.testing.assert_allclose(c.rpy, np.zeros(3)) - - - def test_calibration_auto_reset(self): - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) - assert c.valid_blocks == INPUTS_NEEDED - np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3) - process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10) - assert c.valid_blocks == INPUTS_NEEDED + 1 - assert c.cal_status == log.LiveCalibrationData.Status.calibrated + self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks) - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) - assert c.valid_blocks == INPUTS_NEEDED - np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) - process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10) - assert c.valid_blocks == 1 - assert c.cal_status == log.LiveCalibrationData.Status.recalibrating - np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2) - c = Calibrator(param_put=False) - process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) - assert c.valid_blocks == INPUTS_NEEDED - np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) - process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10) - assert c.valid_blocks == 1 - assert c.cal_status == log.LiveCalibrationData.Status.recalibrating - np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2) +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py deleted file mode 100644 index a3dfce9c296c44..00000000000000 --- a/selfdrive/locationd/test/test_lagd.py +++ /dev/null @@ -1,136 +0,0 @@ -import random -import numpy as np -import time -import pytest - -from cereal import messaging, log, car -from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ - BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC -from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams -from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE -from openpilot.common.params import Params -from openpilot.tools.lib.logreader import LogReader -from openpilot.system.hardware import PC - -MAX_ERR_FRAMES = 1 -DT = 0.05 - - -def process_messages(estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0): - for i in range(n_frames): - t = i * estimator.dt - desired_la = np.cos(10 * t) * 0.1 - actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.1 - - # if sample is masked out, set it to desired value (no lag) - rejected = random.uniform(0, 1) < rejection_threshold - if rejected: - actual_la = desired_la - - desired_cuvature = float(desired_la / (vego ** 2)) - actual_yr = float(actual_la / vego) - msgs = [ - (t, "carControl", car.CarControl(latActive=not rejected)), - (t, "carState", car.CarState(vEgo=vego, steeringPressed=False)), - (t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)), - (t, "livePose", log.LivePose(angularVelocityDevice=log.LivePose.XYZMeasurement(z=actual_yr, valid=True), - posenetOK=True, inputsOK=True)), - (t, "liveCalibration", log.LiveCalibrationData(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), - ] - for t, w, m in msgs: - estimator.handle_log(t, w, m) - estimator.update_points() - estimator.update_estimate() - - -class TestLagd: - def test_read_saved_params(self): - params = Params() - - lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) - CP = next(m for m in lr if m.which() == "carParams").carParams - - msg = messaging.new_message('liveDelay') - msg.liveDelay.lateralDelayEstimate = random.random() - msg.liveDelay.validBlocks = random.randint(1, 10) - params.put("LiveDelay", msg.to_bytes()) - params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) - - saved_lag_params = retrieve_initial_lag(params, CP) - assert saved_lag_params is not None - - lag, valid_blocks = saved_lag_params - assert lag == msg.liveDelay.lateralDelayEstimate - assert valid_blocks == msg.liveDelay.validBlocks - - def test_ncc(self): - lag_frames = random.randint(1, 19) - - desired_sig = np.sin(np.arange(0.0, 10.0, 0.1)) - actual_sig = np.sin(np.arange(0.0, 10.0, 0.1) - lag_frames * 0.1) - mask = np.ones(len(desired_sig), dtype=bool) - - corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20] - assert np.argmax(corr) == lag_frames - - # add some noise - desired_sig += np.random.normal(0, 0.05, len(desired_sig)) - actual_sig += np.random.normal(0, 0.05, len(actual_sig)) - corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20] - assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1) - - # mask out 40% of the values, and make them noise - mask = np.random.choice([True, False], size=len(desired_sig), p=[0.6, 0.4]) - desired_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask)) - actual_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask)) - corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20] - assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1) - - def test_empty_estimator(self): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) - estimator = LateralLagEstimator(mocked_CP, DT) - msg = estimator.get_msg(True) - assert msg.liveDelay.status == 'unestimated' - assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag) - assert msg.liveDelay.validBlocks == 0 - assert msg.liveDelay.calPerc == 0 - - def test_estimator_basics(self, subtests): - for lag_frames in range(5): - with subtests.test(msg=f"lag_frames={lag_frames}"): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) - estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0) - process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) - msg = estimator.get_msg(True) - assert msg.liveDelay.status == 'estimated' - assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) - assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED - assert msg.liveDelay.calPerc == 100 - - def test_estimator_masking(self): - mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(1, 19) - estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1) - process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4) - msg = estimator.get_msg(True) - assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) - assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) - assert msg.liveDelay.calPerc == 100 - - @pytest.mark.skipif(PC, reason="only on device") - @pytest.mark.timeout(60) - def test_estimator_performance(self): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) - estimator = LateralLagEstimator(mocked_CP, DT) - - ds = [] - for _ in range(1000): - st = time.perf_counter() - estimator.update_points() - estimator.update_estimate() - d = time.perf_counter() - st - ds.append(d) - - assert np.mean(ds) < DT diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py new file mode 100755 index 00000000000000..198ecfe93574b6 --- /dev/null +++ b/selfdrive/locationd/test/test_laikad.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +import time +import unittest +from collections import defaultdict +from datetime import datetime +from unittest import mock +from unittest.mock import Mock, patch + +from common.params import Params +from laika.constants import SECS_IN_DAY +from laika.downloader import DownloadFailed +from laika.ephemeris import EphemerisType, GPSEphemeris +from laika.gps_time import GPSTime +from laika.helpers import ConstellationId, TimeRangeHolder +from laika.raw_gnss import GNSSMeasurement, read_raw_ublox +from selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg +from selfdrive.test.openpilotci import get_url +from tools.lib.logreader import LogReader + + +def get_log(segs=range(0)): + logs = [] + for i in segs: + logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i))) + return [m for m in logs if m.which() == 'ubloxGnss'] + + +def verify_messages(lr, laikad, return_one_success=False): + good_msgs = [] + for m in lr: + msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) + if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: + good_msgs.append(msg) + if return_one_success: + return msg + return good_msgs + + +def get_first_gps_time(logs): + for m in logs: + if m.ubloxGnss.which == 'measurementReport': + new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) + if len(new_meas) > 0: + return new_meas[0].recv_time + + +def get_measurement_mock(gpstime, sat_ephemeris): + meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) + # Fake measurement being processed + meas.observables_final = meas.observables + meas.sat_ephemeris = sat_ephemeris + return meas + + +GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC = GPSTime.from_datetime(datetime(2022, month=1, day=29, hour=12)) + + +class TestLaikad(unittest.TestCase): + + @classmethod + def setUpClass(cls): + logs = get_log(range(1)) + cls.logs = logs + first_gps_time = get_first_gps_time(logs) + cls.first_gps_time = first_gps_time + + def setUp(self): + Params().remove(EPHEMERIS_CACHE) + + def test_fetch_orbits_non_blocking(self): + gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) + laikad = Laikad() + laikad.fetch_orbits(gpstime, block=False) + laikad.orbit_fetch_future.result(30) + # Get results and save orbits to laikad: + laikad.fetch_orbits(gpstime, block=False) + + ephem = laikad.astro_dog.orbits['G01'][0] + self.assertIsNotNone(ephem) + + laikad.fetch_orbits(gpstime+2*SECS_IN_DAY, block=False) + laikad.orbit_fetch_future.result(30) + # Get results and save orbits to laikad: + laikad.fetch_orbits(gpstime + 2 * SECS_IN_DAY, block=False) + + ephem2 = laikad.astro_dog.orbits['G01'][0] + self.assertIsNotNone(ephem) + self.assertNotEqual(ephem, ephem2) + + def test_fetch_orbits_with_wrong_clocks(self): + laikad = Laikad() + + def check_has_orbits(): + self.assertGreater(len(laikad.astro_dog.orbits), 0) + ephem = laikad.astro_dog.orbits['G01'][0] + self.assertIsNotNone(ephem) + real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1)) + wrong_future_clock_time = real_current_time + SECS_IN_DAY + + laikad.fetch_orbits(wrong_future_clock_time, block=True) + check_has_orbits() + self.assertEqual(laikad.last_fetch_orbits_t, wrong_future_clock_time) + + # Test fetching orbits with earlier time + assert real_current_time < laikad.last_fetch_orbits_t + + laikad.astro_dog.orbits = {} + laikad.fetch_orbits(real_current_time, block=True) + check_has_orbits() + self.assertEqual(laikad.last_fetch_orbits_t, real_current_time) + + def test_ephemeris_source_in_msg(self): + data_mock = defaultdict(str) + data_mock['sv_id'] = 1 + + gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC + laikad = Laikad() + laikad.fetch_orbits(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + # Verify gps satellite returns same source + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + + # Test nasa source by using older date + gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) + laikad = Laikad() + laikad.fetch_orbits(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0]) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid) + + # Test nav source type + ephem = GPSEphemeris(data_mock, gpstime) + meas = get_measurement_mock(gpstime, ephem) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) + + def test_laika_online(self): + laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) + correct_msgs = verify_messages(self.logs, laikad) + + correct_msgs_expected = 555 + self.assertEqual(correct_msgs_expected, len(correct_msgs)) + self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + + def test_kf_becomes_valid(self): + laikad = Laikad(auto_update=False) + m = self.logs[0] + self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9))) + kf_valid = False + for m in self.logs: + laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) + kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9)) + if kf_valid: + break + self.assertTrue(kf_valid) + + def test_laika_online_nav_only(self): + laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) + # Disable fetch_orbits to test NAV only + laikad.fetch_orbits = Mock() + correct_msgs = verify_messages(self.logs, laikad) + correct_msgs_expected = 559 + self.assertEqual(correct_msgs_expected, len(correct_msgs)) + self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + + @mock.patch('laika.downloader.download_and_cache_file') + def test_laika_offline(self, downloader_mock): + downloader_mock.side_effect = DownloadFailed("Mock download failed") + laikad = Laikad(auto_update=False) + laikad.fetch_orbits(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True) + + @mock.patch('laika.downloader.download_and_cache_file') + def test_download_failed_russian_source(self, downloader_mock): + downloader_mock.side_effect = DownloadFailed + laikad = Laikad(auto_update=False) + correct_msgs = verify_messages(self.logs, laikad) + self.assertEqual(16, len(correct_msgs)) + self.assertEqual(16, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + + def test_laika_get_orbits(self): + laikad = Laikad(auto_update=False) + # Pretend process has loaded the orbits on startup by using the time of the first gps message. + laikad.fetch_orbits(self.first_gps_time, block=True) + self.dict_has_values(laikad.astro_dog.orbits) + + @unittest.skip("Use to debug live data") + def test_laika_get_orbits_now(self): + laikad = Laikad(auto_update=False) + laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) + prn = "G01" + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) + prn = "R01" + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) + print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + + def test_get_orbits_in_process(self): + laikad = Laikad(auto_update=False) + has_orbits = False + for m in self.logs: + laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False) + if laikad.orbit_fetch_future is not None: + laikad.orbit_fetch_future.result() + vals = laikad.astro_dog.orbits.values() + has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0 + if has_orbits: + break + self.assertTrue(has_orbits) + self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) + self.assertEqual(None, laikad.orbit_fetch_future) + + def test_cache(self): + laikad = Laikad(auto_update=True, save_ephemeris=True) + + def wait_for_cache(): + max_time = 2 + while Params().get(EPHEMERIS_CACHE) is None: + time.sleep(0.1) + max_time -= 0.1 + if max_time < 0: + self.fail("Cache has not been written after 2 seconds") + + # Test cache with no ephemeris + laikad.cache_ephemeris(t=GPSTime(0, 0)) + wait_for_cache() + Params().remove(EPHEMERIS_CACHE) + + laikad.astro_dog.get_navs(self.first_gps_time) + laikad.fetch_orbits(self.first_gps_time, block=True) + + # Wait for cache to save + wait_for_cache() + + # Check both nav and orbits separate + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True) + # Verify orbits and nav are loaded from cache + self.dict_has_values(laikad.astro_dog.orbits) + self.dict_has_values(laikad.astro_dog.nav) + # Verify cache is working for only nav by running a segment + msg = verify_messages(self.logs, laikad, return_one_success=True) + self.assertIsNotNone(msg) + + with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: + # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently + laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() + laikad.fetch_orbits(self.first_gps_time, block=False) + mock_method.assert_not_called() + + # Verify cache is working for only orbits by running a segment + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT, save_ephemeris=True) + msg = verify_messages(self.logs, laikad, return_one_success=True) + self.assertIsNotNone(msg) + # Verify orbit data is not downloaded + mock_method.assert_not_called() + + def dict_has_values(self, dct): + self.assertGreater(len(dct), 0) + self.assertGreater(min([len(v) for v in dct.values()]), 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py new file mode 100755 index 00000000000000..e30331a4601ff8 --- /dev/null +++ b/selfdrive/locationd/test/test_locationd.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 +import json +import random +import unittest +import time +import capnp + +import cereal.messaging as messaging +from cereal.services import service_list +from common.params import Params + +from selfdrive.manager.process_config import managed_processes + + +class TestLocationdProc(unittest.TestCase): + MAX_WAITS = 1000 + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'] + + def setUp(self): + random.seed(123489234) + + self.pm = messaging.PubMaster(self.LLD_MSGS) + + managed_processes['locationd'].prepare() + managed_processes['locationd'].start() + + time.sleep(1) + + def tearDown(self): + managed_processes['locationd'].stop() + + def send_msg(self, msg): + self.pm.send(msg.which(), msg) + waits_left = self.MAX_WAITS + while waits_left and not self.pm.all_readers_updated(msg.which()): + time.sleep(0) + waits_left -= 1 + time.sleep(0.0001) + + def get_fake_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.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 = self.lat + msg.gpsLocationExternal.longitude = self.lon + msg.gpsLocationExternal.altitude = self.alt + 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 + return msg + + def test_params_gps(self): + # first reset params + Params().remove('LastGPSPosition') + + self.lat = 30 + (random.random() * 10.0) + self.lon = -70 + (random.random() * 10.0) + self.alt = 5 + (random.random() * 10.0) + self.fake_duration = 90 # secs + # get fake messages at the correct frequency, listed in services.py + fake_msgs = [] + for sec in range(self.fake_duration): + for name in self.LLD_MSGS: + for j in range(int(service_list[name].frequency)): + fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9))) + + for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime): + self.send_msg(fake_msg) + time.sleep(1) # wait for async params write + + lastGPS = json.loads(Params().get('LastGPSPosition')) + + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py deleted file mode 100644 index 0ea7ac183f2dc4..00000000000000 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ /dev/null @@ -1,190 +0,0 @@ -import numpy as np -from collections import defaultdict -from enum import Enum - -from openpilot.tools.lib.logreader import LogReader -from openpilot.selfdrive.test.process_replay.migration import migrate_all -from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name - -# TODO find a new segment to test -TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5" -GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation'] -SELECT_COMPARE_FIELDS = { - 'yaw_rate': ['angularVelocityDevice', 'z'], - 'roll': ['orientationNED', 'x'], - 'inputs_flag': ['inputsOK'], - 'sensors_flag': ['sensorsOK'], -} -JUNK_IDX = 100 -CONSISTENT_SPIKES_COUNT = 10 - - -class Scenario(Enum): - BASE = 'base' - GYRO_OFF = 'gyro_off' - GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' - GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes' - ACCEL_OFF = 'accel_off' - ACCEL_SPIKE_MIDWAY = 'accel_spike_midway' - ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes' - SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes' - SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes' - - -def get_select_fields_data(logs): - def get_nested_keys(msg, keys): - val = None - for key in keys: - val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key] - return val - lp = [x.livePose for x in logs if x.which() == 'livePose'] - data = defaultdict(list) - for msg in lp: - for key, fields in SELECT_COMPARE_FIELDS.items(): - data[key].append(get_nested_keys(msg, fields)) - for key in data: - data[key] = np.array(data[key][JUNK_IDX:], dtype=float) - return data - - -def modify_logs_midway(logs, which, count, fn): - non_which = [x for x in logs if x.which() != which] - which = [x for x in logs if x.which() == which] - temps = which[len(which) // 2:len(which) // 2 + count] - for i, temp in enumerate(temps): - temp = temp.as_builder() - fn(temp) - which[len(which) // 2 + i] = temp.as_reader() - return sorted(non_which + which, key=lambda x: x.logMonoTime) - - -def run_scenarios(scenario, logs): - if scenario == Scenario.BASE: - pass - - elif scenario == Scenario.GYRO_OFF: - logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) - - elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES: - def gyro_spike(msg): - msg.gyroscope.gyroUncalibrated.v[0] += 3.0 - count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT - logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike) - - elif scenario == Scenario.ACCEL_OFF: - logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime) - - elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES: - def acc_spike(msg): - msg.accelerometer.acceleration.v[0] += 100.0 - count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT - logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike) - - elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES: - def timing_spike(msg): - msg.accelerometer.timestamp -= int(0.150 * 1e9) - count = 1 if scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT - logs = modify_logs_midway(logs, 'accelerometer', count, timing_spike) - - replayed_logs = replay_process_with_name(name='locationd', lr=logs) - return get_select_fields_data(logs), get_select_fields_data(replayed_logs) - - -class TestLocationdScenarios: - """ - Test locationd with different scenarios. In all these scenarios, we expect the following: - - locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK) - - faulty values should be ignored, with appropriate flags set - """ - - @classmethod - def setup_class(cls): - cls.logs = migrate_all(LogReader(TEST_ROUTE)) - - def test_base(self): - """ - Test: unchanged log - Expected Result: - - yaw_rate: unchanged - - roll: unchanged - """ - orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - - def test_gyro_off(self): - """ - Test: no gyroscope message for the entire segment - Expected Result: - - yaw_rate: 0 - - roll: 0 - - sensorsOK: False - """ - _, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs) - assert np.allclose(replayed_data['yaw_rate'], 0.0) - assert np.allclose(replayed_data['roll'], 0.0) - assert np.all(replayed_data['sensors_flag'] == 0.0) - - def test_gyro_spike(self): - """ - Test: a gyroscope spike in the middle of the segment - Expected Result: - - yaw_rate: unchanged - - roll: unchanged - - inputsOK: False for some time after the spike, True for the rest - """ - orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag']) - assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag']) - - def test_consistent_gyro_spikes(self): - """ - Test: consistent timing spikes for N gyroscope messages in the middle of the segment - Expected Result: inputsOK becomes False after N of bad measurements - """ - orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs) - assert np.diff(replayed_data['inputs_flag'])[501] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[708] == 1.0 - - def test_accel_off(self): - """ - Test: no accelerometer message for the entire segment - Expected Result: - - yaw_rate: 0 - - roll: 0 - - sensorsOK: False - """ - _, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs) - assert np.allclose(replayed_data['yaw_rate'], 0.0) - assert np.allclose(replayed_data['roll'], 0.0) - assert np.all(replayed_data['sensors_flag'] == 0.0) - - def test_accel_spike(self): - """ - ToDo: - Test: an accelerometer spike in the middle of the segment - Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. - """ - orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - - def test_single_timing_spike(self): - """ - Test: timing of 150ms off for the single accelerometer message in the middle of the segment - Expected Result: the message is ignored, and inputsOK is False for that time - """ - orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_SPIKE_MIDWAY, self.logs) - assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag']) - assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag']) - - def test_consistent_timing_spikes(self): - """ - Test: consistent timing spikes for N accelerometer messages in the middle of the segment - Expected Result: inputsOK becomes False after N of bad measurements - """ - orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs) - assert np.diff(replayed_data['inputs_flag'])[501] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[707] == 1.0 diff --git a/selfdrive/locationd/test/test_paramsd.py b/selfdrive/locationd/test/test_paramsd.py deleted file mode 100644 index dd496b767539f0..00000000000000 --- a/selfdrive/locationd/test/test_paramsd.py +++ /dev/null @@ -1,67 +0,0 @@ -import random -import numpy as np - -from cereal import messaging -from openpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed -from openpilot.selfdrive.locationd.models.car_kf import CarKalman -from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE -from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams -from openpilot.common.params import Params -from openpilot.tools.lib.logreader import LogReader - - -def get_random_live_parameters(CP): - msg = messaging.new_message("liveParameters") - msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio - msg.liveParameters.stiffnessFactor = random.random() - msg.liveParameters.angleOffsetAverageDeg = random.random() - msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])] - return msg - - -class TestParamsd: - def test_read_saved_params(self): - params = Params() - - lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) - CP = next(m for m in lr if m.which() == "carParams").carParams - - msg = get_random_live_parameters(CP) - params.put("LiveParametersV2", msg.to_bytes()) - params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) - - migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error - sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True) - np.testing.assert_allclose(sr, msg.liveParameters.steerRatio) - np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor) - np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg) - np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape) - np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std) - - # TODO Remove this test after the support for old format is removed - def test_read_saved_params_old_format(self): - params = Params() - - lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) - CP = next(m for m in lr if m.which() == "carParams").carParams - - msg = get_random_live_parameters(CP) - params.put("LiveParameters", msg.liveParameters.to_dict()) - params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) - params.remove("LiveParametersV2") - - migrate_cached_vehicle_params_if_needed(params) - sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True) - np.testing.assert_allclose(sr, msg.liveParameters.steerRatio) - np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor) - np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg) - assert params.get("LiveParametersV2") is not None - - def test_read_saved_params_corrupted_old_format(self): - params = Params() - params.put("LiveParameters", {}) - params.remove("LiveParametersV2") - - migrate_cached_vehicle_params_if_needed(params) - assert params.get("LiveParameters") is None - assert params.get("LiveParametersV2") is None diff --git a/selfdrive/locationd/test/test_torqued.py b/selfdrive/locationd/test/test_torqued.py deleted file mode 100644 index 53f3120c362dfc..00000000000000 --- a/selfdrive/locationd/test/test_torqued.py +++ /dev/null @@ -1,25 +0,0 @@ -from cereal import car -from openpilot.selfdrive.locationd.torqued import TorqueEstimator - - -def test_cal_percent(): - est = TorqueEstimator(car.CarParams()) - msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == 0 - - for (low, high), min_pts in zip(est.filtered_points.buckets.keys(), - est.filtered_points.buckets_min_points.values(), strict=True): - for _ in range(int(min_pts)): - est.filtered_points.add_point((low + high) / 2.0, 0.0) - - # enough bucket points, but not enough total points - msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2 - - # add enough points to bucket with most capacity - key = list(est.filtered_points.buckets)[0] - for _ in range(est.min_points_total - len(est.filtered_points)): - est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0) - - msg = est.get_msg() - assert msg.liveTorqueParameters.calPerc == 100 diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py new file mode 100755 index 00000000000000..427003b24c4f94 --- /dev/null +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -0,0 +1,80 @@ +import unittest + +import numpy as np + +from laika import AstroDog +from laika.helpers import ConstellationId +from laika.raw_gnss import calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox +from selfdrive.test.openpilotci import get_url +from tools.lib.logreader import LogReader + + +def get_gnss_measurements(log_reader): + gnss_measurements = [] + for msg in log_reader: + if msg.which() == "ubloxGnss": + ublox_msg = msg.ubloxGnss + if ublox_msg.which == 'measurementReport': + report = ublox_msg.measurementReport + if len(report.measurements) > 0: + gnss_measurements.append(read_raw_ublox(report)) + return gnss_measurements + + +class TestUbloxProcessing(unittest.TestCase): + NUM_TEST_PROCESS_MEAS = 10 + + @classmethod + def setUpClass(cls): + lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) + cls.gnss_measurements = get_gnss_measurements(lr) + + def test_read_ublox_raw(self): + count_gps = 0 + count_glonass = 0 + for measurements in self.gnss_measurements: + for m in measurements: + if m.constellation_id == ConstellationId.GPS: + count_gps += 1 + elif m.constellation_id == ConstellationId.GLONASS: + count_glonass += 1 + + self.assertEqual(count_gps, 5036) + self.assertEqual(count_glonass, 3651) + + def test_get_fix(self): + dog = AstroDog() + position_fix_found = 0 + count_processed_measurements = 0 + count_corrected_measurements = 0 + position_fix_found_after_correcting = 0 + + pos_ests = [] + for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]: + processed_meas = process_measurements(measurements, dog) + count_processed_measurements += len(processed_meas) + pos_fix = calc_pos_fix(processed_meas) + if len(pos_fix) > 0 and all(pos_fix[0] != 0): + position_fix_found += 1 + + corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog) + count_corrected_measurements += len(corrected_meas) + + pos_fix = calc_pos_fix(corrected_meas) + if len(pos_fix) > 0 and all(pos_fix[0] != 0): + pos_ests.append(pos_fix[0]) + position_fix_found_after_correcting += 1 + + mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0) + np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1) + + # Note that can happen that there are less corrected measurements compared to processed when they are invalid. + # However, not for the current segment + self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS) + self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS) + self.assertEqual(count_processed_measurements, 69) + self.assertEqual(count_corrected_measurements, 69) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py new file mode 100644 index 00000000000000..9cffbeac40cff3 --- /dev/null +++ b/selfdrive/locationd/test/ublox.py @@ -0,0 +1,937 @@ +#!/usr/bin/env python3 +# pylint: skip-file +''' +UBlox binary protocol handling + +Copyright Andrew Tridgell, October 2012 +Released under GNU GPL version 3 or later + +WARNING: This code has originally intended for +ublox version 7, it has been adapted to work +for ublox version 8, not all functions may work. +''' + + +import struct +import time + +# protocol constants +PREAMBLE1 = 0xb5 +PREAMBLE2 = 0x62 + +# message classes +CLASS_NAV = 0x01 +CLASS_RXM = 0x02 +CLASS_INF = 0x04 +CLASS_ACK = 0x05 +CLASS_CFG = 0x06 +CLASS_MON = 0x0A +CLASS_AID = 0x0B +CLASS_TIM = 0x0D +CLASS_ESF = 0x10 + +# ACK messages +MSG_ACK_NACK = 0x00 +MSG_ACK_ACK = 0x01 + +# NAV messages +MSG_NAV_POSECEF = 0x1 +MSG_NAV_POSLLH = 0x2 +MSG_NAV_STATUS = 0x3 +MSG_NAV_DOP = 0x4 +MSG_NAV_SOL = 0x6 +MSG_NAV_PVT = 0x7 +MSG_NAV_POSUTM = 0x8 +MSG_NAV_VELNED = 0x12 +MSG_NAV_VELECEF = 0x11 +MSG_NAV_TIMEGPS = 0x20 +MSG_NAV_TIMEUTC = 0x21 +MSG_NAV_CLOCK = 0x22 +MSG_NAV_SVINFO = 0x30 +MSG_NAV_AOPSTATUS = 0x60 +MSG_NAV_DGPS = 0x31 +MSG_NAV_DOP = 0x04 +MSG_NAV_EKFSTATUS = 0x40 +MSG_NAV_SBAS = 0x32 +MSG_NAV_SOL = 0x06 + +# RXM messages +MSG_RXM_RAW = 0x15 +MSG_RXM_SFRB = 0x11 +MSG_RXM_SFRBX = 0x13 +MSG_RXM_SVSI = 0x20 +MSG_RXM_EPH = 0x31 +MSG_RXM_ALM = 0x30 +MSG_RXM_PMREQ = 0x41 + +# AID messages +MSG_AID_ALM = 0x30 +MSG_AID_EPH = 0x31 +MSG_AID_ALPSRV = 0x32 +MSG_AID_AOP = 0x33 +MSG_AID_DATA = 0x10 +MSG_AID_ALP = 0x50 +MSG_AID_DATA = 0x10 +MSG_AID_HUI = 0x02 +MSG_AID_INI = 0x01 +MSG_AID_REQ = 0x00 + +# CFG messages +MSG_CFG_PRT = 0x00 +MSG_CFG_ANT = 0x13 +MSG_CFG_DAT = 0x06 +MSG_CFG_EKF = 0x12 +MSG_CFG_ESFGWT = 0x29 +MSG_CFG_CFG = 0x09 +MSG_CFG_USB = 0x1b +MSG_CFG_RATE = 0x08 +MSG_CFG_SET_RATE = 0x01 +MSG_CFG_NAV5 = 0x24 +MSG_CFG_FXN = 0x0E +MSG_CFG_INF = 0x02 +MSG_CFG_ITFM = 0x39 +MSG_CFG_MSG = 0x01 +MSG_CFG_NAVX5 = 0x23 +MSG_CFG_NMEA = 0x17 +MSG_CFG_NVS = 0x22 +MSG_CFG_PM2 = 0x3B +MSG_CFG_PM = 0x32 +MSG_CFG_ITMF = 0x39 +MSG_CFG_RINV = 0x34 +MSG_CFG_RST = 0x04 +MSG_CFG_RXM = 0x11 +MSG_CFG_SBAS = 0x16 +MSG_CFG_TMODE2 = 0x3D +MSG_CFG_TMODE = 0x1D +MSG_CFG_TPS = 0x31 +MSG_CFG_TP = 0x07 +MSG_CFG_GNSS = 0x3E +MSG_CFG_ODO = 0x1E + +# ESF messages +MSG_ESF_MEAS = 0x02 +MSG_ESF_STATUS = 0x10 + +# INF messages +MSG_INF_DEBUG = 0x04 +MSG_INF_ERROR = 0x00 +MSG_INF_NOTICE = 0x02 +MSG_INF_TEST = 0x03 +MSG_INF_WARNING = 0x01 + +# MON messages +MSG_MON_SCHD = 0x01 +MSG_MON_HW = 0x09 +MSG_MON_HW2 = 0x0B +MSG_MON_IO = 0x02 +MSG_MON_MSGPP = 0x06 +MSG_MON_RXBUF = 0x07 +MSG_MON_RXR = 0x21 +MSG_MON_TXBUF = 0x08 +MSG_MON_VER = 0x04 + +# TIM messages +MSG_TIM_TP = 0x01 +MSG_TIM_TM2 = 0x03 +MSG_TIM_SVIN = 0x04 +MSG_TIM_VRFY = 0x06 + +# port IDs +PORT_DDC = 0 +PORT_SERIAL1 = 1 +PORT_SERIAL2 = 2 +PORT_USB = 3 +PORT_SPI = 4 + +# dynamic models +DYNAMIC_MODEL_PORTABLE = 0 +DYNAMIC_MODEL_STATIONARY = 2 +DYNAMIC_MODEL_PEDESTRIAN = 3 +DYNAMIC_MODEL_AUTOMOTIVE = 4 +DYNAMIC_MODEL_SEA = 5 +DYNAMIC_MODEL_AIRBORNE1G = 6 +DYNAMIC_MODEL_AIRBORNE2G = 7 +DYNAMIC_MODEL_AIRBORNE4G = 8 + +#reset items +RESET_HOT = 0 +RESET_WARM = 1 +RESET_COLD = 0xFFFF + +RESET_HW = 0 +RESET_SW = 1 +RESET_SW_GPS = 2 +RESET_HW_GRACEFUL = 4 +RESET_GPS_STOP = 8 +RESET_GPS_START = 9 + + +class UBloxError(Exception): + '''Ublox error class''' + + def __init__(self, msg): + Exception.__init__(self, msg) + self.message = msg + + +class UBloxAttrDict(dict): + '''allow dictionary members as attributes''' + + def __init__(self): + dict.__init__(self) + + def __getattr__(self, name): + try: + return self.__getitem__(name) + except KeyError: + raise AttributeError(name) + + def __setattr__(self, name, value): + if name in self.__dict__: + # allow set on normal attributes + dict.__setattr__(self, name, value) + else: + self.__setitem__(name, value) + + +def ArrayParse(field): + '''parse an array descriptor''' + arridx = field.find('[') + if arridx == -1: + return (field, -1) + alen = int(field[arridx + 1:-1]) + fieldname = field[:arridx] + return (fieldname, alen) + + +class UBloxDescriptor: + '''class used to describe the layout of a UBlox message''' + + def __init__(self, + name, + msg_format, + fields=None, + count_field=None, + format2=None, + fields2=None): + if fields is None: + fields = [] + + self.name = name + self.msg_format = msg_format + self.fields = fields + self.count_field = count_field + self.format2 = format2 + self.fields2 = fields2 + + def unpack(self, msg): + '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg''' + msg._fields = {} + + # unpack main message blocks. A comm + formats = self.msg_format.split(',') + buf = msg._buf[6:-2] + count = 0 + msg._recs = [] + fields = self.fields[:] + + for fmt in formats: + size1 = struct.calcsize(fmt) + if size1 > len(buf): + raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf))) + f1 = list(struct.unpack(fmt, buf[:size1])) + i = 0 + while i < len(f1): + field = fields.pop(0) + (fieldname, alen) = ArrayParse(field) + if alen == -1: + msg._fields[fieldname] = f1[i] + if self.count_field == fieldname: + count = int(f1[i]) + i += 1 + else: + msg._fields[fieldname] = [0] * alen + for a in range(alen): + msg._fields[fieldname][a] = f1[i] + i += 1 + buf = buf[size1:] + if len(buf) == 0: + break + + if self.count_field == '_remaining': + count = len(buf) // struct.calcsize(self.format2) + + if count == 0: + msg._unpacked = True + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + return + + size2 = struct.calcsize(self.format2) + for c in range(count): + r = UBloxAttrDict() + if size2 > len(buf): + raise UBloxError("INVALID_SIZE=%u, " % len(buf)) + f2 = list(struct.unpack(self.format2, buf[:size2])) + for i in range(len(self.fields2)): + r[self.fields2[i]] = f2[i] + buf = buf[size2:] + msg._recs.append(r) + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + msg._unpacked = True + + def pack(self, msg, msg_class=None, msg_id=None): + '''pack a UBloxMessage from the .fields and ._recs attributes in msg''' + f1 = [] + if msg_class is None: + msg_class = msg.msg_class() + if msg_id is None: + msg_id = msg.msg_id() + msg._buf = '' + + fields = self.fields[:] + for f in fields: + (fieldname, alen) = ArrayParse(f) + if fieldname not in msg._fields: + break + if alen == -1: + f1.append(msg._fields[fieldname]) + else: + for a in range(alen): + f1.append(msg._fields[fieldname][a]) + try: + # try full length message + fmt = self.msg_format.replace(',', '') + msg._buf = struct.pack(fmt, *tuple(f1)) + except Exception: + # try without optional part + fmt = self.msg_format.split(',')[0] + msg._buf = struct.pack(fmt, *tuple(f1)) + + length = len(msg._buf) + if msg._recs: + length += len(msg._recs) * struct.calcsize(self.format2) + header = struct.pack('= level: + print(msg) + + def unpack(self): + '''unpack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if type not in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + msg_types[type].unpack(self) + return self._fields, self._recs + + def pack(self): + '''pack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if type not in msg_types: + raise UBloxError('Unknown message %s' % str(type)) + msg_types[type].pack(self) + + def name(self): + '''return the short string name for a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if type not in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + return msg_types[type].name + + def msg_class(self): + '''return the message class''' + return self._buf[2] + + def msg_id(self): + '''return the message id within the class''' + return self._buf[3] + + def msg_type(self): + '''return the message type tuple (class, id)''' + return (self.msg_class(), self.msg_id()) + + def msg_length(self): + '''return the payload length''' + (payload_length, ) = struct.unpack(' 0 and self._buf[0] != PREAMBLE1: + return False + if len(self._buf) > 1 and self._buf[1] != PREAMBLE2: + self.debug(1, "bad pre2") + return False + if self.needed_bytes() == 0 and not self.valid(): + if len(self._buf) > 8: + self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf), + self.needed_bytes())) + else: + self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes())) + return False + return True + + def add(self, bytes): + '''add some bytes to a message''' + self._buf += bytes + while not self.valid_so_far() and len(self._buf) > 0: + '''handle corrupted streams''' + self._buf = self._buf[1:] + if self.needed_bytes() < 0: + self._buf = "" + + def checksum(self, data=None): + '''return a checksum tuple for a message''' + if data is None: + data = self._buf[2:-2] + #cs = 0 + ck_a = 0 + ck_b = 0 + for i in data: + ck_a = (ck_a + i) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return (ck_a, ck_b) + + def valid_checksum(self): + '''check if the checksum is OK''' + (ck_a, ck_b) = self.checksum() + #d = self._buf[2:-2] + (ck_a2, ck_b2) = struct.unpack('= 8 and self.needed_bytes() == 0 and self.valid_checksum() + + +class UBlox: + def __init__(self, dev, baudrate): + self.dev = dev + self.baudrate = baudrate + + self.use_sendrecv = False + self.read_only = False + self.debug_level = 0 + + self.logfile = None + self.log = None + self.preferred_dynamic_model = None + self.preferred_usePPP = None + self.preferred_dgps_timeout = None + + def close(self): + '''close the device''' + self.dev.close() + self.dev = None + + def set_debug(self, debug_level): + '''set debug level''' + self.debug_level = debug_level + + def debug(self, level, msg): + '''write a debug message''' + if self.debug_level >= level: + print(msg) + + def set_logfile(self, logfile, append=False): + '''setup logging to a file''' + if self.log is not None: + self.log.close() + self.log = None + self.logfile = logfile + if self.logfile is not None: + if append: + mode = 'ab' + else: + mode = 'wb' + self.log = open(self.logfile, mode=mode) + + def set_preferred_dynamic_model(self, model): + '''set the preferred dynamic model for receiver''' + self.preferred_dynamic_model = model + if model is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_dgps_timeout(self, timeout): + '''set the preferred DGPS timeout for receiver''' + self.preferred_dgps_timeout = timeout + if timeout is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_usePPP(self, usePPP): + '''set the preferred usePPP setting for the receiver''' + if usePPP is None: + self.preferred_usePPP = None + return + self.preferred_usePPP = int(usePPP) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def nmea_checksum(self, msg): + d = msg[1:] + cs = 0 + for i in d: + cs ^= ord(i) + return cs + + def write(self, buf): + '''write some bytes''' + if not self.read_only: + if self.use_sendrecv: + return self.dev.send(buf) + if type(buf) == str: + return self.dev.write(str.encode(buf)) + else: + return self.dev.write(buf) + + def read(self, n): + '''read some bytes''' + if self.use_sendrecv: + try: + return self.dev.recv(n) + except OSError: + return '' + return self.dev.read(n) + + def send_nmea(self, msg): + if not self.read_only: + s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n" + self.write(s) + + def set_binary(self): + '''put a UBlox into binary mode using a NMEA string''' + if not self.read_only: + print("try set binary at %u" % self.baudrate) + self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate) + + def disable_nmea(self): + ''' stop sending all types of nmea messages ''' + self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0") + self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0") + + def seek_percent(self, pct): + '''seek to the given percentage of a file''' + self.dev.seek(0, 2) + filesize = self.dev.tell() + self.dev.seek(pct * 0.01 * filesize) + + def special_handling(self, msg): + '''handle automatic configuration changes''' + if msg.name() == 'CFG_NAV5': + msg.unpack() + sendit = False + pollit = False + if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model: + msg.dynModel = self.preferred_dynamic_model + sendit = True + pollit = True + if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout: + msg.dgpsTimeOut = self.preferred_dgps_timeout + self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut) + sendit = True + # we don't re-poll for this one, as some receivers refuse to set it + if sendit: + msg.pack() + self.send(msg) + if pollit: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None: + msg.unpack() + if msg.usePPP != self.preferred_usePPP: + msg.usePPP = self.preferred_usePPP + msg.mask = 1 << 13 + msg.pack() + self.send(msg) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def receive_message(self, ignore_eof=False): + '''blocking receive of one ublox message''' + msg = UBloxMessage() + while True: + n = msg.needed_bytes() + b = self.read(n) + if not b: + if ignore_eof: + time.sleep(0.01) + continue + if len(msg._buf) > 0: + self.debug(1, "dropping %d bytes" % len(msg._buf)) + return None + msg.add(b) + if self.log is not None: + self.log.write(b) + self.log.flush() + if msg.valid(): + self.special_handling(msg) + return msg + + def receive_message_noerror(self, ignore_eof=False): + '''blocking receive of one ublox message, ignoring errors''' + try: + return self.receive_message(ignore_eof=ignore_eof) + except UBloxError as e: + print(e) + return None + except OSError as e: + # Occasionally we get hit with 'resource temporarily unavailable' + # messages here on the serial device, catch them too. + print(e) + return None + + def send(self, msg): + '''send a preformatted ublox message''' + if not msg.valid(): + self.debug(1, "invalid send") + return + if not self.read_only: + self.write(msg._buf) + + def send_message(self, msg_class, msg_id, payload): + '''send a ublox message with class, id and payload''' + msg = UBloxMessage() + msg._buf = struct.pack('= bound_min) and (x < bound_max): - self.buckets[(bound_min, bound_max)].append([x, 1.0, y]) - break - - -class TorqueEstimator(ParameterEstimator): - def __init__(self, CP, decimated=False, track_all_points=False): - self.hist_len = int(HISTORY / DT_MDL) - self.lag = 0.0 - self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters - if decimated: - self.min_bucket_points = MIN_BUCKET_POINTS / 10 - self.min_points_total = MIN_POINTS_TOTAL_QLOG - self.fit_points = FIT_POINTS_TOTAL_QLOG - self.factor_sanity = FACTOR_SANITY_QLOG - self.friction_sanity = FRICTION_SANITY_QLOG - - else: - self.min_bucket_points = MIN_BUCKET_POINTS - self.min_points_total = MIN_POINTS_TOTAL - self.fit_points = FIT_POINTS_TOTAL - self.factor_sanity = FACTOR_SANITY - self.friction_sanity = FRICTION_SANITY - - self.offline_friction = 0.0 - self.offline_latAccelFactor = 0.0 - self.resets = 0.0 - self.use_params = CP.brand in ALLOWED_CARS and CP.lateralTuning.which() == 'torque' - - if CP.lateralTuning.which() == 'torque': - self.offline_friction = CP.lateralTuning.torque.friction - self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor - - self.calibrator = PoseCalibrator() - - self.reset() - - initial_params = { - 'latAccelFactor': self.offline_latAccelFactor, - 'latAccelOffset': 0.0, - 'frictionCoefficient': self.offline_friction, - 'points': [] - } - self.decay = MIN_FILTER_DECAY - self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor - self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor - self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction - self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction - - # try to restore cached params - params = Params() - params_cache = params.get("CarParamsPrevRoute") - torque_cache = params.get("LiveTorqueParameters") - if params_cache is not None and torque_cache is not None: - try: - with log.Event.from_bytes(torque_cache) as log_evt: - cache_ltp = log_evt.liveTorqueParameters - with car.CarParams.from_bytes(params_cache) as msg: - cache_CP = msg - if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION): - if cache_ltp.liveValid: - initial_params = { - 'latAccelFactor': cache_ltp.latAccelFactorFiltered, - 'latAccelOffset': cache_ltp.latAccelOffsetFiltered, - 'frictionCoefficient': cache_ltp.frictionCoefficientFiltered - } - initial_params['points'] = cache_ltp.points - self.decay = cache_ltp.decay - self.filtered_points.load_points(initial_params['points']) - cloudlog.info("restored torque params from cache") - except Exception: - cloudlog.exception("failed to restore cached torque params") - params.remove("LiveTorqueParameters") - - self.filtered_params = {} - for param in initial_params: - self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL) - - @staticmethod - def get_restore_key(CP, version): - a, b = None, None - if CP.lateralTuning.which() == 'torque': - a = CP.lateralTuning.torque.friction - b = CP.lateralTuning.torque.latAccelFactor - return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version) - - def reset(self): - self.resets += 1.0 - self.decay = MIN_FILTER_DECAY - self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len)) - self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS, - min_points=self.min_bucket_points, - min_points_total=self.min_points_total, - points_per_bucket=POINTS_PER_BUCKET, - rowsize=3) - self.all_torque_points = [] - - def estimate_params(self): - points = self.filtered_points.get_points(self.fit_points) - # total least square solution as both x and y are noisy observations - # this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals - try: - _, _, v = np.linalg.svd(points, full_matrices=False) - slope, offset = -v.T[0:2, 2] / v.T[2, 2] - _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T - friction_coeff = np.std(spread) * FRICTION_FACTOR - except np.linalg.LinAlgError as e: - cloudlog.exception(f"Error computing live torque params: {e}") - slope = offset = friction_coeff = np.nan - return slope, offset, friction_coeff - - def update_params(self, params): - self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY) - for param, value in params.items(): - self.filtered_params[param].update(value) - self.filtered_params[param].update_alpha(self.decay) - - def handle_log(self, t, which, msg): - if which == "carControl": - self.raw_points["carControl_t"].append(t + self.lag) - self.raw_points["lat_active"].append(msg.latActive) - elif which == "carOutput": - self.raw_points["carOutput_t"].append(t + self.lag) - self.raw_points["steer_torque"].append(-msg.actuatorsOutput.torque) - elif which == "carState": - self.raw_points["carState_t"].append(t + self.lag) - # TODO: check if high aEgo affects resulting lateral accel - self.raw_points["vego"].append(msg.vEgo) - self.raw_points["steer_override"].append(msg.steeringPressed) - elif which == "liveCalibration": - self.calibrator.feed_live_calib(msg) - elif which == "liveDelay": - self.lag = msg.lateralDelay - # calculate lateral accel from past steering torque - elif which == "livePose": - if len(self.raw_points['steer_torque']) == self.hist_len: - device_pose = Pose.from_live_pose(msg) - calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - angular_velocity_calibrated = calibrated_pose.angular_velocity - - yaw_rate = angular_velocity_calibrated.yaw - roll = device_pose.orientation.roll - # check lat active up to now (without lag compensation) - lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), - self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) - steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), - self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) - vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) - steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item() - lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item() - if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD): - if abs(lateral_acc) <= LAT_ACC_THRESHOLD: - self.filtered_points.add_point(steer, lateral_acc) - - if self.track_all_points: - self.all_torque_points.append([steer, lateral_acc]) - - def get_msg(self, valid=True, with_points=False): - msg = messaging.new_message('liveTorqueParameters') - msg.valid = valid - liveTorqueParameters = msg.liveTorqueParameters - liveTorqueParameters.version = VERSION - liveTorqueParameters.useParams = self.use_params - - # Calculate raw estimates when possible, only update filters when enough points are gathered - if self.filtered_points.is_calculable(): - latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params() - liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor) - liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset) - liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff) - - if self.filtered_points.is_valid(): - if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]): - cloudlog.exception("Live torque parameters are invalid.") - liveTorqueParameters.liveValid = False - self.reset() - else: - liveTorqueParameters.liveValid = True - latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor) - frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction) - self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff}) - - if with_points: - liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() - - liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) - liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) - liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) - liveTorqueParameters.totalBucketPoints = len(self.filtered_points) - liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent() - liveTorqueParameters.decay = self.decay - liveTorqueParameters.maxResets = self.resets - return msg - - -def main(demo=False): - config_realtime_process([0, 1, 2, 3], 5) - - DEBUG = bool(int(os.getenv("DEBUG", "0"))) - - pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') - - params = Params() - estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) - - while True: - sm.update() - if sm.all_checks(): - for which in sm.updated.keys(): - if sm.updated[which]: - t = sm.logMonoTime[which] * 1e-9 - estimator.handle_log(t, which, sm[which]) - - # 4Hz driven by livePose - if sm.frame % 5 == 0: - pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG)) - - # Cache points every 60 seconds while onroad - if sm.frame % 240 == 0: - msg = estimator.get_msg(valid=sm.all_checks(), with_points=True) - params.put_nonblocking("LiveTorqueParameters", msg.to_bytes()) - - -if __name__ == "__main__": - import argparse - - parser = argparse.ArgumentParser(description='Process the --demo argument.') - parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') - args = parser.parse_args() - main(demo=args.demo) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc new file mode 100644 index 00000000000000..c9f732e9ab2bbc --- /dev/null +++ b/selfdrive/locationd/ublox_msg.cc @@ -0,0 +1,345 @@ +#include "ublox_msg.h" + +#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(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { + 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))}; + break; + case 0x0213: + return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; + break; + case 0x0215: + return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; + break; + case 0x0a09: + return {"ubloxGnss", gen_mon_hw(static_cast(body))}; + break; + case 0x0a0b: + return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; + break; + default: + LOGE("Unknown message type %x", ubx_message.msg_type()); + return {"ubloxGnss", kj::Array()}; + break; + } +} + + +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.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.setAccuracy(msg->h_acc() * 1e-03); + 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::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + auto body = *msg->body(); + + if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) { + // 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 + 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 == 1) gps_subframes[msg->sv_id()].clear(); + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + } + + if (gps_subframes[msg->sv_id()].size() == 5) { + MessageBuilder msg_builder; + auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); + eph.setSvId(msg->sv_id()); + + // 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()); + + 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)); + } + + // 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()); + + 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)); + } + + // 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); + } + + // Subframe 4 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); + gps_t subframe(&stream); + gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); + + // This is page 18, why is the page id 56? + if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { + auto iono = static_cast(subframe_4->body()); + double a0 = iono->a0() * pow(2, -30); + double a1 = iono->a1() * pow(2, -27); + double a2 = iono->a2() * pow(2, -24); + double a3 = iono->a3() * pow(2, -24); + eph.setIonoAlpha({a0, a1, a2, a3}); + + double b0 = iono->b0() * pow(2, 11); + double b1 = iono->b1() * pow(2, 14); + double b2 = iono->b2() * pow(2, 16); + double b3 = iono->b3() * pow(2, 16); + eph.setIonoBeta({b0, b1, b2, b3}); + } + } + + return capnp::messageToFlatArray(msg_builder); + } + } + 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->measurements(); + 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_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/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h new file mode 100644 index 00000000000000..542e72816ba7b4 --- /dev/null +++ b/selfdrive/locationd/ublox_msg.h @@ -0,0 +1,111 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/util.h" +#include "selfdrive/locationd/generated/gps.h" +#include "selfdrive/locationd/generated/ubx.h" + +using namespace std::string_literals; + +// 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(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); + + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + std::unordered_map> gps_subframes; + + size_t bytes_in_parse_buf = 0; + uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; + +}; + diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc new file mode 100644 index 00000000000000..d9b3e7647d8a2c --- /dev/null +++ b/selfdrive/locationd/ubloxd.cc @@ -0,0 +1,64 @@ +#include + +#include + +#include "cereal/messaging/messaging.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "selfdrive/locationd/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) { + if (errno == EINTR) { + do_exit = true; + } + continue; + } + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); + cereal::Event::Reader event = cmsg.getRoot(); + auto ubloxRaw = event.getUbloxRaw(); + + 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(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/selfdrive/locationd/ubx.ksy b/selfdrive/locationd/ubx.ksy new file mode 100644 index 00000000000000..6945680d329311 --- /dev/null +++ b/selfdrive/locationd/ubx.ksy @@ -0,0 +1,259 @@ +meta: + id: ubx + endian: le +seq: + - id: magic + contents: [0xb5, 0x62] + - id: msg_type + type: u2be + - id: length + type: u2 + - id: body + type: + switch-on: msg_type + cases: + 0x0107: nav_pvt + 0x0213: rxm_sfrbx + 0x0215: rxm_rawx + 0x0a09: mon_hw + 0x0a0b: mon_hw2 +instances: + checksum: + pos: length + 6 + type: u2 + +types: + mon_hw: + seq: + - id: pin_sel + type: u4 + - id: pin_bank + type: u4 + - id: pin_dir + type: u4 + - id: pin_val + type: u4 + - id: noise_per_ms + type: u2 + - id: agc_cnt + type: u2 + - id: a_status + type: u1 + enum: antenna_status + - id: a_power + type: u1 + enum: antenna_power + - id: flags + type: u1 + - id: reserved1 + size: 1 + - id: used_mask + type: u4 + - id: vp + size: 17 + - id: jam_ind + type: u1 + - id: reserved2 + size: 2 + - id: pin_irq + type: u4 + - id: pull_h + type: u4 + - id: pull_l + type: u4 + enums: + antenna_status: + 0: init + 1: dontknow + 2: ok + 3: short + 4: open + antenna_power: + 0: off + 1: on + 2: dontknow + + mon_hw2: + seq: + - id: ofs_i + type: s1 + - id: mag_i + type: u1 + - id: ofs_q + type: s1 + - id: mag_q + type: u1 + - id: cfg_source + type: u1 + enum: config_source + - id: reserved1 + size: 3 + - id: low_lev_cfg + type: u4 + - id: reserved2 + size: 8 + - id: post_status + type: u4 + - id: reserved3 + size: 4 + + enums: + config_source: + 113: rom + 111: otp + 112: config_pins + 102: flash + + rxm_sfrbx: + seq: + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved1 + size: 1 + - id: freq_id + type: u1 + - id: num_words + type: u1 + - id: reserved2 + size: 1 + - id: version + type: u1 + - id: reserved3 + size: 1 + - id: body + type: u4 + repeat: expr + repeat-expr: num_words + + rxm_rawx: + seq: + - id: rcv_tow + type: f8 + - id: week + type: u2 + - id: leap_s + type: s1 + - id: num_meas + type: u1 + - id: rec_stat + type: u1 + - id: reserved1 + size: 3 + - id: measurements + type: meas + size: 32 + repeat: expr + repeat-expr: num_meas + types: + meas: + seq: + - id: pr_mes + type: f8 + - id: cp_mes + type: f8 + - id: do_mes + type: f4 + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved2 + size: 1 + - id: freq_id + type: u1 + - id: lock_time + type: u2 + - id: cno + type: u1 + - id: pr_stdev + type: u1 + - id: cp_stdev + type: u1 + - id: do_stdev + type: u1 + - id: trk_stat + type: u1 + - id: reserved3 + size: 1 + + nav_pvt: + seq: + - id: i_tow + type: u4 + - id: year + type: u2 + - id: month + type: u1 + - id: day + type: u1 + - id: hour + type: u1 + - id: min + type: u1 + - id: sec + type: u1 + - id: valid + type: u1 + - id: t_acc + type: u4 + - id: nano + type: s4 + - id: fix_type + type: u1 + - id: flags + type: u1 + - id: flags2 + type: u1 + - id: num_sv + type: u1 + - id: lon + type: s4 + - id: lat + type: s4 + - id: height + type: s4 + - id: h_msl + type: s4 + - id: h_acc + type: u4 + - id: v_acc + type: u4 + - id: vel_n + type: s4 + - id: vel_e + type: s4 + - id: vel_d + type: s4 + - id: g_speed + type: s4 + - id: head_mot + type: s4 + - id: s_acc + type: s4 + - id: head_acc + type: u4 + - id: p_dop + type: u2 + - id: flags3 + type: u1 + - id: reserved1 + size: 5 + - id: head_veh + type: s4 + - id: mag_dec + type: s2 + - id: mag_acc + type: u2 +enums: + gnss_type: + 0: gps + 1: sbas + 2: galileo + 3: beidou + 4: imes + 5: qzss + 6: glonass diff --git a/system/loggerd/.gitignore b/selfdrive/loggerd/.gitignore similarity index 100% rename from system/loggerd/.gitignore rename to selfdrive/loggerd/.gitignore diff --git a/selfdrive/loggerd/README.md b/selfdrive/loggerd/README.md new file mode 100644 index 00000000000000..714e4242a04508 --- /dev/null +++ b/selfdrive/loggerd/README.md @@ -0,0 +1,30 @@ +# loggerd + +openpilot records routes in one minute chunks called segments. A route starts on the rising edge of ignition and ends on the falling edge. + +Check out our [python library](https://github.com/commaai/openpilot/blob/master/tools/lib/logreader.py) for reading openpilot logs. Also checkout our [tools](https://github.com/commaai/openpilot/tree/master/tools) to replay and view your data. These are the same tools we use to debug and develop openpilot. + +## log types + +For each segment, openpilot records the following log types: + +## rlog.bz2 + +rlogs contain all the messages passed amongst openpilot's processes. See [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for a list of all the logged services. They're a bzip2 archive of the serialized capnproto messages. + +## {f,e,d}camera.hevc + +Each camera stream is H.265 encoded and written to its respective file. +* fcamera.hevc is the road camera +* ecamera.hevc is the wide road camera +* dcamera.hevc is the driver camera + +## qlog.bz2 & qcamera.ts + +qlogs are a decimated subset of the rlogs. Check out [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for the decimation. + + +qcameras are H.264 encoded, lower res versions of the fcamera.hevc. The video shown in [comma connect](https://connect.comma.ai/) is from the qcameras. + + +qlogs and qcameras are designed to be small enough to upload instantly on slow internet and store forever, yet useful enough for most analysis and debugging. diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript new file mode 100644 index 00000000000000..a15aac380d80c7 --- /dev/null +++ b/selfdrive/loggerd/SConscript @@ -0,0 +1,27 @@ +Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc') + +libs = [common, cereal, messaging, visionipc, + 'zmq', 'capnp', 'kj', 'z', + 'avformat', 'avcodec', 'swscale', 'avutil', + 'yuv', 'OpenCL', 'pthread'] + +src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc'] +if arch == "larch64": + src += ['encoder/v4l_encoder.cc'] +else: + src += ['encoder/ffmpeg_encoder.cc'] + +if arch == "Darwin": + # fix OpenCL + del libs[libs.index('OpenCL')] + env['FRAMEWORKS'] = ['OpenCL'] + +logger_lib = env.Library('logger', src) +libs.insert(0, logger_lib) + +env.Program('loggerd', ['loggerd.cc'], LIBS=libs) +env.Program('encoderd', ['encoderd.cc'], LIBS=libs) +env.Program('bootlog.cc', LIBS=libs) + +if GetOption('test'): + env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=libs + ['curl', 'crypto']) diff --git a/tools/sim/bridge/__init__.py b/selfdrive/loggerd/__init__.py similarity index 100% rename from tools/sim/bridge/__init__.py rename to selfdrive/loggerd/__init__.py diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc new file mode 100644 index 00000000000000..6ff052655a465a --- /dev/null +++ b/selfdrive/loggerd/bootlog.cc @@ -0,0 +1,67 @@ +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/swaglog.h" +#include "selfdrive/loggerd/logger.h" + + +static kj::Array build_boot_log() { + MessageBuilder msg; + auto boot = msg.initEvent().initBoot(); + + boot.setWallTimeNanos(nanos_since_epoch()); + + std::string pstore = "/sys/fs/pstore"; + std::map pstore_map = util::read_files_in_dir(pstore); + + int i = 0; + auto lpstore = boot.initPstore().initEntries(pstore_map.size()); + for (auto& kv : pstore_map) { + auto lentry = lpstore[i]; + lentry.setKey(kv.first); + lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size())); + i++; + } + + // Gather output of commands + std::vector bootlog_commands = { + "[ -x \"$(command -v journalctl)\" ] && journalctl", + }; + + if (Hardware::TICI()) { + bootlog_commands.push_back("[ -e /dev/nvme0 ] && sudo nvme smart-log --output-format=json /dev/nvme0"); + } + + auto commands = boot.initCommands().initEntries(bootlog_commands.size()); + for (int j = 0; j < bootlog_commands.size(); j++) { + auto lentry = commands[j]; + + lentry.setKey(bootlog_commands[j]); + + const std::string result = util::check_output(bootlog_commands[j]); + lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); + } + + boot.setLaunchLog(util::read_file("/tmp/launch_log")); + return capnp::messageToFlatArray(msg); +} + +int main(int argc, char** argv) { + const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name(); + LOGW("bootlog to %s", path.c_str()); + + // Open bootlog + bool r = util::create_directories(LOG_ROOT + "/boot/", 0775); + assert(r); + + RawFile bz_file(path.c_str()); + + // Write initdata + bz_file.write(logger_build_init_data().asBytes()); + + // Write bootlog + bz_file.write(build_boot_log().asBytes()); + + return 0; +} diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py new file mode 100644 index 00000000000000..168c9fba91c56b --- /dev/null +++ b/selfdrive/loggerd/config.py @@ -0,0 +1,41 @@ +import os +from pathlib import Path +from system.hardware import PC + +if os.environ.get('LOG_ROOT', False): + ROOT = os.environ['LOG_ROOT'] +elif PC: + ROOT = os.path.join(str(Path.home()), ".comma", "media", "0", "realdata") +else: + ROOT = '/data/media/0/realdata/' + + +CAMERA_FPS = 20 +SEGMENT_LENGTH = 60 + +STATS_DIR_FILE_LIMIT = 10000 +STATS_SOCKET = "ipc:///tmp/stats" +if PC: + STATS_DIR = os.path.join(str(Path.home()), ".comma", "stats") +else: + STATS_DIR = "/data/stats/" +STATS_FLUSH_TIME_S = 60 + +def get_available_percent(default=None): + try: + statvfs = os.statvfs(ROOT) + available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks + except OSError: + available_percent = default + + return available_percent + + +def get_available_bytes(default=None): + try: + statvfs = os.statvfs(ROOT) + available_bytes = statvfs.f_bavail * statvfs.f_frsize + except OSError: + available_bytes = default + + return available_bytes diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py new file mode 100644 index 00000000000000..5606288024d2b4 --- /dev/null +++ b/selfdrive/loggerd/deleter.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +import os +import shutil +import threading +from system.swaglog import cloudlog +from selfdrive.loggerd.config import ROOT, get_available_bytes, get_available_percent +from selfdrive.loggerd.uploader import listdir_by_creation + +MIN_BYTES = 5 * 1024 * 1024 * 1024 +MIN_PERCENT = 10 + +DELETE_LAST = ['boot', 'crash'] + + +def deleter_thread(exit_event): + while not exit_event.is_set(): + out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES + out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT + + if out_of_percent or out_of_bytes: + # remove the earliest directory we can + dirs = sorted(listdir_by_creation(ROOT), key=lambda x: x in DELETE_LAST) + for delete_dir in dirs: + delete_path = os.path.join(ROOT, delete_dir) + + if any(name.endswith(".lock") for name in os.listdir(delete_path)): + continue + + try: + cloudlog.info(f"deleting {delete_path}") + if os.path.isfile(delete_path): + os.remove(delete_path) + else: + shutil.rmtree(delete_path) + break + except OSError: + cloudlog.exception(f"issue deleting {delete_path}") + exit_event.wait(.1) + else: + exit_event.wait(30) + + +def main(): + deleter_thread(threading.Event()) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/loggerd/encoder/encoder.cc b/selfdrive/loggerd/encoder/encoder.cc new file mode 100644 index 00000000000000..943f37803d9ade --- /dev/null +++ b/selfdrive/loggerd/encoder/encoder.cc @@ -0,0 +1,82 @@ +#include +#include "selfdrive/loggerd/encoder/encoder.h" + +VideoEncoder::~VideoEncoder() {} + +void VideoEncoder::publisher_init() { + // publish + service_name = this->type == DriverCam ? "driverEncodeData" : + (this->type == WideRoadCam ? "wideRoadEncodeData" : + (this->in_width == this->out_width ? "roadEncodeData" : "qRoadEncodeData")); + pm.reset(new PubMaster({service_name})); +} + +void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, + unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat) { + // broadcast packet + MessageBuilder msg; + auto event = msg.initEvent(true); + auto edat = (e->type == DriverCam) ? event.initDriverEncodeData() : + ((e->type == WideRoadCam) ? event.initWideRoadEncodeData() : + (e->in_width == e->out_width ? event.initRoadEncodeData() : event.initQRoadEncodeData())); + auto edata = edat.initIdx(); + struct timespec ts; + timespec_get(&ts, TIME_UTC); + edat.setUnixTimestampNanos((uint64_t)ts.tv_sec*1000000000 + ts.tv_nsec); + edata.setFrameId(extra.frame_id); + edata.setTimestampSof(extra.timestamp_sof); + edata.setTimestampEof(extra.timestamp_eof); + edata.setType(e->codec); + edata.setEncodeId(e->cnt++); + edata.setSegmentNum(segment_num); + edata.setSegmentId(idx); + edata.setFlags(flags); + edata.setLen(dat.size()); + edat.setData(dat); + if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header); + + auto words = new kj::Array(capnp::messageToFlatArray(msg)); + auto bytes = words->asBytes(); + e->pm->send(e->service_name, bytes.begin(), bytes.size()); + if (e->write) { + e->to_write.push(words); + } else { + delete words; + } +} + +// TODO: writing should be moved to loggerd +void VideoEncoder::write_handler(VideoEncoder *e, const char *path) { + VideoWriter writer(path, e->filename, e->codec != cereal::EncodeIndex::Type::FULL_H_E_V_C, e->out_width, e->out_height, e->fps, e->codec); + + bool first = true; + kj::Array* out_buf; + while ((out_buf = e->to_write.pop())) { + capnp::FlatArrayMessageReader cmsg(*out_buf); + cereal::Event::Reader event = cmsg.getRoot(); + + auto edata = (e->type == DriverCam) ? event.getDriverEncodeData() : + ((e->type == WideRoadCam) ? event.getWideRoadEncodeData() : + (e->in_width == e->out_width ? event.getRoadEncodeData() : event.getQRoadEncodeData())); + auto idx = edata.getIdx(); + auto flags = idx.getFlags(); + + if (first) { + assert(flags & V4L2_BUF_FLAG_KEYFRAME); + auto header = edata.getHeader(); + writer.write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false); + first = false; + } + + // dangerous cast from const, but should be fine + auto data = edata.getData(); + if (data.size() > 0) { + writer.write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME); + } + + // free the data + delete out_buf; + } + + // VideoWriter is freed on out of scope +} diff --git a/selfdrive/loggerd/encoder/encoder.h b/selfdrive/loggerd/encoder/encoder.h new file mode 100644 index 00000000000000..21ef65cf12bbef --- /dev/null +++ b/selfdrive/loggerd/encoder/encoder.h @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc.h" +#include "common/queue.h" +#include "selfdrive/loggerd/video_writer.h" +#include "system/camerad/cameras/camera_common.h" + +#define V4L2_BUF_FLAG_KEYFRAME 8 + +class VideoEncoder { +public: + VideoEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, + int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) + : filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps), + bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { } + virtual ~VideoEncoder(); + virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; + virtual void encoder_open(const char* path) = 0; + virtual void encoder_close() = 0; + + void publisher_init(); + static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat); + + void writer_open(const char* path) { + if (this->write) write_handler_thread = std::thread(VideoEncoder::write_handler, this, path); + } + + void writer_close() { + if (this->write) { + to_write.push(NULL); + write_handler_thread.join(); + } + assert(to_write.empty()); + } + +protected: + bool write; + const char* filename; + int in_width, in_height; + int out_width, out_height, fps; + int bitrate; + cereal::EncodeIndex::Type codec; + CameraType type; + +private: + // total frames encoded + int cnt = 0; + + // publishing + std::unique_ptr pm; + const char *service_name; + + // writing support + static void write_handler(VideoEncoder *e, const char *path); + std::thread write_handler_thread; + SafeQueue* > to_write; +}; diff --git a/system/loggerd/encoder/ffmpeg_encoder.cc b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc similarity index 81% rename from system/loggerd/encoder/ffmpeg_encoder.cc rename to selfdrive/loggerd/encoder/ffmpeg_encoder.cc index 4d6be471821a2e..5f8d140e8b395b 100644 --- a/system/loggerd/encoder/ffmpeg_encoder.cc +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.cc @@ -1,4 +1,6 @@ -#include "system/loggerd/encoder/ffmpeg_encoder.h" +#pragma clang diagnostic ignored "-Wdeprecated-declarations" + +#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h" #include #include @@ -9,7 +11,7 @@ #define __STDC_CONSTANT_MACROS -#include "third_party/libyuv/include/libyuv.h" +#include "libyuv.h" extern "C" { #include @@ -22,8 +24,7 @@ extern "C" { const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0; -FfmpegEncoder::FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height) - : VideoEncoder(encoder_info, in_width, in_height) { +void FfmpegEncoder::encoder_init() { frame = av_frame_alloc(); assert(frame); frame->format = AV_PIX_FMT_YUV420P; @@ -38,6 +39,8 @@ FfmpegEncoder::FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int if (in_width != out_width || in_height != out_height) { downscale_buf.resize(out_width * out_height * 3 / 2); } + + publisher_init(); } FfmpegEncoder::~FfmpegEncoder() { @@ -45,21 +48,19 @@ FfmpegEncoder::~FfmpegEncoder() { av_frame_free(&frame); } -void FfmpegEncoder::encoder_open() { - auto codec_id = encoder_info.get_settings(in_width).encode_type == cereal::EncodeIndex::Type::QCAMERA_H264 - ? AV_CODEC_ID_H264 - : AV_CODEC_ID_FFVHUFF; - const AVCodec *codec = avcodec_find_encoder(codec_id); +void FfmpegEncoder::encoder_open(const char* path) { + const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF); this->codec_ctx = avcodec_alloc_context3(codec); assert(this->codec_ctx); this->codec_ctx->width = frame->width; this->codec_ctx->height = frame->height; this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; - this->codec_ctx->time_base = (AVRational){ 1, encoder_info.fps }; + this->codec_ctx->time_base = (AVRational){ 1, fps }; int err = avcodec_open2(this->codec_ctx, codec, NULL); assert(err >= 0); + writer_open(path); is_open = true; segment_num++; counter = 0; @@ -68,6 +69,7 @@ void FfmpegEncoder::encoder_open() { void FfmpegEncoder::encoder_close() { if (!is_open) return; + writer_close(); avcodec_free_context(&codec_ctx); is_open = false; } @@ -117,7 +119,8 @@ int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { ret = -1; } - AVPacket pkt = {}; + AVPacket pkt; + av_init_packet(&pkt); pkt.data = NULL; pkt.size = 0; while (ret >= 0) { @@ -135,10 +138,10 @@ int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { } if (env_debug_encoder) { - printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id); + printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", this->filename, pkt.size, pkt.flags, counter, extra->frame_id); } - publisher_publish(segment_num, counter, *extra, + publisher_publish(this, segment_num, counter, *extra, (pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0, kj::arrayPtr(pkt.data, (size_t)0), // TODO: get the header kj::arrayPtr(pkt.data, pkt.size)); diff --git a/selfdrive/loggerd/encoder/ffmpeg_encoder.h b/selfdrive/loggerd/encoder/ffmpeg_encoder.h new file mode 100644 index 00000000000000..497a28b651f6c2 --- /dev/null +++ b/selfdrive/loggerd/encoder/ffmpeg_encoder.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include +#include + +extern "C" { +#include +#include +#include +} + +#include "selfdrive/loggerd/encoder/encoder.h" +#include "selfdrive/loggerd/loggerd.h" + +class FfmpegEncoder : public VideoEncoder { + public: + FfmpegEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, + int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) : + VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); } + ~FfmpegEncoder(); + void encoder_init(); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); + void encoder_open(const char* path); + void encoder_close(); + +private: + int segment_num = -1; + int counter = 0; + bool is_open = false; + + AVCodecContext *codec_ctx; + AVFrame *frame = NULL; + std::vector convert_buf; + std::vector downscale_buf; +}; diff --git a/selfdrive/loggerd/encoder/v4l_encoder.cc b/selfdrive/loggerd/encoder/v4l_encoder.cc new file mode 100644 index 00000000000000..88aeb212563534 --- /dev/null +++ b/selfdrive/loggerd/encoder/v4l_encoder.cc @@ -0,0 +1,311 @@ +#include +#include +#include + +#include "selfdrive/loggerd/encoder/v4l_encoder.h" +#include "common/util.h" +#include "common/timing.h" + +#include "libyuv.h" +#include "msm_media_info.h" + +// has to be in this order +#include "v4l2-controls.h" +#include +#define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000 +#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000 + +// echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level +const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0; + +#define checked_ioctl(x,y,z) { int _ret = HANDLE_EINTR(ioctl(x,y,z)); if (_ret!=0) { LOGE("checked_ioctl failed %d %lx %p", x, y, z); } assert(_ret==0); } + +static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) { + v4l2_plane plane = {0}; + v4l2_buffer v4l_buf = { + .type = buf_type, + .memory = V4L2_MEMORY_USERPTR, + .m = { .planes = &plane, }, + .length = 1, + }; + checked_ioctl(fd, VIDIOC_DQBUF, &v4l_buf); + + if (index) *index = v4l_buf.index; + if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused; + if (flags) *flags = v4l_buf.flags; + if (timestamp) *timestamp = v4l_buf.timestamp; + assert(v4l_buf.m.planes[0].data_offset == 0); +} + +static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) { + v4l2_plane plane = { + .length = (unsigned int)buf->len, + .m = { .userptr = (unsigned long)buf->addr, }, + .bytesused = (uint32_t)buf->len, + .reserved = {(unsigned int)buf->fd} + }; + + v4l2_buffer v4l_buf = { + .type = buf_type, + .index = index, + .memory = V4L2_MEMORY_USERPTR, + .m = { .planes = &plane, }, + .length = 1, + .flags = V4L2_BUF_FLAG_TIMESTAMP_COPY, + .timestamp = timestamp + }; + + checked_ioctl(fd, VIDIOC_QBUF, &v4l_buf); +} + +static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) { + struct v4l2_requestbuffers reqbuf = { + .type = buf_type, + .memory = V4L2_MEMORY_USERPTR, + .count = count + }; + checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf); +} + +void V4LEncoder::dequeue_handler(V4LEncoder *e) { + std::string dequeue_thread_name = "dq-"+std::string(e->filename); + util::set_thread_name(dequeue_thread_name.c_str()); + + e->segment_num++; + uint32_t idx = -1; + bool exit = false; + + // POLLIN is capture, POLLOUT is frame + struct pollfd pfd; + pfd.events = POLLIN | POLLOUT; + pfd.fd = e->fd; + + // save the header + kj::Array header; + + while (!exit) { + int rc = poll(&pfd, 1, 1000); + if (!rc) { LOGE("encoder dequeue poll timeout"); continue; } + + if (env_debug_encoder >= 2) { + printf("%20s poll %x at %.2f ms\n", e->filename, pfd.revents, millis_since_boot()); + } + + int frame_id = -1; + if (pfd.revents & POLLIN) { + unsigned int bytesused, flags, index; + struct timeval timestamp; + dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &index, &bytesused, &flags, ×tamp); + e->buf_out[index].sync(VISIONBUF_SYNC_FROM_DEVICE); + uint8_t *buf = (uint8_t*)e->buf_out[index].addr; + int64_t ts = timestamp.tv_sec * 1000000 + timestamp.tv_usec; + + // eof packet, we exit + if (flags & V4L2_QCOM_BUF_FLAG_EOS) { + exit = true; + } else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) { + // save header + header = kj::heapArray(buf, bytesused); + } else { + VisionIpcBufExtra extra = e->extras.pop(); + assert(extra.timestamp_eof/1000 == ts); // stay in sync + frame_id = extra.frame_id; + ++idx; + e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr(buf, bytesused)); + } + + if (env_debug_encoder) { + printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n", + e->filename, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size()); + } + + // requeue the buffer + queue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, &e->buf_out[index]); + } + + if (pfd.revents & POLLOUT) { + unsigned int index; + dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index); + e->free_buf_in.push(index); + } + } +} + +void V4LEncoder::encoder_init() { + fd = open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK); + assert(fd >= 0); + + struct v4l2_capability cap; + checked_ioctl(fd, VIDIOC_QUERYCAP, &cap); + LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd); + assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0); + assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0); + + struct v4l2_format fmt_out = { + .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, + .fmt = { + .pix_mp = { + // downscales are free with v4l + .width = (unsigned int)out_width, + .height = (unsigned int)out_height, + .pixelformat = (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264, + .field = V4L2_FIELD_ANY, + .colorspace = V4L2_COLORSPACE_DEFAULT, + } + } + }; + checked_ioctl(fd, VIDIOC_S_FMT, &fmt_out); + + v4l2_streamparm streamparm = { + .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, + .parm = { + .output = { + // TODO: more stuff here? we don't know + .timeperframe = { + .numerator = 1, + .denominator = 20 + } + } + } + }; + checked_ioctl(fd, VIDIOC_S_PARM, &streamparm); + + struct v4l2_format fmt_in = { + .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, + .fmt = { + .pix_mp = { + .width = (unsigned int)in_width, + .height = (unsigned int)in_height, + .pixelformat = V4L2_PIX_FMT_NV12, + .field = V4L2_FIELD_ANY, + .colorspace = V4L2_COLORSPACE_470_SYSTEM_BG, + } + } + }; + checked_ioctl(fd, VIDIOC_S_FMT, &fmt_in); + + LOGD("in buffer size %d, out buffer size %d", + fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage, + fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage); + + // shared ctrls + { + struct v4l2_control ctrls[] = { + { .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE, .value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE}, + { .id = V4L2_CID_MPEG_VIDEO_BITRATE, .value = bitrate}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL, .value = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1}, + }; + for (auto ctrl : ctrls) { + checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + } + } + + if (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) { + struct v4l2_control ctrls[] = { + { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 29}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0}, + }; + for (auto ctrl : ctrls) { + checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + } + } else { + struct v4l2_control ctrls[] = { + { .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH}, + { .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 14}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0}, + { .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC}, + { .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0}, + { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE, .value = 0}, + { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, .value = 0}, + { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, .value = 0}, + { .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0}, + }; + for (auto ctrl : ctrls) { + checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl); + } + } + + // allocate buffers + request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, BUF_OUT_COUNT); + request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, BUF_IN_COUNT); + + // start encoder + v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + checked_ioctl(fd, VIDIOC_STREAMON, &buf_type); + buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + checked_ioctl(fd, VIDIOC_STREAMON, &buf_type); + + // queue up output buffers + for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) { + buf_out[i].allocate(fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage); + queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, i, &buf_out[i]); + } + // queue up input buffers + for (unsigned int i = 0; i < BUF_IN_COUNT; i++) { + free_buf_in.push(i); + } + + publisher_init(); +} + +void V4LEncoder::encoder_open(const char* path) { + dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this); + writer_open(path); + this->is_open = true; + this->counter = 0; +} + +int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { + struct timeval timestamp { + .tv_sec = (long)(extra->timestamp_eof/1000000000), + .tv_usec = (long)((extra->timestamp_eof/1000) % 1000000), + }; + + // reserve buffer + int buffer_in = free_buf_in.pop(); + + // push buffer + extras.push(*extra); + //buf->sync(VISIONBUF_SYNC_TO_DEVICE); + queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp); + + return this->counter++; +} + +void V4LEncoder::encoder_close() { + if (this->is_open) { + // pop all the frames before closing, then put the buffers back + for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.pop(); + for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i); + // no frames, stop the encoder + struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP }; + checked_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd); + // join waits for V4L2_QCOM_BUF_FLAG_EOS + dequeue_handler_thread.join(); + assert(extras.empty()); + writer_close(); + } + this->is_open = false; +} + +V4LEncoder::~V4LEncoder() { + encoder_close(); + v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type); + request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0); + buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type); + request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0); + close(fd); + + for (int i = 0; i < BUF_OUT_COUNT; i++) { + if (buf_out[i].free() != 0) { + LOGE("Failed to free buffer"); + } + } +} diff --git a/selfdrive/loggerd/encoder/v4l_encoder.h b/selfdrive/loggerd/encoder/v4l_encoder.h new file mode 100644 index 00000000000000..b7c378be85a5a4 --- /dev/null +++ b/selfdrive/loggerd/encoder/v4l_encoder.h @@ -0,0 +1,34 @@ +#pragma once + +#include "common/queue.h" +#include "selfdrive/loggerd/encoder/encoder.h" + +#define BUF_IN_COUNT 7 +#define BUF_OUT_COUNT 6 + +class V4LEncoder : public VideoEncoder { +public: + V4LEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, + int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) : + VideoEncoder(filename, type, in_width, in_height, fps, bitrate, codec, out_width, out_height, write) { encoder_init(); } + ~V4LEncoder(); + void encoder_init(); + int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); + void encoder_open(const char* path); + void encoder_close(); +private: + int fd; + + bool is_open = false; + int segment_num = -1; + int counter = 0; + + SafeQueue extras; + + static void dequeue_handler(V4LEncoder *e); + std::thread dequeue_handler_thread; + + VisionBuf buf_in[BUF_IN_COUNT]; + VisionBuf buf_out[BUF_OUT_COUNT]; + SafeQueue free_buf_in; +}; diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc new file mode 100644 index 00000000000000..db5f4b61ab92c0 --- /dev/null +++ b/selfdrive/loggerd/encoderd.cc @@ -0,0 +1,149 @@ +#include "selfdrive/loggerd/loggerd.h" + +ExitHandler do_exit; + +struct EncoderdState { + int max_waiting = 0; + + // Sync logic for startup + std::atomic encoders_ready = 0; + std::atomic start_frame_id = 0; + bool camera_ready[WideRoadCam + 1] = {}; + bool camera_synced[WideRoadCam + 1] = {}; +}; + +// Handle initial encoder syncing by waiting for all encoders to reach the same frame id +bool sync_encoders(EncoderdState *s, CameraType cam_type, uint32_t frame_id) { + if (s->camera_synced[cam_type]) return true; + + if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) { + // add a small margin to the start frame id in case one of the encoders already dropped the next frame + update_max_atomic(s->start_frame_id, frame_id + 2); + if (std::exchange(s->camera_ready[cam_type], true) == false) { + ++s->encoders_ready; + LOGD("camera %d encoder ready", cam_type); + } + return false; + } else { + if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id); + bool synced = frame_id >= s->start_frame_id; + s->camera_synced[cam_type] = synced; + if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); + return synced; + } +} + + +void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { + util::set_thread_name(cam_info.filename); + + std::vector encoders; + VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false); + + int cur_seg = 0; + while (!do_exit) { + if (!vipc_client.connect(false)) { + util::sleep_for(5); + continue; + } + + // init encoders + if (encoders.empty()) { + VisionBuf buf_info = vipc_client.buffers[0]; + LOGW("encoder %s init %dx%d", cam_info.filename, buf_info.width, buf_info.height); + + if (buf_info.width > 0 && buf_info.height > 0) { + // main encoder + encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height, + cam_info.fps, cam_info.bitrate, + cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + buf_info.width, buf_info.height, false)); + // qcamera encoder + if (cam_info.has_qcamera) { + encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height, + qcam_info.fps, qcam_info.bitrate, + qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + qcam_info.frame_width, qcam_info.frame_height, false)); + } + } else { + LOGE("not initting empty encoder"); + s->max_waiting--; + break; + } + } + + for (int i = 0; i < encoders.size(); ++i) { + encoders[i]->encoder_open(NULL); + } + + bool lagging = false; + while (!do_exit) { + VisionIpcBufExtra extra; + VisionBuf* buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; + + // detect loop around and drop the frames + if (buf->get_frame_id() != extra.frame_id) { + if (!lagging) { + LOGE("encoder %s lag buffer id: %d extra id: %d", cam_info.filename, buf->get_frame_id(), extra.frame_id); + lagging = true; + } + continue; + } + lagging = false; + + if (!sync_encoders(s, cam_info.type, extra.frame_id)) { + continue; + } + if (do_exit) break; + + // do rotation if required + const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS; + if (cur_seg >= 0 && extra.frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) { + for (auto &e : encoders) { + e->encoder_close(); + e->encoder_open(NULL); + } + ++cur_seg; + } + + // encode a frame + for (int i = 0; i < encoders.size(); ++i) { + int out_id = encoders[i]->encode_frame(buf, &extra); + + if (out_id == -1) { + LOGE("Failed to encode frame. frame_id: %d", extra.frame_id); + } + } + } + } + + LOG("encoder destroy"); + for(auto &e : encoders) { + e->encoder_close(); + delete e; + } +} + +void encoderd_thread() { + EncoderdState s; + + std::vector encoder_threads; + for (const auto &cam : cameras_logged) { + encoder_threads.push_back(std::thread(encoder_thread, &s, cam)); + s.max_waiting++; + } + for (auto &t : encoder_threads) t.join(); +} + +int main() { + if (!Hardware::PC()) { + int ret; + ret = util::set_realtime_priority(52); + assert(ret == 0); + ret = util::set_core_affinity({3}); + assert(ret == 0); + } + encoderd_thread(); + return 0; +} diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc new file mode 100644 index 00000000000000..aaf267e523a593 --- /dev/null +++ b/selfdrive/loggerd/logger.cc @@ -0,0 +1,250 @@ +#include "selfdrive/loggerd/logger.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/version.h" + +// ***** log metadata ***** +kj::Array logger_build_init_data() { + MessageBuilder msg; + auto init = msg.initEvent().initInitData(); + + init.setVersion(COMMA_VERSION); + init.setDirty(!getenv("CLEAN")); + init.setDeviceType(Hardware::get_device_type()); + + // log kernel args + std::ifstream cmdline_stream("/proc/cmdline"); + std::vector kernel_args; + std::string buf; + while (cmdline_stream >> buf) { + kernel_args.push_back(buf); + } + + auto lkernel_args = init.initKernelArgs(kernel_args.size()); + for (int i=0; i params_map = params.readAll(); + + init.setGitCommit(params_map["GitCommit"]); + init.setGitBranch(params_map["GitBranch"]); + init.setGitRemote(params_map["GitRemote"]); + init.setPassive(params.getBool("Passive")); + init.setDongleId(params_map["DongleId"]); + + auto lparams = init.initParams().initEntries(params_map.size()); + int j = 0; + for (auto& [key, value] : params_map) { + auto lentry = lparams[j]; + lentry.setKey(key); + if ( !(params.getKeyType(key) & DONT_LOG) ) { + lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size())); + } + j++; + } + + // log commands + std::vector log_commands = { + "df -h", // usage for all filesystems + }; + + auto commands = init.initCommands().initEntries(log_commands.size()); + for (int i = 0; i < log_commands.size(); i++) { + auto lentry = commands[i]; + + lentry.setKey(log_commands[i]); + + const std::string result = util::check_output(log_commands[i]); + lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); + } + + return capnp::messageToFlatArray(msg); +} + +std::string logger_get_route_name() { + char route_name[64] = {'\0'}; + time_t rawtime = time(NULL); + struct tm timeinfo; + localtime_r(&rawtime, &timeinfo); + strftime(route_name, sizeof(route_name), "%Y-%m-%d--%H-%M-%S", &timeinfo); + return route_name; +} + +void log_init_data(LoggerState *s) { + auto bytes = s->init_data.asBytes(); + logger_log(s, bytes.begin(), bytes.size(), s->has_qlog); +} + + +static void lh_log_sentinel(LoggerHandle *h, SentinelType type) { + MessageBuilder msg; + auto sen = msg.initEvent().initSentinel(); + sen.setType(type); + sen.setSignal(h->exit_signal); + auto bytes = msg.toBytes(); + + lh_log(h, bytes.begin(), bytes.size(), true); +} + +// ***** logging functions ***** + +void logger_init(LoggerState *s, bool has_qlog) { + pthread_mutex_init(&s->lock, NULL); + + s->part = -1; + s->has_qlog = has_qlog; + s->route_name = logger_get_route_name(); + s->init_data = logger_build_init_data(); +} + +static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { + LoggerHandle *h = NULL; + for (int i=0; ihandles[i].refcnt == 0) { + h = &s->handles[i]; + break; + } + } + assert(h); + + snprintf(h->segment_path, sizeof(h->segment_path), + "%s/%s--%d", root_path, s->route_name.c_str(), s->part); + + snprintf(h->log_path, sizeof(h->log_path), "%s/rlog", h->segment_path); + snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog", h->segment_path); + snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path); + h->end_sentinel_type = SentinelType::END_OF_SEGMENT; + h->exit_signal = 0; + + if (!util::create_directories(h->segment_path, 0775)) return nullptr; + + FILE* lock_file = fopen(h->lock_path, "wb"); + if (lock_file == NULL) return NULL; + fclose(lock_file); + + h->log = std::make_unique(h->log_path); + if (s->has_qlog) { + h->q_log = std::make_unique(h->qlog_path); + } + + pthread_mutex_init(&h->lock, NULL); + h->refcnt++; + return h; +} + +int logger_next(LoggerState *s, const char* root_path, + char* out_segment_path, size_t out_segment_path_len, + int* out_part) { + bool is_start_of_route = !s->cur_handle; + + pthread_mutex_lock(&s->lock); + s->part++; + + LoggerHandle* next_h = logger_open(s, root_path); + if (!next_h) { + pthread_mutex_unlock(&s->lock); + return -1; + } + + if (s->cur_handle) { + lh_close(s->cur_handle); + } + s->cur_handle = next_h; + + if (out_segment_path) { + snprintf(out_segment_path, out_segment_path_len, "%s", next_h->segment_path); + } + if (out_part) { + *out_part = s->part; + } + + pthread_mutex_unlock(&s->lock); + + // write beginning of log metadata + log_init_data(s); + lh_log_sentinel(s->cur_handle, is_start_of_route ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT); + return 0; +} + +LoggerHandle* logger_get_handle(LoggerState *s) { + pthread_mutex_lock(&s->lock); + LoggerHandle* h = s->cur_handle; + if (h) { + pthread_mutex_lock(&h->lock); + h->refcnt++; + pthread_mutex_unlock(&h->lock); + } + pthread_mutex_unlock(&s->lock); + return h; +} + +void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) { + pthread_mutex_lock(&s->lock); + if (s->cur_handle) { + lh_log(s->cur_handle, data, data_size, in_qlog); + } + pthread_mutex_unlock(&s->lock); +} + +void logger_close(LoggerState *s, ExitHandler *exit_handler) { + pthread_mutex_lock(&s->lock); + if (s->cur_handle) { + s->cur_handle->exit_signal = exit_handler && exit_handler->signal.load(); + s->cur_handle->end_sentinel_type = SentinelType::END_OF_ROUTE; + lh_close(s->cur_handle); + } + pthread_mutex_unlock(&s->lock); +} + +void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) { + pthread_mutex_lock(&h->lock); + assert(h->refcnt > 0); + h->log->write(data, data_size); + if (in_qlog && h->q_log) { + h->q_log->write(data, data_size); + } + pthread_mutex_unlock(&h->lock); +} + +void lh_close(LoggerHandle* h) { + pthread_mutex_lock(&h->lock); + assert(h->refcnt > 0); + if (h->refcnt == 1) { + // a very ugly hack. only here can guarantee sentinel is the last msg + pthread_mutex_unlock(&h->lock); + lh_log_sentinel(h, h->end_sentinel_type); + pthread_mutex_lock(&h->lock); + } + h->refcnt--; + if (h->refcnt == 0) { + h->log.reset(nullptr); + h->q_log.reset(nullptr); + unlink(h->lock_path); + pthread_mutex_unlock(&h->lock); + pthread_mutex_destroy(&h->lock); + return; + } + pthread_mutex_unlock(&h->lock); +} diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h new file mode 100644 index 00000000000000..e7594cee881b8d --- /dev/null +++ b/selfdrive/loggerd/logger.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/util.h" +#include "common/swaglog.h" +#include "system/hardware/hw.h" + +const std::string LOG_ROOT = Path::log_root(); + +#define LOGGER_MAX_HANDLES 16 + +class RawFile { + public: + RawFile(const char* path) { + file = util::safe_fopen(path, "wb"); + assert(file != nullptr); + } + ~RawFile() { + util::safe_fflush(file); + int err = fclose(file); + assert(err == 0); + } + inline void write(void* data, size_t size) { + int written = util::safe_fwrite(data, 1, size, file); + assert(written == size); + } + inline void write(kj::ArrayPtr array) { write(array.begin(), array.size()); } + + private: + FILE* file = nullptr; +}; + +typedef cereal::Sentinel::SentinelType SentinelType; + +typedef struct LoggerHandle { + pthread_mutex_t lock; + SentinelType end_sentinel_type; + int exit_signal; + int refcnt; + char segment_path[4096]; + char log_path[4096]; + char qlog_path[4096]; + char lock_path[4096]; + std::unique_ptr log, q_log; +} LoggerHandle; + +typedef struct LoggerState { + pthread_mutex_t lock; + int part; + kj::Array init_data; + std::string route_name; + char log_name[64]; + bool has_qlog; + + LoggerHandle handles[LOGGER_MAX_HANDLES]; + LoggerHandle* cur_handle; +} LoggerState; + +kj::Array logger_build_init_data(); +std::string logger_get_route_name(); +void logger_init(LoggerState *s, bool has_qlog); +int logger_next(LoggerState *s, const char* root_path, + char* out_segment_path, size_t out_segment_path_len, + int* out_part); +LoggerHandle* logger_get_handle(LoggerState *s); +void logger_close(LoggerState *s, ExitHandler *exit_handler=nullptr); +void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog); + +void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog); +void lh_close(LoggerHandle* h); diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc new file mode 100644 index 00000000000000..e0892e68b42ed4 --- /dev/null +++ b/selfdrive/loggerd/loggerd.cc @@ -0,0 +1,276 @@ +#include "selfdrive/loggerd/loggerd.h" +#include "selfdrive/loggerd/video_writer.h" + +ExitHandler do_exit; + +struct LoggerdState { + LoggerState logger = {}; + char segment_path[4096]; + std::atomic rotate_segment; + std::atomic last_camera_seen_tms; + std::atomic ready_to_rotate; // count of encoders ready to rotate + int max_waiting = 0; + double last_rotate_tms = 0.; // last rotate time in ms +}; + +void logger_rotate(LoggerdState *s) { + int segment = -1; + int err = logger_next(&s->logger, LOG_ROOT.c_str(), s->segment_path, sizeof(s->segment_path), &segment); + assert(err == 0); + s->rotate_segment = segment; + s->ready_to_rotate = 0; + s->last_rotate_tms = millis_since_boot(); + LOGW((s->logger.part == 0) ? "logging to %s" : "rotated to %s", s->segment_path); +} + +void rotate_if_needed(LoggerdState *s) { + if (s->ready_to_rotate == s->max_waiting) { + logger_rotate(s); + } + + double tms = millis_since_boot(); + if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 && + (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE && + !LOGGERD_TEST) { + LOGW("no camera packet seen. auto rotating"); + logger_rotate(s); + } +} + +struct RemoteEncoder { + std::unique_ptr writer; + int encoderd_segment_offset; + int current_segment = -1; + std::vector q; + int dropped_frames = 0; + bool recording = false; + bool marked_ready_to_rotate = false; + bool seen_first_packet = false; +}; + +int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct RemoteEncoder &re) { + const LogCameraInfo &cam_info = (name == "driverEncodeData") ? cameras_logged[1] : + ((name == "wideRoadEncodeData") ? cameras_logged[2] : + ((name == "qRoadEncodeData") ? qcam_info : cameras_logged[0])); + int bytes_count = 0; + + // extract the message + capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr((capnp::word *)msg->getData(), msg->getSize())); + auto event = cmsg.getRoot(); + auto edata = (name == "driverEncodeData") ? event.getDriverEncodeData() : + ((name == "wideRoadEncodeData") ? event.getWideRoadEncodeData() : + ((name == "qRoadEncodeData") ? event.getQRoadEncodeData() : event.getRoadEncodeData())); + auto idx = edata.getIdx(); + auto flags = idx.getFlags(); + + // encoderd can have started long before loggerd + if (!re.seen_first_packet) { + re.seen_first_packet = true; + re.encoderd_segment_offset = idx.getSegmentNum(); + LOGD("%s: has encoderd offset %d", name.c_str(), re.encoderd_segment_offset); + } + int offset_segment_num = idx.getSegmentNum() - re.encoderd_segment_offset; + + if (offset_segment_num == s->rotate_segment) { + // loggerd is now on the segment that matches this packet + + // if this is a new segment, we close any possible old segments, move to the new, and process any queued packets + if (re.current_segment != s->rotate_segment) { + if (re.recording) { + re.writer.reset(); + re.recording = false; + } + re.current_segment = s->rotate_segment; + re.marked_ready_to_rotate = false; + // we are in this segment now, process any queued messages before this one + if (!re.q.empty()) { + for (auto &qmsg: re.q) { + bytes_count += handle_encoder_msg(s, qmsg, name, re); + } + re.q.clear(); + } + } + + // if we aren't recording yet, try to start, since we are in the correct segment + if (!re.recording) { + if (flags & V4L2_BUF_FLAG_KEYFRAME) { + // only create on iframe + if (re.dropped_frames) { + // this should only happen for the first segment, maybe + LOGW("%s: dropped %d non iframe packets before init", name.c_str(), re.dropped_frames); + re.dropped_frames = 0; + } + // if we aren't actually recording, don't create the writer + if (cam_info.record) { + re.writer.reset(new VideoWriter(s->segment_path, + cam_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C, + cam_info.frame_width, cam_info.frame_height, cam_info.fps, idx.getType())); + // write the header + auto header = edata.getHeader(); + re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false); + } + re.recording = true; + } else { + // this is a sad case when we aren't recording, but don't have an iframe + // nothing we can do but drop the frame + delete msg; + ++re.dropped_frames; + return bytes_count; + } + } + + // we have to be recording if we are here + assert(re.recording); + + // if we are actually writing the video file, do so + if (re.writer) { + auto data = edata.getData(); + re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME); + } + + // put it in log stream as the idx packet + MessageBuilder bmsg; + auto evt = bmsg.initEvent(event.getValid()); + evt.setLogMonoTime(event.getLogMonoTime()); + if (name == "driverEncodeData") { evt.setDriverEncodeIdx(idx); } + if (name == "wideRoadEncodeData") { evt.setWideRoadEncodeIdx(idx); } + if (name == "qRoadEncodeData") { evt.setQRoadEncodeIdx(idx); } + if (name == "roadEncodeData") { evt.setRoadEncodeIdx(idx); } + auto new_msg = bmsg.toBytes(); + logger_log(&s->logger, (uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog? + bytes_count += new_msg.size(); + + // free the message, we used it + delete msg; + } else if (offset_segment_num > s->rotate_segment) { + // encoderd packet has a newer segment, this means encoderd has rolled over + if (!re.marked_ready_to_rotate) { + re.marked_ready_to_rotate = true; + ++s->ready_to_rotate; + LOGD("rotate %d -> %d ready %d/%d for %s", + s->rotate_segment.load(), offset_segment_num, + s->ready_to_rotate.load(), s->max_waiting, name.c_str()); + } + // queue up all the new segment messages, they go in after the rotate + re.q.push_back(msg); + } else { + LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->rotate_segment:%d re.encoderd_segment_offset:%d", + name.c_str(), idx.getSegmentNum(), s->rotate_segment.load(), re.encoderd_segment_offset); + // free the message, it's useless. this should never happen + // actually, this can happen if you restart encoderd + re.encoderd_segment_offset = -s->rotate_segment.load(); + delete msg; + } + + return bytes_count; +} + +void loggerd_thread() { + // setup messaging + typedef struct QlogState { + std::string name; + int counter, freq; + bool encoder; + } QlogState; + std::unordered_map qlog_states; + std::unordered_map remote_encoders; + + std::unique_ptr ctx(Context::create()); + std::unique_ptr poller(Poller::create()); + + // subscribe to all socks + for (const auto& it : services) { + const bool encoder = strcmp(it.name+strlen(it.name)-strlen("EncodeData"), "EncodeData") == 0; + if (!it.should_log && !encoder) continue; + LOGD("logging %s (on port %d)", it.name, it.port); + + SubSocket * sock = SubSocket::create(ctx.get(), it.name); + assert(sock != NULL); + poller->registerSocket(sock); + qlog_states[sock] = { + .name = it.name, + .counter = 0, + .freq = it.decimation, + .encoder = encoder, + }; + } + + LoggerdState s; + // init logger + logger_init(&s.logger, true); + logger_rotate(&s); + Params().put("CurrentRoute", s.logger.route_name); + + // init encoders + s.last_camera_seen_tms = millis_since_boot(); + for (const auto &cam : cameras_logged) { + s.max_waiting++; + if (cam.has_qcamera) { s.max_waiting++; } + } + + uint64_t msg_count = 0, bytes_count = 0; + double start_ts = millis_since_boot(); + while (!do_exit) { + // poll for new messages on all sockets + for (auto sock : poller->poll(1000)) { + if (do_exit) break; + + // drain socket + int count = 0; + QlogState &qs = qlog_states[sock]; + Message *msg = nullptr; + while (!do_exit && (msg = sock->receive(true))) { + const bool in_qlog = qs.freq != -1 && (qs.counter++ % qs.freq == 0); + + if (qs.encoder) { + s.last_camera_seen_tms = millis_since_boot(); + bytes_count += handle_encoder_msg(&s, msg, qs.name, remote_encoders[sock]); + } else { + logger_log(&s.logger, (uint8_t *)msg->getData(), msg->getSize(), in_qlog); + bytes_count += msg->getSize(); + delete msg; + } + + rotate_if_needed(&s); + + if ((++msg_count % 1000) == 0) { + double seconds = (millis_since_boot() - start_ts) / 1000.0; + LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds); + } + + count++; + if (count >= 200) { + LOGD("large volume of '%s' messages", qs.name.c_str()); + break; + } + } + } + } + + LOGW("closing logger"); + logger_close(&s.logger, &do_exit); + + if (do_exit.power_failure) { + LOGE("power failure"); + sync(); + LOGE("sync done"); + } + + // messaging cleanup + for (auto &[sock, qs] : qlog_states) delete sock; +} + +int main(int argc, char** argv) { + if (!Hardware::PC()) { + int ret; + ret = util::set_core_affinity({0, 1, 2, 3}); + assert(ret == 0); + // TODO: why does this impact camerad timings? + //ret = util::set_realtime_priority(1); + //assert(ret == 0); + } + + loggerd_thread(); + + return 0; +} diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h new file mode 100644 index 00000000000000..6eafbe08d0aa67 --- /dev/null +++ b/selfdrive/loggerd/loggerd.h @@ -0,0 +1,102 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "cereal/services.h" +#include "cereal/visionipc/visionipc.h" +#include "cereal/visionipc/visionipc_client.h" +#include "system/camerad/cameras/camera_common.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "system/hardware/hw.h" + +#include "selfdrive/loggerd/encoder/encoder.h" +#include "selfdrive/loggerd/logger.h" +#ifdef QCOM2 +#include "selfdrive/loggerd/encoder/v4l_encoder.h" +#define Encoder V4LEncoder +#else +#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h" +#define Encoder FfmpegEncoder +#endif + +constexpr int MAIN_FPS = 20; +const int MAIN_BITRATE = 10000000; +const int DCAM_BITRATE = MAIN_BITRATE; + +#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead + +const bool LOGGERD_TEST = getenv("LOGGERD_TEST"); +const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60; + +struct LogCameraInfo { + CameraType type; + const char *filename; + VisionStreamType stream_type; + int frame_width, frame_height; + int fps; + int bitrate; + bool is_h265; + bool has_qcamera; + bool record; +}; + +const LogCameraInfo cameras_logged[] = { + { + .type = RoadCam, + .stream_type = VISION_STREAM_ROAD, + .filename = "fcamera.hevc", + .fps = MAIN_FPS, + .bitrate = MAIN_BITRATE, + .is_h265 = true, + .has_qcamera = true, + .record = true, + .frame_width = 1928, + .frame_height = 1208, + }, + { + .type = DriverCam, + .stream_type = VISION_STREAM_DRIVER, + .filename = "dcamera.hevc", + .fps = MAIN_FPS, + .bitrate = DCAM_BITRATE, + .is_h265 = true, + .has_qcamera = false, + .record = Params().getBool("RecordFront"), + .frame_width = 1928, + .frame_height = 1208, + }, + { + .type = WideRoadCam, + .stream_type = VISION_STREAM_WIDE_ROAD, + .filename = "ecamera.hevc", + .fps = MAIN_FPS, + .bitrate = MAIN_BITRATE, + .is_h265 = true, + .has_qcamera = false, + .record = true, + .frame_width = 1928, + .frame_height = 1208, + }, +}; +const LogCameraInfo qcam_info = { + .filename = "qcamera.ts", + .fps = MAIN_FPS, + .bitrate = 256000, + .is_h265 = false, + .record = true, + .frame_width = 526, + .frame_height = 330, +}; diff --git a/docs/glossary.toml b/selfdrive/loggerd/tests/__init__.py similarity index 100% rename from docs/glossary.toml rename to selfdrive/loggerd/tests/__init__.py diff --git a/selfdrive/loggerd/tests/fill_eon.py b/selfdrive/loggerd/tests/fill_eon.py new file mode 100755 index 00000000000000..b40982fa9f2dea --- /dev/null +++ b/selfdrive/loggerd/tests/fill_eon.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +"""Script to fill up EON with fake data""" + +import os + +from selfdrive.loggerd.config import ROOT, get_available_percent +from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file + + +if __name__ == "__main__": + segment_idx = 0 + while True: + seg_name = "1970-01-01--00-00-00--%d" % segment_idx + seg_path = os.path.join(ROOT, seg_name) + + print(seg_path) + + create_random_file(os.path.join(seg_path, 'fcamera.hevc'), 36) + create_random_file(os.path.join(seg_path, 'rlog.bz2'), 2) + + segment_idx += 1 + + # Fill up to 99 percent + available_percent = get_available_percent() + if available_percent < 1.0: + break diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py new file mode 100644 index 00000000000000..80cfb162f1f5a8 --- /dev/null +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -0,0 +1,102 @@ +import os +import errno +import shutil +import random +import tempfile +import unittest + +import selfdrive.loggerd.uploader as uploader + +def create_random_file(file_path, size_mb, lock=False): + try: + os.mkdir(os.path.dirname(file_path)) + except OSError: + pass + + lock_path = file_path + ".lock" + if lock: + os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL)) + + chunks = 128 + chunk_bytes = int(size_mb * 1024 * 1024 / chunks) + data = os.urandom(chunk_bytes) + + with open(file_path, 'wb') as f: + for _ in range(chunks): + f.write(data) + +class MockResponse(): + def __init__(self, text, status_code): + self.text = text + self.status_code = status_code + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}', 200) + + def get_token(self): + return "fake-token" + +class MockApiIgnore(): + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('', 412) + + def get_token(self): + return "fake-token" + +class MockParams(): + def __init__(self): + self.params = { + "DongleId": b"0000000000000000", + "IsOffroad": b"1", + } + + def get(self, k, block=False, encoding=None): + val = self.params[k] + + if encoding is not None: + return val.decode(encoding) + else: + return val + + def get_bool(self, k): + val = self.params[k] + return (val == b'1') + +class UploaderTestCase(unittest.TestCase): + f_type = "UNKNOWN" + + def set_ignore(self): + uploader.Api = MockApiIgnore + + def setUp(self): + self.root = tempfile.mkdtemp() + uploader.ROOT = self.root # Monkey patch root dir + uploader.Api = MockApi + uploader.Params = MockParams + uploader.fake_upload = True + uploader.force_wifi = True + uploader.allow_sleep = False + self.seg_num = random.randint(1, 300) + self.seg_format = "2019-04-18--12-52-54--{}" + self.seg_format2 = "2019-05-18--11-22-33--{}" + self.seg_dir = self.seg_format.format(self.seg_num) + + def tearDown(self): + try: + shutil.rmtree(self.root) + except OSError as e: + if e.errno != errno.ENOENT: + raise + + def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False): + file_path = os.path.join(self.root, f_dir, fn) + create_random_file(file_path, size_mb, lock) + + return file_path diff --git a/selfdrive/loggerd/tests/test_deleter.py b/selfdrive/loggerd/tests/test_deleter.py new file mode 100755 index 00000000000000..80fb5c997f575e --- /dev/null +++ b/selfdrive/loggerd/tests/test_deleter.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import os +import time +import threading +import unittest +from collections import namedtuple + +from common.timeout import Timeout, TimeoutException +import selfdrive.loggerd.deleter as deleter +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase + +Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize']) + + +class TestDeleter(UploaderTestCase): + def fake_statvfs(self, d): + return self.fake_stats + + def setUp(self): + self.f_type = "fcamera.hevc" + super().setUp() + self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096) + deleter.os.statvfs = self.fake_statvfs + deleter.ROOT = self.root + + def start_thread(self): + self.end_event = threading.Event() + self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event]) + self.del_thread.daemon = True + self.del_thread.start() + + def join_thread(self): + self.end_event.set() + self.del_thread.join() + + def test_delete(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + self.join_thread() + + self.assertFalse(os.path.exists(f_path), "File not deleted") + + def test_delete_files_in_create_order(self): + f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type) + time.sleep(1) + self.seg_num += 1 + self.seg_dir = self.seg_format.format(self.seg_num) + f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path_1) and os.path.exists(f_path_2): + time.sleep(0.01) + + self.join_thread() + + self.assertFalse(os.path.exists(f_path_1), "Older file not deleted") + + self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file") + + def test_no_delete_when_available_space(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type) + + block_size = 4096 + available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free + self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted with available space") + + def test_no_delete_with_lock_file(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted when locked") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/tests/test_encoder.py b/selfdrive/loggerd/tests/test_encoder.py new file mode 100755 index 00000000000000..1b9bcef2d7c2ef --- /dev/null +++ b/selfdrive/loggerd/tests/test_encoder.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import math +import os +import random +import shutil +import subprocess +import time +import unittest +from pathlib import Path + +from parameterized import parameterized +from tqdm import trange + +from common.params import Params +from common.timeout import Timeout +from system.hardware import TICI +from selfdrive.loggerd.config import ROOT +from selfdrive.manager.process_config import managed_processes +from tools.lib.logreader import LogReader + +SEGMENT_LENGTH = 2 +FULL_SIZE = 2507572 +CAMERAS = [ + ("fcamera.hevc", 20, FULL_SIZE, "roadEncodeIdx"), + ("dcamera.hevc", 20, FULL_SIZE, "driverEncodeIdx"), + ("ecamera.hevc", 20, FULL_SIZE, "wideRoadEncodeIdx"), + ("qcamera.ts", 20, 130000, None), +] + +# we check frame count, so we don't have to be too strict on size +FILE_SIZE_TOLERANCE = 0.5 + + +class TestEncoder(unittest.TestCase): + + # TODO: all of loggerd should work on PC + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + def setUp(self): + self._clear_logs() + os.environ["LOGGERD_TEST"] = "1" + os.environ["LOGGERD_SEGMENT_LENGTH"] = str(SEGMENT_LENGTH) + + def tearDown(self): + self._clear_logs() + + def _clear_logs(self): + if os.path.exists(ROOT): + shutil.rmtree(ROOT) + + def _get_latest_segment_path(self): + last_route = sorted(Path(ROOT).iterdir())[-1] + return os.path.join(ROOT, last_route) + + # TODO: this should run faster than real time + @parameterized.expand([(True, ), (False, )]) + def test_log_rotation(self, record_front): + Params().put_bool("RecordFront", record_front) + + managed_processes['sensord'].start() + managed_processes['loggerd'].start() + managed_processes['encoderd'].start() + + time.sleep(1.0) + managed_processes['camerad'].start() + + num_segments = int(os.getenv("SEGMENTS", random.randint(10, 15))) + + # wait for loggerd to make the dir for first segment + route_prefix_path = None + with Timeout(int(SEGMENT_LENGTH*3)): + while route_prefix_path is None: + try: + route_prefix_path = self._get_latest_segment_path().rsplit("--", 1)[0] + except Exception: + time.sleep(0.1) + + def check_seg(i): + # check each camera file size + counts = [] + first_frames = [] + for camera, fps, size, encode_idx_name in CAMERAS: + if not record_front and "dcamera" in camera: + continue + + file_path = f"{route_prefix_path}--{i}/{camera}" + + # check file exists + self.assertTrue(os.path.exists(file_path), f"segment #{i}: '{file_path}' missing") + + # TODO: this ffprobe call is really slow + # check frame count + cmd = f"ffprobe -v error -select_streams v:0 -count_packets -show_entries stream=nb_read_packets -of csv=p=0 {file_path}" + if TICI: + cmd = "LD_LIBRARY_PATH=/usr/local/lib " + cmd + + expected_frames = fps * SEGMENT_LENGTH + probe = subprocess.check_output(cmd, shell=True, encoding='utf8') + frame_count = int(probe.split('\n')[0].strip()) + counts.append(frame_count) + + self.assertEqual(frame_count, expected_frames, + f"segment #{i}: {camera} failed frame count check: expected {expected_frames}, got {frame_count}") + + # sanity check file size + file_size = os.path.getsize(file_path) + self.assertTrue(math.isclose(file_size, size, rel_tol=FILE_SIZE_TOLERANCE), + f"{file_path} size {file_size} isn't close to target size {size}") + + # Check encodeIdx + if encode_idx_name is not None: + rlog_path = f"{route_prefix_path}--{i}/rlog" + msgs = [m for m in LogReader(rlog_path) if m.which() == encode_idx_name] + encode_msgs = [getattr(m, encode_idx_name) for m in msgs] + + valid = [m.valid for m in msgs] + segment_idxs = [m.segmentId for m in encode_msgs] + encode_idxs = [m.encodeId for m in encode_msgs] + frame_idxs = [m.frameId for m in encode_msgs] + + # Check frame count + self.assertEqual(frame_count, len(segment_idxs)) + self.assertEqual(frame_count, len(encode_idxs)) + + # Check for duplicates or skips + self.assertEqual(0, segment_idxs[0]) + self.assertEqual(len(set(segment_idxs)), len(segment_idxs)) + + self.assertTrue(all(valid)) + + self.assertEqual(expected_frames * i, encode_idxs[0]) + first_frames.append(frame_idxs[0]) + self.assertEqual(len(set(encode_idxs)), len(encode_idxs)) + + self.assertEqual(1, len(set(first_frames))) + + if TICI: + expected_frames = fps * SEGMENT_LENGTH + self.assertEqual(min(counts), expected_frames) + shutil.rmtree(f"{route_prefix_path}--{i}") + + try: + for i in trange(num_segments): + # poll for next segment + with Timeout(int(SEGMENT_LENGTH*10), error_msg=f"timed out waiting for segment {i}"): + while Path(f"{route_prefix_path}--{i+1}") not in Path(ROOT).iterdir(): + time.sleep(0.1) + check_seg(i) + finally: + managed_processes['loggerd'].stop() + managed_processes['encoderd'].stop() + managed_processes['camerad'].stop() + managed_processes['sensord'].stop() + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/tests/test_logger.cc b/selfdrive/loggerd/tests/test_logger.cc new file mode 100644 index 00000000000000..18a0e57df75da9 --- /dev/null +++ b/selfdrive/loggerd/tests/test_logger.cc @@ -0,0 +1,146 @@ +#include + +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "cereal/messaging/messaging.h" +#include "common/util.h" +#include "selfdrive/loggerd/logger.h" +#include "tools/replay/util.h" + +typedef cereal::Sentinel::SentinelType SentinelType; + +void verify_segment(const std::string &route_path, int segment, int max_segment, int required_event_cnt) { + const std::string segment_path = route_path + "--" + std::to_string(segment); + SentinelType begin_sentinel = segment == 0 ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT; + SentinelType end_sentinel = segment == max_segment - 1 ? SentinelType::END_OF_ROUTE : SentinelType::END_OF_SEGMENT; + + REQUIRE(!util::file_exists(segment_path + "/rlog.lock")); + for (const char *fn : {"/rlog", "/qlog"}) { + const std::string log_file = segment_path + fn; + std::string log = util::read_file(log_file); + REQUIRE(!log.empty()); + int event_cnt = 0, i = 0; + kj::ArrayPtr words((capnp::word *)log.data(), log.size() / sizeof(capnp::word)); + while (words.size() > 0) { + try { + capnp::FlatArrayMessageReader reader(words); + auto event = reader.getRoot(); + words = kj::arrayPtr(reader.getEnd(), words.end()); + if (i == 0) { + REQUIRE(event.which() == cereal::Event::INIT_DATA); + } else if (i == 1) { + REQUIRE(event.which() == cereal::Event::SENTINEL); + REQUIRE(event.getSentinel().getType() == begin_sentinel); + REQUIRE(event.getSentinel().getSignal() == 0); + } else if (words.size() > 0) { + REQUIRE(event.which() == cereal::Event::CLOCKS); + ++event_cnt; + } else { + // the last event must be SENTINEL + REQUIRE(event.which() == cereal::Event::SENTINEL); + REQUIRE(event.getSentinel().getType() == end_sentinel); + REQUIRE(event.getSentinel().getSignal() == (end_sentinel == SentinelType::END_OF_ROUTE ? 1 : 0)); + } + ++i; + } catch (const kj::Exception &ex) { + INFO("failed parse " << i << " exception :" << ex.getDescription()); + REQUIRE(0); + break; + } + } + REQUIRE(event_cnt == required_event_cnt); + } +} + +void write_msg(LoggerHandle *logger) { + MessageBuilder msg; + msg.initEvent().initClocks(); + auto bytes = msg.toBytes(); + lh_log(logger, bytes.begin(), bytes.size(), true); +} + +TEST_CASE("logger") { + const std::string log_root = "/tmp/test_logger"; + system(("rm " + log_root + " -rf").c_str()); + + ExitHandler do_exit; + + LoggerState logger = {}; + logger_init(&logger, true); + char segment_path[PATH_MAX] = {}; + int segment = -1; + + SECTION("single thread logging & rotation(100 segments, one thread)") { + const int segment_cnt = 100; + for (int i = 0; i < segment_cnt; ++i) { + REQUIRE(logger_next(&logger, log_root.c_str(), segment_path, sizeof(segment_path), &segment) == 0); + REQUIRE(util::file_exists(std::string(segment_path) + "/rlog.lock")); + REQUIRE(segment == i); + write_msg(logger.cur_handle); + } + do_exit = true; + do_exit.signal = 1; + logger_close(&logger, &do_exit); + for (int i = 0; i < segment_cnt; ++i) { + verify_segment(log_root + "/" + logger.route_name, i, segment_cnt, 1); + } + } + SECTION("multiple threads logging & rotation(100 segments, 10 threads") { + const int segment_cnt = 100, thread_cnt = 10; + std::atomic event_cnt[segment_cnt] = {}; + std::atomic main_segment = -1; + + auto logging_thread = [&]() -> void { + LoggerHandle *lh = logger_get_handle(&logger); + REQUIRE(lh != nullptr); + int segment = main_segment; + int delayed_cnt = 0; + while (!do_exit) { + // write 2 more messages in the current segment and then rotate to the new segment. + if (main_segment > segment && ++delayed_cnt == 2) { + lh_close(lh); + lh = logger_get_handle(&logger); + segment = main_segment; + delayed_cnt = 0; + } + write_msg(lh); + event_cnt[segment] += 1; + usleep(1); + } + lh_close(lh); + }; + + // start logging + std::vector threads; + for (int i = 0; i < segment_cnt; ++i) { + REQUIRE(logger_next(&logger, log_root.c_str(), segment_path, sizeof(segment_path), &segment) == 0); + REQUIRE(segment == i); + main_segment = segment; + if (i == 0) { + for (int j = 0; j < thread_cnt; ++j) { + threads.push_back(std::thread(logging_thread)); + } + } + for (int j = 0; j < 100; ++j) { + write_msg(logger.cur_handle); + usleep(1); + } + event_cnt[segment] += 100; + } + + // end logging + for (auto &t : threads) t.join(); + do_exit = true; + do_exit.signal = 1; + logger_close(&logger, &do_exit); + REQUIRE(logger.cur_handle->refcnt == 0); + + for (int i = 0; i < segment_cnt; ++i) { + verify_segment(log_root + "/" + logger.route_name, i, segment_cnt, event_cnt[i]); + } + } +} diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py new file mode 100755 index 00000000000000..b0907c54af0934 --- /dev/null +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python3 +import numpy as np +import os +import random +import string +import subprocess +import time +import unittest +from collections import defaultdict +from pathlib import Path + +import cereal.messaging as messaging +from cereal import log +from cereal.services import service_list +from common.basedir import BASEDIR +from common.params import Params +from common.timeout import Timeout +from selfdrive.loggerd.config import ROOT +from selfdrive.manager.process_config import managed_processes +from system.version import get_version +from tools.lib.logreader import LogReader +from cereal.visionipc import VisionIpcServer, VisionStreamType +from common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size + +SentinelType = log.Sentinel.SentinelType + +CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in service_list + and service_list[f].should_log and "encode" not in f.lower()] + + +class TestLoggerd(unittest.TestCase): + def _get_latest_log_dir(self): + log_dirs = sorted(Path(ROOT).iterdir(), key=lambda f: f.stat().st_mtime) + return log_dirs[-1] + + def _get_log_dir(self, x): + for l in x.splitlines(): + for p in l.split(' '): + path = Path(p.strip()) + if path.is_dir(): + return path + return None + + def _get_log_fn(self, x): + for l in x.splitlines(): + for p in l.split(' '): + path = Path(p.strip()) + if path.is_file(): + return path + return None + + def _gen_bootlog(self): + with Timeout(5): + out = subprocess.check_output("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"), encoding='utf-8') + + log_fn = self._get_log_fn(out) + + # check existence + assert log_fn is not None + + return log_fn + + def _check_init_data(self, msgs): + msg = msgs[0] + self.assertEqual(msg.which(), 'initData') + + def _check_sentinel(self, msgs, route): + start_type = SentinelType.startOfRoute if route else SentinelType.startOfSegment + self.assertTrue(msgs[1].sentinel.type == start_type) + + end_type = SentinelType.endOfRoute if route else SentinelType.endOfSegment + self.assertTrue(msgs[-1].sentinel.type == end_type) + + def test_init_data_values(self): + os.environ["CLEAN"] = random.choice(["0", "1"]) + + dongle = ''.join(random.choice(string.printable) for n in range(random.randint(1, 100))) + fake_params = [ + # param, initData field, value + ("DongleId", "dongleId", dongle), + ("GitCommit", "gitCommit", "commit"), + ("GitBranch", "gitBranch", "branch"), + ("GitRemote", "gitRemote", "remote"), + ] + params = Params() + for k, _, v in fake_params: + params.put(k, v) + + lr = list(LogReader(str(self._gen_bootlog()))) + initData = lr[0].initData + + self.assertTrue(initData.dirty != bool(os.environ["CLEAN"])) + self.assertEqual(initData.version, get_version()) + + if os.path.isfile("/proc/cmdline"): + with open("/proc/cmdline") as f: + self.assertEqual(list(initData.kernelArgs), f.read().strip().split(" ")) + + with open("/proc/version") as f: + self.assertEqual(initData.kernelVersion, f.read()) + + for _, k, v in fake_params: + self.assertEqual(getattr(initData, k), v) + + def test_rotation(self): + os.environ["LOGGERD_TEST"] = "1" + Params().put("RecordFront", "1") + + expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"), + (VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] + + pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) + vipc_server = VisionIpcServer("camerad") + for stream_type, frame_spec, _ in streams: + vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) + vipc_server.start_listener() + + for _ in range(5): + num_segs = random.randint(2, 5) + length = random.randint(1, 3) + os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length) + managed_processes["loggerd"].start() + managed_processes["encoderd"].start() + + fps = 20.0 + for n in range(1, int(num_segs*length*fps)+1): + for stream_type, frame_spec, state in streams: + dat = np.empty(frame_spec[2], dtype=np.uint8) + vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) + + camera_state = messaging.new_message(state) + frame = getattr(camera_state, state) + frame.frameId = n + pm.send(state, camera_state) + time.sleep(1.0/fps) + + managed_processes["loggerd"].stop() + managed_processes["encoderd"].stop() + + route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0] + for n in range(num_segs): + p = Path(f"{route_path}--{n}") + logged = {f.name for f in p.iterdir() if f.is_file()} + diff = logged ^ expected_files + self.assertEqual(len(diff), 0, f"didn't get all expected files. run={_} seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}") + + def test_bootlog(self): + # generate bootlog with fake launch log + launch_log = ''.join(str(random.choice(string.printable)) for _ in range(100)) + with open("/tmp/launch_log", "w") as f: + f.write(launch_log) + + bootlog_path = self._gen_bootlog() + lr = list(LogReader(str(bootlog_path))) + + # check length + assert len(lr) == 2 # boot + initData + + self._check_init_data(lr) + + # check msgs + bootlog_msgs = [m for m in lr if m.which() == 'boot'] + assert len(bootlog_msgs) == 1 + + # sanity check values + boot = bootlog_msgs.pop().boot + assert abs(boot.wallTimeNanos - time.time_ns()) < 5*1e9 # within 5s + assert boot.launchLog == launch_log + + for fn in ["console-ramoops", "pmsg-ramoops-0"]: + path = Path(os.path.join("/sys/fs/pstore/", fn)) + if path.is_file(): + expected_val = open(path, "rb").read() + bootlog_val = [e.value for e in boot.pstore.entries if e.key == fn][0] + self.assertEqual(expected_val, bootlog_val) + + def test_qlog(self): + qlog_services = [s for s in CEREAL_SERVICES if service_list[s].decimation is not None] + no_qlog_services = [s for s in CEREAL_SERVICES if service_list[s].decimation is None] + + services = random.sample(qlog_services, random.randint(2, min(10, len(qlog_services)))) + \ + random.sample(no_qlog_services, random.randint(2, min(10, len(no_qlog_services)))) + + pm = messaging.PubMaster(services) + + # sleep enough for the first poll to time out + # TODO: fix loggerd bug dropping the msgs from the first poll + managed_processes["loggerd"].start() + for s in services: + while not pm.all_readers_updated(s): + time.sleep(0.1) + + sent_msgs = defaultdict(list) + for _ in range(random.randint(2, 10) * 100): + for s in services: + try: + m = messaging.new_message(s) + except Exception: + m = messaging.new_message(s, random.randint(2, 10)) + pm.send(s, m) + sent_msgs[s].append(m) + time.sleep(0.01) + + time.sleep(1) + managed_processes["loggerd"].stop() + + qlog_path = os.path.join(self._get_latest_log_dir(), "qlog") + lr = list(LogReader(qlog_path)) + + # check initData and sentinel + self._check_init_data(lr) + self._check_sentinel(lr, True) + + recv_msgs = defaultdict(list) + for m in lr: + recv_msgs[m.which()].append(m) + + for s, msgs in sent_msgs.items(): + recv_cnt = len(recv_msgs[s]) + + if s in no_qlog_services: + # check services with no specific decimation aren't in qlog + self.assertEqual(recv_cnt, 0, f"got {recv_cnt} {s} msgs in qlog") + else: + # check logged message count matches decimation + expected_cnt = (len(msgs) - 1) // service_list[s].decimation + 1 + self.assertEqual(recv_cnt, expected_cnt, f"expected {expected_cnt} msgs for {s}, got {recv_cnt}") + + def test_rlog(self): + services = random.sample(CEREAL_SERVICES, random.randint(5, 10)) + pm = messaging.PubMaster(services) + + # sleep enough for the first poll to time out + # TODO: fix loggerd bug dropping the msgs from the first poll + managed_processes["loggerd"].start() + for s in services: + while not pm.all_readers_updated(s): + time.sleep(0.1) + + sent_msgs = defaultdict(list) + for _ in range(random.randint(2, 10) * 100): + for s in services: + try: + m = messaging.new_message(s) + except Exception: + m = messaging.new_message(s, random.randint(2, 10)) + pm.send(s, m) + sent_msgs[s].append(m) + + time.sleep(2) + managed_processes["loggerd"].stop() + + lr = list(LogReader(os.path.join(self._get_latest_log_dir(), "rlog"))) + + # check initData and sentinel + self._check_init_data(lr) + self._check_sentinel(lr, True) + + # check all messages were logged and in order + lr = lr[2:-1] # slice off initData and both sentinels + for m in lr: + sent = sent_msgs[m.which()].pop(0) + sent.clear_write_flag() + self.assertEqual(sent.to_bytes(), m.as_builder().to_bytes()) + + +if __name__ == "__main__": + unittest.main() diff --git a/common/tests/test_runner.cc b/selfdrive/loggerd/tests/test_runner.cc similarity index 100% rename from common/tests/test_runner.cc rename to selfdrive/loggerd/tests/test_runner.cc diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py new file mode 100755 index 00000000000000..6090bbe2aae6d1 --- /dev/null +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +import os +import time +import threading +import unittest +import logging +import json + +from system.swaglog import cloudlog +import selfdrive.loggerd.uploader as uploader + +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase + + +class TestLogHandler(logging.Handler): + def __init__(self): + logging.Handler.__init__(self) + self.reset() + + def reset(self): + self.upload_order = list() + self.upload_ignored = list() + + def emit(self, record): + try: + j = json.loads(record.getMessage()) + if j["event"] == "upload_success": + self.upload_order.append(j["key"]) + if j["event"] == "upload_ignored": + self.upload_ignored.append(j["key"]) + except Exception: + pass + +log_handler = TestLogHandler() +cloudlog.addHandler(log_handler) + + +class TestUploader(UploaderTestCase): + def setUp(self): + super().setUp() + log_handler.reset() + + def start_thread(self): + self.end_event = threading.Event() + self.up_thread = threading.Thread(target=uploader.uploader_fn, args=[self.end_event]) + self.up_thread.daemon = True + self.up_thread.start() + + def join_thread(self): + self.end_event.set() + self.up_thread.join() + + def gen_files(self, lock=False, boot=True): + f_paths = list() + for t in ["qlog", "rlog", "dcamera.hevc", "fcamera.hevc"]: + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock)) + + if boot: + f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock)) + return f_paths + + def gen_order(self, seg1, seg2, boot=True): + keys = [] + if boot: + keys += [f"boot/{self.seg_format.format(i)}.bz2" for i in seg1] + keys += [f"boot/{self.seg_format2.format(i)}.bz2" for i in seg2] + keys += [f"{self.seg_format.format(i)}/qlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/qlog.bz2" for i in seg2] + return keys + + def test_upload(self): + self.gen_files(lock=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + self.assertTrue(len(log_handler.upload_ignored) == 0, "Some files were ignored") + self.assertFalse(len(log_handler.upload_order) < len(exp_order), "Some files failed to upload") + self.assertFalse(len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice") + for f_path in exp_order: + self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not uploaded") + + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_upload_ignored(self): + self.set_ignore() + self.gen_files(lock=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + self.assertTrue(len(log_handler.upload_order) == 0, "Some files were not ignored") + self.assertFalse(len(log_handler.upload_ignored) < len(exp_order), "Some files failed to ignore") + self.assertFalse(len(log_handler.upload_ignored) > len(exp_order), "Some files were ignored twice") + for f_path in exp_order: + self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not ignored") + + self.assertTrue(log_handler.upload_ignored == exp_order, "Files ignored in wrong order") + + def test_upload_files_in_create_order(self): + seg1_nums = [0, 1, 2, 10, 20] + for i in seg1_nums: + self.seg_dir = self.seg_format.format(i) + self.gen_files(boot=False) + seg2_nums = [5, 50, 51] + for i in seg2_nums: + self.seg_dir = self.seg_format2.format(i) + self.gen_files(boot=False) + + exp_order = self.gen_order(seg1_nums, seg2_nums, boot=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + self.assertTrue(len(log_handler.upload_ignored) == 0, "Some files were ignored") + self.assertFalse(len(log_handler.upload_order) < len(exp_order), "Some files failed to upload") + self.assertFalse(len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice") + for f_path in exp_order: + self.assertTrue(os.getxattr(os.path.join(self.root, f_path.replace('.bz2', '')), uploader.UPLOAD_ATTR_NAME), "All files not uploaded") + + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_no_upload_with_lock_file(self): + self.start_thread() + + time.sleep(0.25) + f_paths = self.gen_files(lock=True, boot=False) + + # allow enough time that files should have been uploaded if they would be uploaded + time.sleep(5) + self.join_thread() + + for f_path in f_paths: + uploaded = uploader.UPLOAD_ATTR_NAME in os.listxattr(f_path.replace('.bz2', '')) + self.assertFalse(uploaded, "File upload when locked") + + def test_clear_locks_on_startup(self): + f_paths = self.gen_files(lock=True, boot=False) + self.start_thread() + time.sleep(1) + self.join_thread() + + for f_path in f_paths: + self.assertFalse(os.path.isfile(f_path + ".lock"), "File lock not cleared on startup") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/tools/mark_all_uploaded.py b/selfdrive/loggerd/tools/mark_all_uploaded.py new file mode 100644 index 00000000000000..e60e6cfa2cf7c6 --- /dev/null +++ b/selfdrive/loggerd/tools/mark_all_uploaded.py @@ -0,0 +1,8 @@ +import os +from selfdrive.loggerd.uploader import UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE + +from selfdrive.loggerd.config import ROOT +for folder in os.walk(ROOT): + for file1 in folder[2]: + full_path = os.path.join(folder[0], file1) + os.setxattr(full_path, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) diff --git a/selfdrive/loggerd/tools/mark_unuploaded.py b/selfdrive/loggerd/tools/mark_unuploaded.py new file mode 100755 index 00000000000000..343805d5fc5d39 --- /dev/null +++ b/selfdrive/loggerd/tools/mark_unuploaded.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 +import os +import sys +from selfdrive.loggerd.uploader import UPLOAD_ATTR_NAME + +for fn in sys.argv[1:]: + print(f"unmarking {fn}") + os.removexattr(fn, UPLOAD_ATTR_NAME) diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py new file mode 100644 index 00000000000000..f97bafecb90e75 --- /dev/null +++ b/selfdrive/loggerd/uploader.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python3 +import bz2 +import io +import json +import os +import random +import requests +import threading +import time +import traceback +from pathlib import Path + +from cereal import log +import cereal.messaging as messaging +from common.api import Api +from common.params import Params +from common.realtime import set_core_affinity +from system.hardware import TICI +from selfdrive.loggerd.xattr_cache import getxattr, setxattr +from selfdrive.loggerd.config import ROOT +from system.swaglog import cloudlog + +NetworkType = log.DeviceState.NetworkType +UPLOAD_ATTR_NAME = 'user.upload' +UPLOAD_ATTR_VALUE = b'1' + +UPLOAD_QLOG_QCAM_MAX_SIZE = 100 * 1e6 # MB + +allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1")) +force_wifi = os.getenv("FORCEWIFI") is not None +fake_upload = os.getenv("FAKEUPLOAD") is not None + + +def get_directory_sort(d): + return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1))) + +def listdir_by_creation(d): + try: + paths = os.listdir(d) + paths = sorted(paths, key=get_directory_sort) + return paths + except OSError: + cloudlog.exception("listdir_by_creation failed") + return list() + +def clear_locks(root): + for logname in os.listdir(root): + path = os.path.join(root, logname) + try: + for fname in os.listdir(path): + if fname.endswith(".lock"): + os.unlink(os.path.join(path, fname)) + except OSError: + cloudlog.exception("clear_locks failed") + + +class Uploader(): + def __init__(self, dongle_id, root): + self.dongle_id = dongle_id + self.api = Api(dongle_id) + self.root = root + + self.upload_thread = None + + self.last_resp = None + self.last_exc = None + + self.immediate_size = 0 + self.immediate_count = 0 + + # stats for last successfully uploaded file + self.last_time = 0.0 + self.last_speed = 0.0 + self.last_filename = "" + + self.immediate_folders = ["crash/", "boot/"] + self.immediate_priority = {"qlog": 0, "qlog.bz2": 0, "qcamera.ts": 1} + + def get_upload_sort(self, name): + if name in self.immediate_priority: + return self.immediate_priority[name] + return 1000 + + def list_upload_files(self): + if not os.path.isdir(self.root): + return + + self.immediate_size = 0 + self.immediate_count = 0 + + for logname in listdir_by_creation(self.root): + path = os.path.join(self.root, logname) + try: + names = os.listdir(path) + except OSError: + continue + + if any(name.endswith(".lock") for name in names): + continue + + for name in sorted(names, key=self.get_upload_sort): + key = os.path.join(logname, name) + fn = os.path.join(path, name) + # skip files already uploaded + try: + is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) + except OSError: + cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn) + is_uploaded = True # deleter could have deleted + if is_uploaded: + continue + + try: + if name in self.immediate_priority: + self.immediate_count += 1 + self.immediate_size += os.path.getsize(fn) + except OSError: + pass + + yield (name, key, fn) + + def next_file_to_upload(self): + upload_files = list(self.list_upload_files()) + + for name, key, fn in upload_files: + if any(f in fn for f in self.immediate_folders): + return (name, key, fn) + + for name, key, fn in upload_files: + if name in self.immediate_priority: + return (name, key, fn) + + return None + + def do_upload(self, key, fn): + try: + url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) + if url_resp.status_code == 412: + self.last_resp = url_resp + return + + url_resp_json = json.loads(url_resp.text) + url = url_resp_json['url'] + headers = url_resp_json['headers'] + cloudlog.debug("upload_url v1.4 %s %s", url, str(headers)) + + if fake_upload: + cloudlog.debug(f"*** WARNING, THIS IS A FAKE UPLOAD TO {url} ***") + + class FakeResponse(): + def __init__(self): + self.status_code = 200 + + self.last_resp = FakeResponse() + else: + with open(fn, "rb") as f: + if key.endswith('.bz2') and not fn.endswith('.bz2'): + data = bz2.compress(f.read()) + data = io.BytesIO(data) + else: + data = f + + self.last_resp = requests.put(url, data=data, headers=headers, timeout=10) + except Exception as e: + self.last_exc = (e, traceback.format_exc()) + raise + + def normal_upload(self, key, fn): + self.last_resp = None + self.last_exc = None + + try: + self.do_upload(key, fn) + except Exception: + pass + + return self.last_resp + + def upload(self, name, key, fn, network_type, metered): + try: + sz = os.path.getsize(fn) + except OSError: + cloudlog.exception("upload: getsize failed") + return False + + cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + + if sz == 0: + # tag files of 0 size as uploaded + success = True + elif name in self.immediate_priority and sz > UPLOAD_QLOG_QCAM_MAX_SIZE: + cloudlog.event("uploader_too_large", key=key, fn=fn, sz=sz) + success = True + else: + start_time = time.monotonic() + stat = self.normal_upload(key, fn) + if stat is not None and stat.status_code in (200, 201, 401, 403, 412): + self.last_filename = fn + self.last_time = time.monotonic() - start_time + self.last_speed = (sz / 1e6) / self.last_time + success = True + cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + else: + success = False + cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) + + if success: + # tag file as uploaded + try: + setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) + except OSError: + cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) + + return success + + def get_msg(self): + msg = messaging.new_message("uploaderState") + us = msg.uploaderState + us.immediateQueueSize = int(self.immediate_size / 1e6) + us.immediateQueueCount = self.immediate_count + us.lastTime = self.last_time + us.lastSpeed = self.last_speed + us.lastFilename = self.last_filename + return msg + + +def uploader_fn(exit_event): + try: + set_core_affinity([0, 1, 2, 3]) + except Exception: + cloudlog.exception("failed to set core affinity") + + clear_locks(ROOT) + + params = Params() + dongle_id = params.get("DongleId", encoding='utf8') + + if dongle_id is None: + cloudlog.info("uploader missing dongle_id") + raise Exception("uploader can't start without dongle id") + + if TICI and not Path("/data/media").is_mount(): + cloudlog.warning("NVME not mounted") + + sm = messaging.SubMaster(['deviceState']) + pm = messaging.PubMaster(['uploaderState']) + uploader = Uploader(dongle_id, ROOT) + + backoff = 0.1 + while not exit_event.is_set(): + sm.update(0) + offroad = params.get_bool("IsOffroad") + network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi + if network_type == NetworkType.none: + if allow_sleep: + time.sleep(60 if offroad else 5) + continue + + d = uploader.next_file_to_upload() + if d is None: # Nothing to upload + if allow_sleep: + time.sleep(60 if offroad else 5) + continue + + name, key, fn = d + + # qlogs and bootlogs need to be compressed before uploading + if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.bz2')): + key += ".bz2" + + success = uploader.upload(name, key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered) + if success: + backoff = 0.1 + elif allow_sleep: + cloudlog.info("upload backoff %r", backoff) + time.sleep(backoff + random.uniform(0, backoff)) + backoff = min(backoff*2, 120) + + pm.send("uploaderState", uploader.get_msg()) + + +def main(): + uploader_fn(threading.Event()) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/loggerd/video_writer.cc b/selfdrive/loggerd/video_writer.cc new file mode 100644 index 00000000000000..4f79ccafc87224 --- /dev/null +++ b/selfdrive/loggerd/video_writer.cc @@ -0,0 +1,114 @@ +#pragma clang diagnostic ignored "-Wdeprecated-declarations" +#include +#include + +#include "selfdrive/loggerd/video_writer.h" +#include "common/swaglog.h" +#include "common/util.h" + +VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec) + : remuxing(remuxing) { + raw = codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS; + vid_path = util::string_format("%s/%s", path, filename); + lock_path = util::string_format("%s/%s.lock", path, filename); + + int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664)); + assert(lock_fd >= 0); + close(lock_fd); + + LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing); + if (this->remuxing) { + avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str()); + assert(this->ofmt_ctx); + + // set codec correctly. needed? + assert(codec != cereal::EncodeIndex::Type::FULL_H_E_V_C); + const AVCodec *avcodec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264); + assert(avcodec); + + this->codec_ctx = avcodec_alloc_context3(avcodec); + assert(this->codec_ctx); + this->codec_ctx->width = width; + this->codec_ctx->height = height; + this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; + this->codec_ctx->time_base = (AVRational){ 1, fps }; + + if (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS) { + // without this, there's just noise + int err = avcodec_open2(this->codec_ctx, avcodec, NULL); + assert(err >= 0); + } + + this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? avcodec : NULL); + assert(this->out_stream); + + int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE); + assert(err >= 0); + + } else { + this->of = util::safe_fopen(this->vid_path.c_str(), "wb"); + assert(this->of); + } +} + +void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe) { + if (of && data) { + size_t written = util::safe_fwrite(data, 1, len, of); + if (written != len) { + LOGE("failed to write file.errno=%d", errno); + } + } + + if (remuxing) { + if (codecconfig) { + if (len > 0) { + codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE); + codec_ctx->extradata_size = len; + memcpy(codec_ctx->extradata, data, len); + } + int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx); + assert(err >= 0); + err = avformat_write_header(ofmt_ctx, NULL); + assert(err >= 0); + } else { + // input timestamps are in microseconds + AVRational in_timebase = {1, 1000000}; + + AVPacket pkt; + av_init_packet(&pkt); + pkt.data = data; + pkt.size = len; + + enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); + pkt.pts = pkt.dts = av_rescale_q_rnd(timestamp, in_timebase, ofmt_ctx->streams[0]->time_base, rnd); + pkt.duration = av_rescale_q(50*1000, in_timebase, ofmt_ctx->streams[0]->time_base); + + if (keyframe) { + pkt.flags |= AV_PKT_FLAG_KEY; + } + + // TODO: can use av_write_frame for non raw? + int err = av_interleaved_write_frame(ofmt_ctx, &pkt); + if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lu", len, timestamp); } + + av_packet_unref(&pkt); + } + } +} + +VideoWriter::~VideoWriter() { + if (this->remuxing) { + if (this->raw) { avcodec_close(this->codec_ctx); } + int err = av_write_trailer(this->ofmt_ctx); + if (err != 0) LOGE("av_write_trailer failed %d", err); + avcodec_free_context(&this->codec_ctx); + err = avio_closep(&this->ofmt_ctx->pb); + if (err != 0) LOGE("avio_closep failed %d", err); + avformat_free_context(this->ofmt_ctx); + } else { + util::safe_fflush(this->of); + fclose(this->of); + this->of = nullptr; + } + unlink(this->lock_path.c_str()); +} diff --git a/selfdrive/loggerd/video_writer.h b/selfdrive/loggerd/video_writer.h new file mode 100644 index 00000000000000..01a243904cf040 --- /dev/null +++ b/selfdrive/loggerd/video_writer.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +extern "C" { +#include +#include +} + +#include "cereal/messaging/messaging.h" + +class VideoWriter { +public: + VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec); + void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe); + ~VideoWriter(); +private: + std::string vid_path, lock_path; + + FILE *of = nullptr; + + AVCodecContext *codec_ctx; + AVFormatContext *ofmt_ctx; + AVStream *out_stream; + bool remuxing, raw; +}; \ No newline at end of file diff --git a/selfdrive/loggerd/xattr_cache.py b/selfdrive/loggerd/xattr_cache.py new file mode 100644 index 00000000000000..95e39f20323fac --- /dev/null +++ b/selfdrive/loggerd/xattr_cache.py @@ -0,0 +1,23 @@ +import os +import errno +from typing import Dict, Tuple, Optional + +_cached_attributes: Dict[Tuple, Optional[bytes]] = {} + +def getxattr(path: str, attr_name: str) -> Optional[bytes]: + key = (path, attr_name) + if key not in _cached_attributes: + try: + response = os.getxattr(path, attr_name) + except OSError as e: + # ENODATA means attribute hasn't been set + if e.errno == errno.ENODATA: + response = None + else: + raise + _cached_attributes[key] = response + return _cached_attributes[key] + +def setxattr(path: str, attr_name: str, attr_value: bytes) -> None: + _cached_attributes.pop((path, attr_name), None) + return os.setxattr(path, attr_name, attr_value) diff --git a/selfdrive/manager/__init__.py b/selfdrive/manager/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py new file mode 100755 index 00000000000000..c8a7d415398b04 --- /dev/null +++ b/selfdrive/manager/build.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +import os +import subprocess +import textwrap +from pathlib import Path + +# NOTE: Do NOT import anything here that needs be built (e.g. params) +from common.basedir import BASEDIR +from common.spinner import Spinner +from common.text_window import TextWindow +from system.hardware import AGNOS +from system.swaglog import cloudlog, add_file_handler +from system.version import is_dirty + +MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 +CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") + +TOTAL_SCONS_NODES = 2395 +MAX_BUILD_PROGRESS = 100 +PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) + + +def build(spinner: Spinner, dirty: bool = False) -> None: + env = os.environ.copy() + env['SCONS_PROGRESS'] = "1" + nproc = os.cpu_count() + j_flag = "" if nproc is None else f"-j{nproc - 1}" + + scons: subprocess.Popen = subprocess.Popen(["scons", j_flag, "--cache-populate"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) + assert scons.stderr is not None + + compile_output = [] + + # Read progress from stderr and update spinner + while scons.poll() is None: + try: + line = scons.stderr.readline() + if line is None: + continue + line = line.rstrip() + + prefix = b'progress: ' + if line.startswith(prefix): + i = int(line[len(prefix):]) + spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.) + elif len(line): + compile_output.append(line) + print(line.decode('utf8', 'replace')) + except Exception: + pass + + if scons.returncode != 0: + # Read remaining output + r = scons.stderr.read().split(b'\n') + compile_output += r + + # Build failed log errors + errors = [line.decode('utf8', 'replace') for line in compile_output + if any(err in line for err in [b'error: ', b'not found, needed by target'])] + error_s = "\n".join(errors) + add_file_handler(cloudlog) + cloudlog.error("scons build failed\n" + error_s) + + # Show TextWindow + spinner.close() + if not os.getenv("CI"): + error_s = "\n \n".join("\n".join(textwrap.wrap(e, 65)) for e in errors) + with TextWindow("openpilot failed to build\n \n" + error_s) as t: + t.wait_for_exit() + exit(1) + + + # enforce max cache size + cache_files = [f for f in CACHE_DIR.rglob('*') if f.is_file()] + cache_files.sort(key=lambda f: f.stat().st_mtime) + cache_size = sum(f.stat().st_size for f in cache_files) + for f in cache_files: + if cache_size < MAX_CACHE_SIZE: + break + cache_size -= f.stat().st_size + f.unlink() + + +if __name__ == "__main__" and not PREBUILT: + spinner = Spinner() + spinner.update_progress(0, 100) + build(spinner, is_dirty()) diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py new file mode 100644 index 00000000000000..983c7cc0b16c56 --- /dev/null +++ b/selfdrive/manager/helpers.py @@ -0,0 +1,38 @@ +import os +import sys +import fcntl +import errno +import signal + + +def unblock_stdout() -> None: + # get a non-blocking stdout + child_pid, child_pty = os.forkpty() + if child_pid != 0: # parent + + # child is in its own process group, manually pass kill signals + signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT)) + signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM)) + + fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) + + while True: + try: + dat = os.read(child_pty, 4096) + except OSError as e: + if e.errno == errno.EIO: + break + continue + + if not dat: + break + + try: + sys.stdout.write(dat.decode('utf8')) + except (OSError, UnicodeDecodeError): + pass + + # os.wait() returns a tuple with the pid and a 16 bit value + # whose low byte is the signal number and whose high byte is the exit status + exit_status = os.wait()[1] >> 8 + os._exit(exit_status) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py new file mode 100755 index 00000000000000..ff2bf4bc89442f --- /dev/null +++ b/selfdrive/manager/manager.py @@ -0,0 +1,223 @@ +#!/usr/bin/env python3 +import datetime +import os +import signal +import subprocess +import sys +import traceback +from typing import List, Tuple, Union + +import cereal.messaging as messaging +import selfdrive.sentry as sentry +from common.basedir import BASEDIR +from common.params import Params, ParamKeyType +from common.text_window import TextWindow +from selfdrive.boardd.set_time import set_time +from system.hardware import HARDWARE, PC +from selfdrive.manager.helpers import unblock_stdout +from selfdrive.manager.process import ensure_running +from selfdrive.manager.process_config import managed_processes +from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID +from system.swaglog import cloudlog, add_file_handler +from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ + terms_version, training_version, is_tested_branch + + +sys.path.append(os.path.join(BASEDIR, "pyextra")) + + +def manager_init() -> None: + # update system time from panda + set_time(cloudlog) + + # save boot log + subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + + params = Params() + params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) + + default_params: List[Tuple[str, Union[str, bytes]]] = [ + ("CompletedTrainingVersion", "0"), + ("DisengageOnAccelerator", "1"), + ("HasAcceptedTerms", "0"), + ("LanguageSetting", "main_en"), + ("OpenpilotEnabledToggle", "1"), + ] + if not PC: + default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) + + if params.get_bool("RecordFrontLock"): + params.put_bool("RecordFront", True) + + # set unset params + for k, v in default_params: + if params.get(k) is None: + params.put(k, v) + + # is this dashcam? + if os.getenv("PASSIVE") is not None: + params.put_bool("Passive", bool(int(os.getenv("PASSIVE", "0")))) + + if params.get("Passive") is None: + raise Exception("Passive must be set to continue") + + # Create folders needed for msgq + try: + os.mkdir("/dev/shm") + except FileExistsError: + pass + except PermissionError: + print("WARNING: failed to make /dev/shm") + + # set version params + params.put("Version", get_version()) + params.put("TermsVersion", terms_version) + params.put("TrainingVersion", training_version) + params.put("GitCommit", get_commit(default="")) + params.put("GitBranch", get_short_branch(default="")) + params.put("GitRemote", get_origin(default="")) + params.put_bool("IsTestedBranch", is_tested_branch()) + + # set dongle id + reg_res = register(show_spinner=True) + if reg_res: + dongle_id = reg_res + else: + serial = params.get("HardwareSerial") + raise Exception(f"Registration failed for device {serial}") + os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog + + if not is_dirty(): + os.environ['CLEAN'] = '1' + + # init logging + sentry.init(sentry.SentryProject.SELFDRIVE) + cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty(), + device=HARDWARE.get_device_type()) + + +def manager_prepare() -> None: + for p in managed_processes.values(): + p.prepare() + + +def manager_cleanup() -> None: + # send signals to kill all procs + for p in managed_processes.values(): + p.stop(block=False) + + # ensure all are killed + for p in managed_processes.values(): + p.stop(block=True) + + cloudlog.info("everything is dead") + + +def manager_thread() -> None: + cloudlog.bind(daemon="manager") + cloudlog.info("manager start") + cloudlog.info({"environ": os.environ}) + + params = Params() + + ignore: List[str] = [] + if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): + ignore += ["manage_athenad", "uploader"] + if os.getenv("NOBOARD") is not None: + ignore.append("pandad") + ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] + + sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState']) + pm = messaging.PubMaster(['managerState']) + + ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) + + while True: + sm.update() + + started = sm['deviceState'].started + ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) + + running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) + for p in managed_processes.values() if p.proc) + print(running) + cloudlog.debug(running) + + # send managerState + msg = messaging.new_message('managerState') + msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] + pm.send('managerState', msg) + + # Exit main loop when uninstall/shutdown/reboot is needed + shutdown = False + for param in ("DoUninstall", "DoShutdown", "DoReboot"): + if params.get_bool(param): + shutdown = True + params.put("LastManagerExitReason", param) + cloudlog.warning(f"Shutting down manager - {param} set") + + if shutdown: + break + + +def main() -> None: + prepare_only = os.getenv("PREPAREONLY") is not None + + manager_init() + + # Start UI early so prepare can happen in the background + if not prepare_only: + managed_processes['ui'].start() + + manager_prepare() + + if prepare_only: + return + + # SystemExit on sigterm + signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) + + try: + manager_thread() + except Exception: + traceback.print_exc() + sentry.capture_exception() + finally: + manager_cleanup() + + params = Params() + if params.get_bool("DoUninstall"): + cloudlog.warning("uninstalling") + HARDWARE.uninstall() + elif params.get_bool("DoReboot"): + cloudlog.warning("reboot") + HARDWARE.reboot() + elif params.get_bool("DoShutdown"): + cloudlog.warning("shutdown") + HARDWARE.shutdown() + + +if __name__ == "__main__": + unblock_stdout() + + try: + main() + except Exception: + add_file_handler(cloudlog) + cloudlog.exception("Manager failed to start") + + try: + managed_processes['ui'].stop() + except Exception: + pass + + # Show last 3 lines of traceback + error = traceback.format_exc(-3) + error = "Manager failed to start\n\n" + error + with TextWindow(error) as t: + t.wait_for_exit() + + raise + + # manual exit because we are forked + sys.exit(0) diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py new file mode 100644 index 00000000000000..dabfbe4ee0b1f6 --- /dev/null +++ b/selfdrive/manager/process.py @@ -0,0 +1,317 @@ +import importlib +import os +import signal +import struct +import time +import subprocess +from typing import Optional, Callable, List, ValuesView +from abc import ABC, abstractmethod +from multiprocessing import Process + +from setproctitle import setproctitle # pylint: disable=no-name-in-module + +import cereal.messaging as messaging +import selfdrive.sentry as sentry +from cereal import car +from common.basedir import BASEDIR +from common.params import Params +from common.realtime import sec_since_boot +from system.swaglog import cloudlog +from system.hardware import HARDWARE +from cereal import log + +WATCHDOG_FN = "/dev/shm/wd_" +ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None + + +def launcher(proc: str, name: str) -> None: + try: + # import the process + mod = importlib.import_module(proc) + + # rename the process + setproctitle(proc) + + # create new context since we forked + messaging.context = messaging.Context() + + # add daemon name tag to logs + cloudlog.bind(daemon=name) + sentry.set_tag("daemon", name) + + # exec the process + getattr(mod, 'main')() + except KeyboardInterrupt: + cloudlog.warning(f"child {proc} got SIGINT") + except Exception: + # can't install the crash handler because sys.excepthook doesn't play nice + # with threads, so catch it here. + sentry.capture_exception() + raise + + +def nativelauncher(pargs: List[str], cwd: str, name: str) -> None: + os.environ['MANAGER_DAEMON'] = name + + # exec the process + os.chdir(cwd) + os.execvp(pargs[0], pargs) + + +def join_process(process: Process, timeout: float) -> None: + # Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382 + # We have to poll the exitcode instead + t = time.monotonic() + while time.monotonic() - t < timeout and process.exitcode is None: + time.sleep(0.001) + + +class ManagerProcess(ABC): + unkillable = False + daemon = False + sigkill = False + onroad = True + offroad = False + callback: Optional[Callable[[bool, Params, car.CarParams], bool]] = None + proc: Optional[Process] = None + enabled = True + name = "" + + last_watchdog_time = 0 + watchdog_max_dt: Optional[int] = None + watchdog_seen = False + shutting_down = False + + @abstractmethod + def prepare(self) -> None: + pass + + @abstractmethod + def start(self) -> None: + pass + + def restart(self) -> None: + self.stop() + self.start() + + def check_watchdog(self, started: bool) -> None: + if self.watchdog_max_dt is None or self.proc is None: + return + + try: + fn = WATCHDOG_FN + str(self.proc.pid) + # TODO: why can't pylint find struct.unpack? + self.last_watchdog_time = struct.unpack('Q', open(fn, "rb").read())[0] # pylint: disable=no-member + except Exception: + pass + + dt = sec_since_boot() - self.last_watchdog_time / 1e9 + + if dt > self.watchdog_max_dt: + # Only restart while offroad for now + if self.watchdog_seen and ENABLE_WATCHDOG: + cloudlog.error(f"Watchdog timeout for {self.name} (exitcode {self.proc.exitcode}) restarting ({started=})") + self.restart() + else: + self.watchdog_seen = True + + def stop(self, retry: bool=True, block: bool=True) -> Optional[int]: + if self.proc is None: + return None + + if self.proc.exitcode is None: + if not self.shutting_down: + cloudlog.info(f"killing {self.name}") + sig = signal.SIGKILL if self.sigkill else signal.SIGINT + self.signal(sig) + self.shutting_down = True + + if not block: + return None + + join_process(self.proc, 5) + + # If process failed to die send SIGKILL or reboot + if self.proc.exitcode is None and retry: + if self.unkillable: + cloudlog.critical(f"unkillable process {self.name} failed to exit! rebooting in 15 if it doesn't die") + join_process(self.proc, 15) + + if self.proc.exitcode is None: + cloudlog.critical(f"unkillable process {self.name} failed to die!") + os.system("date >> /data/unkillable_reboot") + os.sync() + HARDWARE.reboot() + raise RuntimeError + else: + cloudlog.info(f"killing {self.name} with SIGKILL") + self.signal(signal.SIGKILL) + self.proc.join() + + ret = self.proc.exitcode + cloudlog.info(f"{self.name} is dead with {ret}") + + if self.proc.exitcode is not None: + self.shutting_down = False + self.proc = None + + return ret + + def signal(self, sig: int) -> None: + if self.proc is None: + return + + # Don't signal if already exited + if self.proc.exitcode is not None and self.proc.pid is not None: + return + + # Can't signal if we don't have a pid + if self.proc.pid is None: + return + + cloudlog.info(f"sending signal {sig} to {self.name}") + os.kill(self.proc.pid, sig) + + def get_process_state_msg(self): + state = log.ManagerState.ProcessState.new_message() + state.name = self.name + if self.proc: + state.running = self.proc.is_alive() + state.shouldBeRunning = self.proc is not None and not self.shutting_down + state.pid = self.proc.pid or 0 + state.exitCode = self.proc.exitcode or 0 + return state + + +class NativeProcess(ManagerProcess): + def __init__(self, name, cwd, cmdline, enabled=True, onroad=True, offroad=False, callback=None, unkillable=False, sigkill=False, watchdog_max_dt=None): + self.name = name + self.cwd = cwd + self.cmdline = cmdline + self.enabled = enabled + self.onroad = onroad + self.offroad = offroad + self.callback = callback + self.unkillable = unkillable + self.sigkill = sigkill + self.watchdog_max_dt = watchdog_max_dt + + def prepare(self) -> None: + pass + + def start(self) -> None: + # In case we only tried a non blocking stop we need to stop it before restarting + if self.shutting_down: + self.stop() + + if self.proc is not None: + return + + cwd = os.path.join(BASEDIR, self.cwd) + cloudlog.info(f"starting process {self.name}") + self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd, self.name)) + self.proc.start() + self.watchdog_seen = False + self.shutting_down = False + + +class PythonProcess(ManagerProcess): + def __init__(self, name, module, enabled=True, onroad=True, offroad=False, callback=None, unkillable=False, sigkill=False, watchdog_max_dt=None): + self.name = name + self.module = module + self.enabled = enabled + self.onroad = onroad + self.offroad = offroad + self.callback = callback + self.unkillable = unkillable + self.sigkill = sigkill + self.watchdog_max_dt = watchdog_max_dt + + def prepare(self) -> None: + if self.enabled: + cloudlog.info(f"preimporting {self.module}") + importlib.import_module(self.module) + + def start(self) -> None: + # In case we only tried a non blocking stop we need to stop it before restarting + if self.shutting_down: + self.stop() + + if self.proc is not None: + return + + cloudlog.info(f"starting python {self.module}") + self.proc = Process(name=self.name, target=launcher, args=(self.module, self.name)) + self.proc.start() + self.watchdog_seen = False + self.shutting_down = False + + +class DaemonProcess(ManagerProcess): + """Python process that has to stay running across manager restart. + This is used for athena so you don't lose SSH access when restarting manager.""" + def __init__(self, name, module, param_name, enabled=True): + self.name = name + self.module = module + self.param_name = param_name + self.enabled = enabled + self.onroad = True + self.offroad = True + + def prepare(self) -> None: + pass + + def start(self) -> None: + params = Params() + pid = params.get(self.param_name, encoding='utf-8') + + if pid is not None: + try: + os.kill(int(pid), 0) + with open(f'/proc/{pid}/cmdline') as f: + if self.module in f.read(): + # daemon is running + return + except (OSError, FileNotFoundError): + # process is dead + pass + + cloudlog.info(f"starting daemon {self.name}") + proc = subprocess.Popen(['python', '-m', self.module], # pylint: disable=subprocess-popen-preexec-fn + stdin=open('/dev/null'), + stdout=open('/dev/null', 'w'), + stderr=open('/dev/null', 'w'), + preexec_fn=os.setpgrp) + + params.put(self.param_name, str(proc.pid)) + + def stop(self, retry=True, block=True) -> None: + pass + + +def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, + not_run: Optional[List[str]]=None) -> None: + if not_run is None: + not_run = [] + + for p in procs: + # Conditions that make a process run + run = any(( + p.offroad and not started, + p.onroad and started, + )) + if p.callback is not None and None not in (params, CP): + run = run or p.callback(started, params, CP) + + # Conditions that block a process from starting + run = run and not any(( + not p.enabled, + p.name in not_run, + )) + + if run: + p.start() + else: + p.stop(block=False) + + p.check_watchdog(started) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py new file mode 100644 index 00000000000000..7702b96eaa66d4 --- /dev/null +++ b/selfdrive/manager/process_config.py @@ -0,0 +1,64 @@ +import os + +from cereal import car +from common.params import Params +from system.hardware import PC, TICI +from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess + +WEBCAM = os.getenv("USE_WEBCAM") is not None + +def driverview(started: bool, params: Params, CP: car.CarParams) -> bool: + return params.get_bool("IsDriverViewEnabled") # type: ignore + +def notcar(started: bool, params: Params, CP: car.CarParams) -> bool: + return CP.notCar # type: ignore + +def logging(started, params, CP: car.CarParams) -> bool: + run = (not CP.notCar) or not params.get_bool("DisableLogging") + return started and run + +procs = [ + # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption + NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview), + NativeProcess("clocksd", "system/clocksd", ["./clocksd"]), + NativeProcess("logcatd", "system/logcatd", ["./logcatd"]), + NativeProcess("proclogd", "system/proclogd", ["./proclogd"]), + PythonProcess("logmessaged", "system.logmessaged", offroad=True), + PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True), + + DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), + NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview), + NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), + NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), + NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), + NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC), + NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), + NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)), + NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True), + NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]), + NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False), + PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"), + PythonProcess("controlsd", "selfdrive.controls.controlsd"), + PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), + PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), + PythonProcess("laikad", "selfdrive.locationd.laikad"), + PythonProcess("navd", "selfdrive.navd.navd"), + PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), + PythonProcess("paramsd", "selfdrive.locationd.paramsd"), + PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI), + PythonProcess("plannerd", "selfdrive.controls.plannerd"), + PythonProcess("radard", "selfdrive.controls.radard"), + PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True), + PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True), + PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True), + PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), + PythonProcess("statsd", "selfdrive.statsd", offroad=True), + + NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), + PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar), + + # Experimental + PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=(TICI and os.path.isfile("/persist/comma/use-quectel-rawgps"))), +] + +managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/manager/test/__init__.py b/selfdrive/manager/test/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py new file mode 100755 index 00000000000000..f2e5319e8eb7d5 --- /dev/null +++ b/selfdrive/manager/test/test_manager.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +import os +import signal +import time +import unittest + +import selfdrive.manager.manager as manager +from selfdrive.manager.process import DaemonProcess +from selfdrive.manager.process_config import managed_processes +from system.hardware import HARDWARE + +os.environ['FAKEUPLOAD'] = "1" + +MAX_STARTUP_TIME = 3 +ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])] + + +class TestManager(unittest.TestCase): + def setUp(self): + os.environ['PASSIVE'] = '0' + HARDWARE.set_power_save(False) + + def tearDown(self): + manager.manager_cleanup() + + def test_manager_prepare(self): + os.environ['PREPAREONLY'] = '1' + manager.main() + + def test_startup_time(self): + for _ in range(10): + start = time.monotonic() + os.environ['PREPAREONLY'] = '1' + manager.main() + t = time.monotonic() - start + assert t < MAX_STARTUP_TIME, f"startup took {t}s, expected <{MAX_STARTUP_TIME}s" + + def test_clean_exit(self): + """ + Ensure all processes exit cleanly when stopped. + """ + HARDWARE.set_power_save(False) + manager.manager_prepare() + for p in ALL_PROCESSES: + managed_processes[p].start() + + time.sleep(10) + + for p in reversed(ALL_PROCESSES): + with self.subTest(proc=p): + state = managed_processes[p].get_process_state_msg() + self.assertTrue(state.running, f"{p} not running") + exit_code = managed_processes[p].stop(retry=False) + + self.assertTrue(exit_code is not None, f"{p} failed to exit") + + # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code + exit_codes = [0, 1] + if managed_processes[p].sigkill: + exit_codes = [-signal.SIGKILL] + self.assertIn(exit_code, exit_codes, f"{p} died with {exit_code}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 91f3597447bd66..246f8c2941575b 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,70 +1,113 @@ import os -import glob -Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc') +Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations') lenv = env.Clone() -lenvCython = envCython.Clone() -libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread'] -frameworks = [] +libs = [cereal, messaging, common, visionipc, gpucommon, + 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv'] + +def get_dlsym_offset(): + """Returns the offset between dlopen and dlsym in libdl.so""" + import ctypes + libdl = ctypes.PyDLL('libdl.so') + dlopen = ctypes.cast(libdl.dlopen, ctypes.c_void_p).value + dlsym = ctypes.cast(libdl.dlsym, ctypes.c_void_p).value + return dlsym - dlopen + common_src = [ "models/commonmodel.cc", + "runners/snpemodel.cc", "transforms/loadyuv.cc", - "transforms/transform.cc", + "transforms/transform.cc" ] -# OpenCL is a framework on Mac -if arch == "Darwin": - frameworks += ['OpenCL'] +thneed_src = [ + "thneed/thneed_common.cc", + "thneed/thneed_qcom2.cc", + "thneed/serialize.cc", + "runners/thneedmodel.cc", +] + +use_thneed = not GetOption('no_thneed') + +if arch == "larch64": + libs += ['gsl', 'CB', 'pthread', 'dl'] + + if use_thneed: + common_src += thneed_src + dlsym_offset = get_dlsym_offset() + lenv['CXXFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append(f"-DDLSYM_OFFSET={dlsym_offset}") else: - libs += ['OpenCL'] - -# Set path definitions -for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transforms/loadyuv.cl'}.items(): - for xenv in (lenv, lenvCython): - xenv['CXXFLAGS'].append(f'-D{pathdef}_PATH=\\"{File(fn).abspath}\\"') - -# Compile cython -cython_libs = envCython["LIBS"] + libs -commonmodel_lib = lenv.Library('commonmodel', common_src) -lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) -tinygrad_files = sorted(["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]) - -# Get model metadata -for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: - fn = File(f"models/{model_name}").abspath - script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)] - cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx' - lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files + script_files, cmd) - -def tg_compile(flags, model_name): - pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"' - fn = File(f"models/{model_name}").abspath - return lenv.Command( - fn + "_tinygrad.pkl", - [fn + ".onnx"] + tinygrad_files, - f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl' - ) - -# Compile small models -for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: - flags = { - 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 IMAGE=2 JIT_BATCH_SIZE=0', - 'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")}', # tinygrad calls brew which needs a $HOME in the env - }.get(arch, 'DEV=CPU CPU_LLVM=1') - tg_compile(flags, model_name) - -# Compile BIG model if USB GPU is available -if "USBGPU" in os.environ: - import subprocess - # because tg doesn't support multi-process - devs = subprocess.check_output('python3 -c "from tinygrad import Device; print(list(Device.get_available_devices()))"', shell=True, cwd=env.Dir('#').abspath) - if b"AMD" in devs: - print("USB GPU detected... building") - flags = "DEV=AMD AMD_IFACE=USB AMD_LLVM=1 NOLOCALS=0 IMAGE=0" - bp = tg_compile(flags, "big_driving_policy") - bv = tg_compile(flags, "big_driving_vision") - lenv.SideEffect('lock', [bp, bv]) # tg doesn't support multi-process so build serially + libs += ['pthread'] + + if not GetOption('snpe'): + # for onnx support + common_src += ['runners/onnxmodel.cc'] + + # tell runners to use onnx + lenv['CFLAGS'].append("-DUSE_ONNX_MODEL") + lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL") + + if arch == "Darwin": + # fix OpenCL + del libs[libs.index('OpenCL')] + lenv['FRAMEWORKS'] = ['OpenCL'] + + # no SNPE on Mac + del libs[libs.index('SNPE')] + del common_src[common_src.index('runners/snpemodel.cc')] + +common_model = lenv.Object(common_src) + +lenv.Program('_dmonitoringmodeld', [ + "dmonitoringmodeld.cc", + "models/dmonitoring.cc", + ]+common_model, LIBS=libs) + +# build thneed model +if use_thneed and arch == "larch64" or GetOption('pc_thneed'): + fn = File("models/supercombo").abspath + + if GetOption('pc_thneed'): + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" else: - print("USB GPU not detected... skipping") + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 PYOPENCL_NO_CACHE=1 MATMUL=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + + # is there a better way then listing all of tinygrad? + lenv.Command(fn + ".thneed", [fn + ".onnx", + "#tinygrad_repo/openpilot/compile.py", + "#tinygrad_repo/accel/opencl/conv.cl", + "#tinygrad_repo/accel/opencl/matmul.cl", + "#tinygrad_repo/accel/opencl/ops_opencl.py", + "#tinygrad_repo/accel/opencl/preprocessing.py", + "#tinygrad_repo/extra/onnx.py", + "#tinygrad_repo/extra/utils.py", + "#tinygrad_repo/tinygrad/llops/ops_gpu.py", + "#tinygrad_repo/tinygrad/llops/ops_opencl.py", + "#tinygrad_repo/tinygrad/helpers.py", + "#tinygrad_repo/tinygrad/mlops.py", + "#tinygrad_repo/tinygrad/ops.py", + "#tinygrad_repo/tinygrad/shapetracker.py", + "#tinygrad_repo/tinygrad/tensor.py", + "#tinygrad_repo/tinygrad/nn/__init__.py" + ], cmd) + +llenv = lenv.Clone() +if GetOption('pc_thneed'): + pc_thneed_src = [ + "thneed/thneed_common.cc", + "thneed/thneed_pc.cc", + "thneed/serialize.cc", + "runners/thneedmodel.cc", + ] + llenv['CFLAGS'].append("-DUSE_THNEED") + llenv['CXXFLAGS'].append("-DUSE_THNEED") + common_model += llenv.Object(pc_thneed_src) + libs += ['dl'] + +llenv.Program('_modeld', [ + "modeld.cc", + "models/driving.cc", + ]+common_model, LIBS=libs + transformations) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index ff7e1d86006e83..125864b98b0e4b 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -1,87 +1,7 @@ -import numpy as np +IDX_N = 33 def index_function(idx, max_val=192, max_idx=32): return (max_val) * ((idx/max_idx)**2) -class ModelConstants: - # time and distance indices - IDX_N = 33 - T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] - X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)] - LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] - LEAD_T_OFFSETS = [0., 2., 4.] - META_T_IDXS = [2., 4., 6., 8., 10.] - # model inputs constants - N_FRAMES = 2 - MODEL_RUN_FREQ = 20 - MODEL_CONTEXT_FREQ = 5 # "model_trained_fps" - - FEATURE_LEN = 512 - - DESIRE_LEN = 8 - TRAFFIC_CONVENTION_LEN = 2 - LAT_PLANNER_STATE_LEN = 4 - LATERAL_CONTROL_PARAMS_LEN = 2 - PREV_DESIRED_CURV_LEN = 1 - - # model outputs constants - FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) - FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) - FCW_5MS2_PROBS_WIDTH = 5 - FCW_3MS2_PROBS_WIDTH = 2 - - DISENGAGE_WIDTH = 5 - POSE_WIDTH = 6 - WIDE_FROM_DEVICE_WIDTH = 3 - LEAD_WIDTH = 4 - LANE_LINES_WIDTH = 2 - ROAD_EDGES_WIDTH = 2 - PLAN_WIDTH = 15 - DESIRE_PRED_WIDTH = 8 - LAT_PLANNER_SOLUTION_WIDTH = 4 - DESIRED_CURV_WIDTH = 1 - - NUM_LANE_LINES = 4 - NUM_ROAD_EDGES = 2 - - LEAD_TRAJ_LEN = 6 - DESIRE_PRED_LEN = 4 - - PLAN_MHP_N = 5 - LEAD_MHP_N = 2 - PLAN_MHP_SELECTION = 1 - LEAD_MHP_SELECTION = 3 - - FCW_THRESHOLD_5MS2_HIGH = 0.15 - FCW_THRESHOLD_5MS2_LOW = 0.05 - FCW_THRESHOLD_3MS2 = 0.7 - - CONFIDENCE_BUFFER_LEN = 5 - RYG_GREEN = 0.01165 - RYG_YELLOW = 0.06157 - - POLY_PATH_DEGREE = 4 - -# model outputs slices -class Plan: - POSITION = slice(0, 3) - VELOCITY = slice(3, 6) - ACCELERATION = slice(6, 9) - T_FROM_CURRENT_EULER = slice(9, 12) - ORIENTATION_RATE = slice(12, 15) - -class Meta: - ENGAGED = slice(0, 1) - # next 2, 4, 6, 8, 10 seconds - GAS_DISENGAGE = slice(1, 31, 6) - BRAKE_DISENGAGE = slice(2, 31, 6) - STEER_OVERRIDE = slice(3, 31, 6) - HARD_BRAKE_3 = slice(4, 31, 6) - HARD_BRAKE_4 = slice(5, 31, 6) - HARD_BRAKE_5 = slice(6, 31, 6) - # next 0, 2, 4, 6, 8, 10 seconds - GAS_PRESS = slice(31, 55, 4) - BRAKE_PRESS = slice(32, 55, 4) - LEFT_BLINKER = slice(33, 55, 4) - RIGHT_BLINKER = slice(34, 55, 4) +T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld new file mode 100755 index 00000000000000..f292fe4c0bd2a4 --- /dev/null +++ b/selfdrive/modeld/dmonitoringmodeld @@ -0,0 +1,12 @@ +#!/bin/sh + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +if [ -f /TICI ]; then + export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH" + export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/" +else + export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH" +fi +exec ./_dmonitoringmodeld diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc new file mode 100644 index 00000000000000..cde13a9beeb6ba --- /dev/null +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -0,0 +1,65 @@ +#include +#include + +#include +#include + +#include "cereal/visionipc/visionipc_client.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "selfdrive/modeld/models/dmonitoring.h" + +ExitHandler do_exit; + +void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { + PubMaster pm({"driverStateV2"}); + SubMaster sm({"liveCalibration"}); + float calib[CALIB_LEN] = {0}; + double last = 0; + + while (!do_exit) { + VisionIpcBufExtra extra = {}; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; + + sm.update(0); + if (sm.updated("liveCalibration")) { + auto calib_msg = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + for (int i = 0; i < CALIB_LEN; i++) { + calib[i] = calib_msg[i]; + } + } + + double t1 = millis_since_boot(); + DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib); + double t2 = millis_since_boot(); + + // send dm packet + dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output); + + //printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last); + last = t1; + } +} + +int main(int argc, char **argv) { + setpriority(PRIO_PROCESS, 0, -15); + + // init the models + DMonitoringModelState model; + dmonitoring_init(&model); + + VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_DRIVER, true); + while (!do_exit && !vipc_client.connect(false)) { + util::sleep_for(100); + } + + // run the models + if (vipc_client.connected) { + LOGW("connected with buffer size: %d", vipc_client.buffers[0].len); + run_model(model, vipc_client); + } + + dmonitoring_free(&model); + return 0; +} diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py deleted file mode 100755 index fca762c69bf504..00000000000000 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python3 -import os -from openpilot.system.hardware import TICI -os.environ['DEV'] = 'QCOM' if TICI else 'CPU' -from tinygrad.tensor import Tensor -from tinygrad.dtype import dtypes -import time -import pickle -import numpy as np -from pathlib import Path - -from cereal import messaging -from cereal.messaging import PubMaster, SubMaster -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog -from openpilot.common.realtime import config_realtime_process -from openpilot.common.transformations.model import dmonitoringmodel_intrinsics -from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye -from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame -from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp -from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address - -PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld" -SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' -METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl' - - -class ModelState: - inputs: dict[str, np.ndarray] - output: np.ndarray - - def __init__(self, cl_ctx): - with open(METADATA_PATH, 'rb') as f: - model_metadata = pickle.load(f) - self.input_shapes = model_metadata['input_shapes'] - self.output_slices = model_metadata['output_slices'] - - self.frame = MonitoringModelFrame(cl_ctx) - self.numpy_inputs = { - 'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32), - } - - self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} - with open(MODEL_PKL_PATH, "rb") as f: - self.model_run = pickle.load(f) - - def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: - self.numpy_inputs['calib'][0,:] = calib - - t1 = time.perf_counter() - - input_img_cl = self.frame.prepare(buf, transform.flatten()) - if TICI: - # The imgs tensors are backed by opencl memory, only need init once - if 'input_img' not in self.tensor_inputs: - self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8) - else: - self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize() - - - output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy() - - t2 = time.perf_counter() - return output, t2 - t1 - -def slice_outputs(model_outputs, output_slices): - return {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} - -def parse_model_output(model_output): - parsed = {} - parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right']) - for ds_suffix in ['lhd', 'rhd']: - face_descs = model_output[f'face_descs_{ds_suffix}'] - parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] - parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) - for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']: - parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}']) - return parsed - -def fill_driver_data(msg, model_output, ds_suffix): - msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist() - msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist() - msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist() - msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist() - msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item() - msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item() - msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item() - msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item() - msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item() - msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item() - msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() - -def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): - msg = messaging.new_message('driverStateV2', valid=True) - ds = msg.driverStateV2 - ds.frameId = frame_id - ds.modelExecutionTime = exec_time - ds.gpuExecutionTime = gpu_exec_time - ds.rawPredictions = model_output['raw_pred'] - ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item() - fill_driver_data(ds.leftDriverData, model_output, 'lhd') - fill_driver_data(ds.rightDriverData, model_output, 'rhd') - return msg - - -def main(): - config_realtime_process(7, 5) - - cl_context = CLContext() - model = ModelState(cl_context) - cloudlog.warning("models loaded, dmonitoringmodeld starting") - - cloudlog.warning("connecting to driver stream") - vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True, cl_context) - while not vipc_client.connect(False): - time.sleep(0.1) - assert vipc_client.is_connected() - cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - - sm = SubMaster(["liveCalibration"]) - pm = PubMaster(["driverStateV2"]) - - calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32) - model_transform = None - - while True: - buf = vipc_client.recv() - if buf is None: - continue - - if model_transform is None: - cam = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye - model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32) - - sm.update(0) - if sm.updated["liveCalibration"]: - calib[:] = np.array(sm["liveCalibration"].rpyCalib) - - t1 = time.perf_counter() - model_output, gpu_execution_time = model.run(buf, calib, model_transform) - t2 = time.perf_counter() - raw_pred = model_output.tobytes() if SEND_RAW_PRED else b'' - model_output = slice_outputs(model_output, model.output_slices) - model_output = parse_model_output(model_output) - model_output['raw_pred'] = raw_pred - msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time) - pm.send("driverStateV2", msg) - - -if __name__ == "__main__": - try: - main() - except KeyboardInterrupt: - cloudlog.warning("got SIGINT") diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py deleted file mode 100644 index 82c4c92b1d53c7..00000000000000 --- a/selfdrive/modeld/fill_model_msg.py +++ /dev/null @@ -1,193 +0,0 @@ -import os -import capnp -import numpy as np -from cereal import log -from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta - -SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') - -ConfidenceClass = log.ModelDataV2.ConfidenceClass - - -class PublishState: - def __init__(self): - self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) - self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32) - self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32) - -def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None): - builder.t = t - builder.x = x.tolist() - builder.y = y.tolist() - builder.z = z.tolist() - if x_std is not None: - builder.xStd = x_std.tolist() - if y_std is not None: - builder.yStd = y_std.tolist() - if z_std is not None: - builder.zStd = z_std.tolist() - -def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None): - builder.t = t - builder.x = x.tolist() - builder.y = y.tolist() - builder.v = v.tolist() - builder.a = a.tolist() - if x_std is not None: - builder.xStd = x_std.tolist() - if y_std is not None: - builder.yStd = y_std.tolist() - if v_std is not None: - builder.vStd = v_std.tolist() - if a_std is not None: - builder.aStd = a_std.tolist() - -def fill_xyz_poly(builder, degree, x, y, z): - xyz = np.stack([x, y, z], axis=1) - coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree) - builder.xCoefficients = coeffs[:, 0].tolist() - builder.yCoefficients = coeffs[:, 1].tolist() - builder.zCoefficients = coeffs[:, 2].tolist() - -def fill_lane_line_meta(builder, lane_lines, lane_line_probs): - builder.leftY = lane_lines[1].y[0] - builder.leftProb = lane_line_probs[1] - builder.rightY = lane_lines[2].y[0] - builder.rightProb = lane_line_probs[2] - -def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, - publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, - frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, - valid: bool) -> None: - frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 - frame_drop_perc = frame_drop * 100 - extended_msg.valid = valid - base_msg.valid = valid - - driving_model_data = base_msg.drivingModelData - - driving_model_data.frameId = vipc_frame_id - driving_model_data.frameIdExtra = vipc_frame_id_extra - driving_model_data.frameDropPerc = frame_drop_perc - driving_model_data.modelExecutionTime = model_execution_time - - driving_model_data.action = action - - modelV2 = extended_msg.modelV2 - modelV2.frameId = vipc_frame_id - modelV2.frameIdExtra = vipc_frame_id_extra - modelV2.frameAge = frame_age - modelV2.frameDropPerc = frame_drop_perc - modelV2.timestampEof = timestamp_eof - modelV2.modelExecutionTime = model_execution_time - - # plan - fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) - fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) - fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) - fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) - fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) - - # poly path - fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) - - # action - modelV2.action = action - - # times at X_IDXS of edges and lines aren't used - LINE_T_IDXS: list[float] = [] - - # lane lines - modelV2.init('laneLines', 4) - for i in range(4): - lane_line = modelV2.laneLines[i] - fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) - modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() - modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() - - fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs) - - # road edges - modelV2.init('roadEdges', 2) - for i in range(2): - road_edge = modelV2.roadEdges[i] - fill_xyzt(road_edge, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) - modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist() - - # leads - modelV2.init('leadsV3', 3) - for i in range(3): - lead = modelV2.leadsV3[i] - fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T) - lead.prob = net_output_data['lead_prob'][0,i].tolist() - lead.probTime = ModelConstants.LEAD_T_OFFSETS[i] - - # meta - meta = modelV2.meta - meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist() - meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist() - meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item() - meta.init('disengagePredictions') - disengage_predictions = meta.disengagePredictions - disengage_predictions.t = ModelConstants.META_T_IDXS - disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist() - disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist() - disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist() - disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist() - disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist() - disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() - disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist() - disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist() - - publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:] - publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0] - publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:] - publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0] - hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \ - (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all() - meta.hardBrakePredicted = hard_brake_predicted.item() - - # confidence - 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] - steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE] - any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs)) - # independent disengage prob for each 2s slice - ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])] - # rolling buf for 2, 4, 6, 8, 10s - publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:] - publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs - - score = 0. - for i in range(ModelConstants.DISENGAGE_WIDTH): - score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH - if score < ModelConstants.RYG_GREEN: - modelV2.confidence = ConfidenceClass.green - elif score < ModelConstants.RYG_YELLOW: - modelV2.confidence = ConfidenceClass.yellow - else: - modelV2.confidence = ConfidenceClass.red - - # raw prediction if enabled - if SEND_RAW_PRED: - modelV2.rawPredictions = net_output_data['raw_pred'].tobytes() - -def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], - vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: - msg.valid = live_calib_seen & (vipc_dropped_frames < 1) - cameraOdometry = msg.cameraOdometry - - cameraOdometry.frameId = vipc_frame_id - cameraOdometry.timestampEof = timestamp_eof - - cameraOdometry.trans = net_output_data['pose'][0,:3].tolist() - cameraOdometry.rot = net_output_data['pose'][0,3:].tolist() - cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist() - cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist() - cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist() - cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist() - cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist() - cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist() diff --git a/selfdrive/modeld/get_model_metadata.py b/selfdrive/modeld/get_model_metadata.py deleted file mode 100755 index 2001d23d752bc7..00000000000000 --- a/selfdrive/modeld/get_model_metadata.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -import sys -import pathlib -import onnx -import codecs -import pickle -from typing import Any - -def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]: - shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) - name = value_info.name - return name, shape - -def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any: - for prop in model.metadata_props: - if prop.key == name: - return prop.value - return None - -if __name__ == "__main__": - model_path = pathlib.Path(sys.argv[1]) - model = onnx.load(str(model_path)) - output_slices = get_metadata_value_by_name(model, 'output_slices') - assert output_slices is not None, 'output_slices not found in metadata' - - metadata = { - 'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'), - 'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")), - 'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]), - 'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output]) - } - - metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl') - with open(metadata_path, 'wb') as f: - pickle.dump(metadata, f) - - print(f'saved metadata to {metadata_path}') diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld new file mode 100755 index 00000000000000..c067cf3d622489 --- /dev/null +++ b/selfdrive/modeld/modeld @@ -0,0 +1,11 @@ +#!/bin/sh + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +if [ -f /TICI ]; then + export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH" +else + export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH" +fi +exec ./_modeld diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc new file mode 100644 index 00000000000000..653661a3a8c764 --- /dev/null +++ b/selfdrive/modeld/modeld.cc @@ -0,0 +1,219 @@ +#include +#include +#include +#include + +#include + +#include "cereal/messaging/messaging.h" +#include "common/transformations/orientation.hpp" + +#include "cereal/visionipc/visionipc_client.h" +#include "common/clutil.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "system/hardware/hw.h" +#include "selfdrive/modeld/models/driving.h" + + +ExitHandler do_exit; + +mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camera, bool bigmodel_frame) { + /* + import numpy as np + from common.transformations.model import medmodel_frame_from_calib_frame + medmodel_frame_from_calib_frame = medmodel_frame_from_calib_frame[:, :3] + calib_from_smedmodel_frame = np.linalg.inv(medmodel_frame_from_calib_frame) + */ + static const auto calib_from_medmodel = (Eigen::Matrix() << + 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, + 1.09890110e-03, 0.00000000e+00, -2.81318681e-01, + -2.25466395e-20, 1.09890110e-03,-5.23076923e-02).finished(); + + static const auto calib_from_sbigmodel = (Eigen::Matrix() << + 0.00000000e+00, 7.31372216e-19, 1.00000000e+00, + 2.19780220e-03, 4.11497335e-19, -5.62637363e-01, + -6.66298828e-20, 2.19780220e-03, -3.33626374e-01).finished(); + + static const auto view_from_device = (Eigen::Matrix() << + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0).finished(); + + + const auto cam_intrinsics = Eigen::Matrix(wide_camera ? ecam_intrinsic_matrix.v : fcam_intrinsic_matrix.v); + Eigen::Matrix device_from_calib = euler2rot(device_from_calib_euler).cast (); + auto calib_from_model = bigmodel_frame ? calib_from_sbigmodel : calib_from_medmodel; + auto camera_from_calib = cam_intrinsics * view_from_device * device_from_calib; + auto warp_matrix = camera_from_calib * calib_from_model; + + mat3 transform = {}; + for (int i=0; i<3*3; i++) { + transform.v[i] = warp_matrix(i / 3, i % 3); + } + static const mat3 yuv_transform = get_model_yuv_transform(); + return matmul3(yuv_transform, transform); +} + + +void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) { + // messaging + PubMaster pm({"modelV2", "cameraOdometry"}); + SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState"}); + + // setup filter to track dropped frames + FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); + + uint32_t frame_id = 0, last_vipc_frame_id = 0; + double last = 0; + uint32_t run_count = 0; + + mat3 model_transform_main = {}; + mat3 model_transform_extra = {}; + bool live_calib_seen = false; + + VisionBuf *buf_main = nullptr; + VisionBuf *buf_extra = nullptr; + + VisionIpcBufExtra meta_main = {0}; + VisionIpcBufExtra meta_extra = {0}; + + while (!do_exit) { + // Keep receiving frames until we are at least 1 frame ahead of previous extra frame + while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) { + buf_main = vipc_client_main.recv(&meta_main); + if (buf_main == nullptr) break; + } + + if (buf_main == nullptr) { + LOGE("vipc_client_main no frame"); + continue; + } + + if (use_extra_client) { + // Keep receiving extra frames until frame id matches main camera + do { + buf_extra = vipc_client_extra.recv(&meta_extra); + } while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL); + + if (buf_extra == nullptr) { + LOGE("vipc_client_extra no frame"); + continue; + } + + if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) { + LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)", + meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9, + meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9); + } + } else { + // Use single camera + buf_extra = buf_main; + meta_extra = meta_main; + } + + // TODO: path planner timeout? + sm.update(0); + int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); + bool is_rhd = ((bool)sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); + frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); + if (sm.updated("liveCalibration")) { + auto rpy_calib = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + Eigen::Vector3d device_from_calib_euler; + for (int i=0; i<3; i++) { + device_from_calib_euler(i) = rpy_calib[i]; + } + model_transform_main = update_calibration(device_from_calib_euler, main_wide_camera, false); + model_transform_extra = update_calibration(device_from_calib_euler, true, true); + live_calib_seen = true; + } + + float vec_desire[DESIRE_LEN] = {0}; + if (desire >= 0 && desire < DESIRE_LEN) { + vec_desire[desire] = 1.0; + } + + // tracked dropped frames + uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1; + float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U)); + if (run_count < 10) { // let frame drops warm up + frame_dropped_filter.reset(0); + frames_dropped = 0.; + } + run_count++; + + float frame_drop_ratio = frames_dropped / (1 + frames_dropped); + bool prepare_only = vipc_dropped_frames > 0; + + if (prepare_only) { + LOGE("skipping model eval. Dropped %d frames", vipc_dropped_frames); + } + + double mt1 = millis_since_boot(); + ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only); + double mt2 = millis_since_boot(); + float model_execution_time = (mt2 - mt1) / 1000.0; + + if (model_output != nullptr) { + model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time, + kj::ArrayPtr(model.output.data(), model.output.size()), live_calib_seen); + posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen); + } + + //printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio); + last = mt1; + last_vipc_frame_id = meta_main.frame_id; + } +} + +int main(int argc, char **argv) { + if (!Hardware::PC()) { + int ret; + ret = util::set_realtime_priority(54); + assert(ret == 0); + util::set_core_affinity({7}); + assert(ret == 0); + } + + bool main_wide_camera = Params().getBool("WideCameraOnly"); + bool use_extra_client = !main_wide_camera; // set for single camera mode + + // cl init + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); + + // init the models + ModelState model; + model_init(&model, device_id, context); + LOGW("models loaded, modeld starting"); + + VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context); + VisionIpcClient vipc_client_extra = VisionIpcClient("camerad", VISION_STREAM_WIDE_ROAD, false, device_id, context); + + while (!do_exit && !vipc_client_main.connect(false)) { + util::sleep_for(100); + } + + while (!do_exit && use_extra_client && !vipc_client_extra.connect(false)) { + util::sleep_for(100); + } + + // run the models + // vipc_client.connected is false only when do_exit is true + if (!do_exit) { + const VisionBuf *b = &vipc_client_main.buffers[0]; + LOGW("connected main cam with buffer size: %d (%d x %d)", b->len, b->width, b->height); + + if (use_extra_client) { + const VisionBuf *wb = &vipc_client_extra.buffers[0]; + LOGW("connected extra cam with buffer size: %d (%d x %d)", wb->len, wb->width, wb->height); + } + + run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra_client); + } + + model_free(&model); + CL_CHECK(clReleaseContext(context)); + return 0; +} diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py deleted file mode 100755 index 1f347dc32a019d..00000000000000 --- a/selfdrive/modeld/modeld.py +++ /dev/null @@ -1,411 +0,0 @@ -#!/usr/bin/env python3 -import os -from openpilot.system.hardware import TICI -os.environ['DEV'] = 'QCOM' if TICI else 'CPU' -USBGPU = "USBGPU" in os.environ -if USBGPU: - os.environ['DEV'] = 'AMD' - os.environ['AMD_IFACE'] = 'USB' -from tinygrad.tensor import Tensor -from tinygrad.dtype import dtypes -import time -import pickle -import numpy as np -import cereal.messaging as messaging -from cereal import car, log -from pathlib import Path -from cereal.messaging import PubMaster, SubMaster -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from opendbc.car.car_helpers import get_demo_car_params -from openpilot.common.swaglog import cloudlog -from openpilot.common.params import Params -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import config_realtime_process, DT_MDL -from openpilot.common.transformations.camera import DEVICE_CAMERAS -from openpilot.common.transformations.model import get_warp_matrix -from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan -from openpilot.selfdrive.modeld.parse_model_outputs import Parser -from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState -from openpilot.selfdrive.modeld.constants import ModelConstants, Plan -from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext -from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address - - -PROCESS_NAME = "selfdrive.modeld.modeld" -SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') - -VISION_PKL_PATH = Path(__file__).parent / 'models/driving_vision_tinygrad.pkl' -POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' -VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' -POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' - -LAT_SMOOTH_SECONDS = 0.0 -LONG_SMOOTH_SECONDS = 0.3 -MIN_LAT_CONTROL_SPEED = 0.3 - - -def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: - plan = model_output['plan'][0] - desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], - plan[:,Plan.ACCELERATION][:,0], - ModelConstants.T_IDXS, - action_t=long_action_t) - desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - - desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2], - plan[:,Plan.ORIENTATION_RATE][:,2], - ModelConstants.T_IDXS, - v_ego, - lat_action_t) - if v_ego > MIN_LAT_CONTROL_SPEED: - desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) - else: - desired_curvature = prev_action.desiredCurvature - - return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), - desiredAcceleration=float(desired_accel), - shouldStop=bool(should_stop)) - -class FrameMeta: - frame_id: int = 0 - timestamp_sof: int = 0 - timestamp_eof: int = 0 - - def __init__(self, vipc=None): - 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: - frames: dict[str, DrivingModelFrame] - inputs: dict[str, np.ndarray] - output: np.ndarray - prev_desire: np.ndarray # for tracking the rising edge of the pulse - - def __init__(self, context: CLContext): - with open(VISION_METADATA_PATH, 'rb') as f: - vision_metadata = pickle.load(f) - self.vision_input_shapes = vision_metadata['input_shapes'] - self.vision_input_names = list(self.vision_input_shapes.keys()) - self.vision_output_slices = vision_metadata['output_slices'] - vision_output_size = vision_metadata['output_shapes']['outputs'][1] - - with open(POLICY_METADATA_PATH, 'rb') as f: - policy_metadata = pickle.load(f) - self.policy_input_shapes = policy_metadata['input_shapes'] - self.policy_output_slices = policy_metadata['output_slices'] - policy_output_size = policy_metadata['output_shapes']['outputs'][1] - - 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) - - # policy inputs - 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] = {} - self.vision_output = np.zeros(vision_output_size, dtype=np.float32) - self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} - self.policy_output = np.zeros(policy_output_size, dtype=np.float32) - self.parser = Parser() - - with open(VISION_PKL_PATH, "rb") as f: - self.vision_run = pickle.load(f) - - with open(POLICY_PKL_PATH, "rb") as f: - self.policy_run = pickle.load(f) - - def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: - parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} - return parsed_model_outputs - - 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_pulse'][0] = 0 - new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) - self.prev_desire[:] = inputs['desire_pulse'] - - imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names} - - if TICI and not USBGPU: - # The imgs tensors are backed by opencl memory, only need init once - for key in imgs_cl: - if key not in self.vision_inputs: - self.vision_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.vision_input_shapes[key], dtype=dtypes.uint8) - else: - for key in imgs_cl: - frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key]) - self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize() - - if prepare_only: - return None - - 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_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)) - - combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} - if SEND_RAW_PRED: - combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()]) - - return combined_outputs_dict - - -def main(demo=False): - cloudlog.warning("modeld init") - - if not USBGPU: - # USB GPU currently saturates a core so can't do this yet, - # also need to move the aux USB interrupts for good timings - config_realtime_process(7, 54) - - st = time.monotonic() - cloudlog.warning("setting up CL context") - cl_context = CLContext() - cloudlog.warning("CL context ready; loading model") - model = ModelState(cl_context) - cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting") - - # visionipc clients - while True: - available_streams = VisionIpcClient.available_streams("camerad", block=False) - if available_streams: - use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams - main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams - break - time.sleep(.1) - - vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD - vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context) - vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) - cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") - - while not vipc_client_main.connect(False): - time.sleep(0.1) - while use_extra_client and not vipc_client_extra.connect(False): - time.sleep(0.1) - - cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})") - if use_extra_client: - cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") - - # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) - - publish_state = PublishState() - params = Params() - - # setup filter to track dropped frames - frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ) - frame_id = 0 - last_vipc_frame_id = 0 - run_count = 0 - - model_transform_main = np.zeros((3, 3), dtype=np.float32) - model_transform_extra = np.zeros((3, 3), dtype=np.float32) - live_calib_seen = False - buf_main, buf_extra = None, None - meta_main = FrameMeta() - meta_extra = FrameMeta() - - - if demo: - CP = get_demo_car_params() - else: - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - cloudlog.info("modeld got CarParams: %s", CP.brand) - - # TODO this needs more thought, use .2s extra for now to estimate other delays - # TODO Move smooth seconds to action function - long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS - prev_action = log.ModelDataV2.Action() - - DH = DesireHelper() - - while True: - # Keep receiving frames until we are at least 1 frame ahead of previous extra frame - while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: - buf_main = vipc_client_main.recv() - meta_main = FrameMeta(vipc_client_main) - if buf_main is None: - break - - if buf_main is None: - cloudlog.debug("vipc_client_main no frame") - continue - - if use_extra_client: - # Keep receiving extra frames until frame id matches main camera - while True: - buf_extra = vipc_client_extra.recv() - meta_extra = FrameMeta(vipc_client_extra) - if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: - break - - if buf_extra is None: - cloudlog.debug("vipc_client_extra no frame") - continue - - if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: - cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\ - extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})") - - else: - # Use single camera - buf_extra = buf_main - meta_extra = meta_main - - sm.update(0) - desire = DH.desire - is_rhd = sm["driverMonitoringState"].isRHD - frame_id = sm["roadCameraState"].frameId - v_ego = max(sm["carState"].vEgo, 0.) - lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS - if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: - device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) - dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] - model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) - model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) - live_calib_seen = True - - traffic_convention = np.zeros(2) - traffic_convention[int(is_rhd)] = 1 - - vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - if desire >= 0 and desire < ModelConstants.DESIRE_LEN: - vec_desire[desire] = 1 - - # tracked dropped frames - vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) - frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) - if run_count < 10: # let frame drops warm up - frame_dropped_filter.x = 0. - frames_dropped = 0. - run_count = run_count + 1 - - frame_drop_ratio = frames_dropped / (1 + frames_dropped) - prepare_only = vipc_dropped_frames > 0 - if prepare_only: - cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") - - 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_pulse': vec_desire, - 'traffic_convention': traffic_convention, - } - - mt1 = time.perf_counter() - model_output = model.run(bufs, transforms, inputs, prepare_only) - mt2 = time.perf_counter() - model_execution_time = mt2 - mt1 - - if model_output is not None: - modelv2_send = messaging.new_message('modelV2') - drivingdata_send = messaging.new_message('drivingModelData') - posenet_send = messaging.new_message('cameraOdometry') - - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) - prev_action = action - fill_model_msg(drivingdata_send, modelv2_send, model_output, action, - publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, - frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) - - desire_state = modelv2_send.modelV2.meta.desireState - l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] - r_lane_change_prob = desire_state[log.Desire.laneChangeRight] - lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) - modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state - modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction - drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state - drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction - - fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) - pm.send('modelV2', modelv2_send) - pm.send('drivingModelData', drivingdata_send) - pm.send('cameraOdometry', posenet_send) - last_vipc_frame_id = meta_main.frame_id - - -if __name__ == "__main__": - try: - import argparse - parser = argparse.ArgumentParser() - parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') - args = parser.parse_args() - main(demo=args.demo) - except KeyboardInterrupt: - cloudlog.warning("got SIGINT") diff --git a/selfdrive/modeld/models/README.md b/selfdrive/modeld/models/README.md index 04b69c61c3d6d3..6b704cbfa89fe0 100644 --- a/selfdrive/modeld/models/README.md +++ b/selfdrive/modeld/models/README.md @@ -1,8 +1,8 @@ ## Neural networks in openpilot To view the architecture of the ONNX networks, you can use [netron](https://netron.app/) -## Driving Model (vision model + temporal policy model) -### Vision inputs (Full size: 799906 x float32) +## Supercombo +### Supercombo input format (Full size: 393738 x float32) * **image stream** * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256 * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256 @@ -15,21 +15,16 @@ To view the architecture of the ONNX networks, you can use [netron](https://netr * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2] * Channel 4 represents the half-res U channel * Channel 5 represents the half-res V channel -### Policy inputs * **desire** - * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8 + * one-hot encoded vector to command model to execute certain actions, bit only needs to be sent for 1 frame : 8 * **traffic convention** * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2 -* **lateral control params** - * speed and steering delay for predicting the desired curvature: 2 -* **previous desired curvatures** - * vector of previously predicted desired curvatures: 100 * 1 -* **feature buffer** - * a buffer of intermediate features including the current feature to form a 5 seconds temporal context (at 20FPS) : 100 * 512 +* **recurrent state** + * The recurrent state vector that is fed back into the GRU for temporal context : 512 -### Driving Model output format (Full size: XXX x float32) -Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in modeld. +### Supercombo output format (Full size: XXX x float32) +Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more. ## Driver Monitoring Model @@ -37,30 +32,28 @@ Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in * .dlc file is a pre-quantized model and only runs on qualcomm DSPs ### input format -* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format: - * full input size is 1440 * 960 = 1382400 - * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner) -* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs +* single image (640 * 320 * 3 in RGB): + * full input size is 6 * 640/2 * 320/2 = 307200 + * represented in YUV420 with 6 channels: + * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2] + * Channel 4 represents the half-res U channel + * Channel 5 represents the half-res V channel + * normalized, ranging from -1.0 to 1.0 ### output format -* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33)) - * for each person in the front seats (2 * 41) - * face pose: 12 = 6 + 6 - * face orientation [pitch, yaw, roll] in camera frame: 3 - * face position [dx, dy] relative to image center: 2 - * normalized face size: 1 - * standard deviations for above outputs: 6 - * face visible probability: 1 - * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1 - * eye position and size, and their standard deviations: 8 - * eye visible probability: 1 - * eye closed probability: 1 - * wearing sunglasses probability: 1 - * face occluded probability: 1 - * touching wheel probability: 1 - * paying attention probability: 1 - * (deprecated) distracted probabilities: 2 - * using phone probability: 1 - * distracted probability: 1 - * common outputs 1 - * left hand drive probability: 1 +* 39 x float32 outputs ([parsing example](https://github.com/commaai/openpilot/blob/master/selfdrive/modeld/models/dmonitoring.cc#L165)) + * face pose: 12 = 6 + 6 + * face orientation [pitch, yaw, roll] in camera frame: 3 + * face position [dx, dy] relative to image center: 2 + * normalized face size: 1 + * standard deviations for above outputs: 6 + * face visible probability: 1 + * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1 + * eye position and size, and their standard deviations: 8 + * eye visible probability: 1 + * eye closed probability: 1 + * wearing sunglasses probability: 1 + * poor camera vision probability: 1 + * face partially out-of-frame probability: 1 + * (deprecated) distracted probabilities: 2 + * face covered probability: 1 diff --git a/selfdrive/modeld/models/big_driving_policy.onnx b/selfdrive/modeld/models/big_driving_policy.onnx deleted file mode 120000 index e1b653a14a03d6..00000000000000 --- a/selfdrive/modeld/models/big_driving_policy.onnx +++ /dev/null @@ -1 +0,0 @@ -driving_policy.onnx \ No newline at end of file diff --git a/selfdrive/modeld/models/big_driving_vision.onnx b/selfdrive/modeld/models/big_driving_vision.onnx deleted file mode 120000 index 28ee71dd746e63..00000000000000 --- a/selfdrive/modeld/models/big_driving_vision.onnx +++ /dev/null @@ -1 +0,0 @@ -driving_vision.onnx \ No newline at end of file diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index d3341e76ec3669..b7c9051c6e9462 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -1,64 +1,72 @@ #include "selfdrive/modeld/models/commonmodel.h" +#include +#include #include #include #include "common/clutil.h" +#include "common/mat.h" +#include "common/timing.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip) : ModelFrame(device_id, context) { - input_frames = std::make_unique(buf_size); - temporal_skip = _temporal_skip; - input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); - img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (temporal_skip+1)*frame_size_bytes, NULL, &err)); - region.origin = temporal_skip * frame_size_bytes; - region.size = frame_size_bytes; - last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err)); +ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) { + input_frames = std::make_unique(buf_size); + q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); + y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_WIDTH * MODEL_HEIGHT, NULL, &err)); + u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err)); + v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err)); + net_input_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_FRAME_SIZE * sizeof(float), NULL, &err)); + + transform_init(&transform, context, device_id); loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT); - init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT); } -cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { - run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - - for (int i = 0; i < temporal_skip; i++) { - CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr)); - } - loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl); +float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) { + transform_queue(&this->transform, q, + yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset, + y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection); - copy_queue(&loadyuv, q, img_buffer_20hz_cl, input_frames_cl, 0, 0, frame_size_bytes); - copy_queue(&loadyuv, q, last_img_cl, input_frames_cl, 0, frame_size_bytes, frame_size_bytes); + if (output == NULL) { + loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl); - // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready. - clFinish(q); - return &input_frames_cl; + std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE); + CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr)); + clFinish(q); + return &input_frames[0]; + } else { + loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true); + // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready. + clFinish(q); + return NULL; + } } -DrivingModelFrame::~DrivingModelFrame() { - deinit_transform(); +ModelFrame::~ModelFrame() { + transform_destroy(&transform); loadyuv_destroy(&loadyuv); - CL_CHECK(clReleaseMemObject(input_frames_cl)); - CL_CHECK(clReleaseMemObject(img_buffer_20hz_cl)); - CL_CHECK(clReleaseMemObject(last_img_cl)); + CL_CHECK(clReleaseMemObject(net_input_cl)); + CL_CHECK(clReleaseMemObject(v_cl)); + CL_CHECK(clReleaseMemObject(u_cl)); + CL_CHECK(clReleaseMemObject(y_cl)); CL_CHECK(clReleaseCommandQueue(q)); } +void softmax(const float* input, float* output, size_t len) { + const float max_val = *std::max_element(input, input + len); + float denominator = 0; + for(int i = 0; i < len; i++) { + float const v_exp = expf(input[i] - max_val); + denominator += v_exp; + output[i] = v_exp; + } -MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) { - input_frames = std::make_unique(buf_size); - input_frame_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); - - init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT); -} - -cl_mem* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { - run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - clFinish(q); - return &y_cl; + const float inv_denominator = 1. / denominator; + for(int i = 0; i < len; i++) { + output[i] *= inv_denominator; + } } -MonitoringModelFrame::~MonitoringModelFrame() { - deinit_transform(); - CL_CHECK(clReleaseMemObject(input_frame_cl)); - CL_CHECK(clReleaseCommandQueue(q)); +float sigmoid(float input) { + return 1 / (1 + expf(-input)); } diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 176d7eb6dcf601..40c82a8c21b84f 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -2,7 +2,6 @@ #include #include -#include #include @@ -17,81 +16,26 @@ #include "selfdrive/modeld/transforms/loadyuv.h" #include "selfdrive/modeld/transforms/transform.h" -class ModelFrame { -public: - ModelFrame(cl_device_id device_id, cl_context context) { - q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); - } - virtual ~ModelFrame() {} - virtual cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { return NULL; } - uint8_t* buffer_from_cl(cl_mem *in_frames, int buffer_size) { - CL_CHECK(clEnqueueReadBuffer(q, *in_frames, CL_TRUE, 0, buffer_size, input_frames.get(), 0, nullptr, nullptr)); - clFinish(q); - return &input_frames[0]; - } - - int MODEL_WIDTH; - int MODEL_HEIGHT; - int MODEL_FRAME_SIZE; - int buf_size; +const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; -protected: - cl_mem y_cl, u_cl, v_cl; - Transform transform; - cl_command_queue q; - std::unique_ptr input_frames; - - void init_transform(cl_device_id device_id, cl_context context, int model_width, int model_height) { - y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, model_width * model_height, NULL, &err)); - u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err)); - v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err)); - transform_init(&transform, context, device_id); - } - - void deinit_transform() { - transform_destroy(&transform); - CL_CHECK(clReleaseMemObject(v_cl)); - CL_CHECK(clReleaseMemObject(u_cl)); - CL_CHECK(clReleaseMemObject(y_cl)); - } - - void run_transform(cl_mem yuv_cl, int model_width, int model_height, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { - transform_queue(&transform, q, - yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset, - y_cl, u_cl, v_cl, model_width, model_height, projection); - } -}; +void softmax(const float* input, float* output, size_t len); +float sigmoid(float input); -class DrivingModelFrame : public ModelFrame { +class ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip); - ~DrivingModelFrame(); - cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); + ModelFrame(cl_device_id device_id, cl_context context); + ~ModelFrame(); + float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; - const int buf_size = MODEL_FRAME_SIZE * 2; // 2 frames are temporal_skip frames apart - const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); + const int buf_size = MODEL_FRAME_SIZE * 2; private: + Transform transform; LoadYUVState loadyuv; - cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl; - cl_buffer_region region; - int temporal_skip; -}; - -class MonitoringModelFrame : public ModelFrame { -public: - MonitoringModelFrame(cl_device_id device_id, cl_context context); - ~MonitoringModelFrame(); - cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); - - const int MODEL_WIDTH = 1440; - const int MODEL_HEIGHT = 960; - const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT; - const int buf_size = MODEL_FRAME_SIZE; - -private: - cl_mem input_frame_cl; + cl_command_queue q; + cl_mem y_cl, u_cl, v_cl, net_input_cl; + std::unique_ptr input_frames; }; diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd deleted file mode 100644 index 4ac64d917205d3..00000000000000 --- a/selfdrive/modeld/models/commonmodel.pxd +++ /dev/null @@ -1,27 +0,0 @@ -# distutils: language = c++ - -from msgq.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem - -cdef extern from "common/mat.h": - cdef struct mat3: - float v[9] - -cdef extern from "common/clutil.h": - cdef unsigned long CL_DEVICE_TYPE_DEFAULT - cl_device_id cl_get_device_id(unsigned long) - cl_context cl_create_context(cl_device_id) - void cl_release_context(cl_context) - -cdef extern from "selfdrive/modeld/models/commonmodel.h": - cppclass ModelFrame: - int buf_size - unsigned char * buffer_from_cl(cl_mem*, int); - cl_mem * prepare(cl_mem, int, int, int, int, mat3) - - cppclass DrivingModelFrame: - int buf_size - DrivingModelFrame(cl_device_id, cl_context, int) - - cppclass MonitoringModelFrame: - int buf_size - MonitoringModelFrame(cl_device_id, cl_context) diff --git a/selfdrive/modeld/models/commonmodel_pyx.pxd b/selfdrive/modeld/models/commonmodel_pyx.pxd deleted file mode 100644 index 0bb798625be28d..00000000000000 --- a/selfdrive/modeld/models/commonmodel_pyx.pxd +++ /dev/null @@ -1,13 +0,0 @@ -# distutils: language = c++ - -from msgq.visionipc.visionipc cimport cl_mem -from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext - -cdef class CLContext(BaseCLContext): - pass - -cdef class CLMem: - cdef cl_mem * mem - - @staticmethod - cdef create(void*) diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx deleted file mode 100644 index 5b7d11bc71aa66..00000000000000 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ /dev/null @@ -1,74 +0,0 @@ -# distutils: language = c++ -# cython: c_string_encoding=ascii, language_level=3 - -import numpy as np -cimport numpy as cnp -from libc.string cimport memcpy -from libc.stdint cimport uintptr_t - -from msgq.visionipc.visionipc cimport cl_mem -from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext -from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context -from .commonmodel cimport mat3, ModelFrame as cppModelFrame, DrivingModelFrame as cppDrivingModelFrame, MonitoringModelFrame as cppMonitoringModelFrame - - -cdef class CLContext(BaseCLContext): - def __cinit__(self): - self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT) - self.context = cl_create_context(self.device_id) - - def __dealloc__(self): - if self.context: - cl_release_context(self.context) - -cdef class CLMem: - @staticmethod - cdef create(void * cmem): - mem = CLMem() - mem.mem = cmem - return mem - - @property - def mem_address(self): - return (self.mem) - -def cl_from_visionbuf(VisionBuf buf): - return CLMem.create(&buf.buf.buf_cl) - - -cdef class ModelFrame: - cdef cppModelFrame * frame - cdef int buf_size - - def __dealloc__(self): - del self.frame - - def prepare(self, VisionBuf buf, float[:] projection): - cdef mat3 cprojection - memcpy(cprojection.v, &projection[0], 9*sizeof(float)) - cdef cl_mem * data - data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection) - return CLMem.create(data) - - def buffer_from_cl(self, CLMem in_frames): - cdef unsigned char * data2 - data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size) - return np.asarray( data2) - - -cdef class DrivingModelFrame(ModelFrame): - cdef cppDrivingModelFrame * _frame - - def __cinit__(self, CLContext context, int temporal_skip): - self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip) - self.frame = (self._frame) - self.buf_size = self._frame.buf_size - -cdef class MonitoringModelFrame(ModelFrame): - cdef cppMonitoringModelFrame * _frame - - def __cinit__(self, CLContext context): - self._frame = new cppMonitoringModelFrame(context.device_id, context.context) - self.frame = (self._frame) - self.buf_size = self._frame.buf_size - diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc new file mode 100644 index 00000000000000..e7e6d466128987 --- /dev/null +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -0,0 +1,134 @@ +#include + +#include "libyuv.h" + +#include "common/mat.h" +#include "common/modeldata.h" +#include "common/params.h" +#include "common/timing.h" +#include "system/hardware/hw.h" + +#include "selfdrive/modeld/models/dmonitoring.h" + +constexpr int MODEL_WIDTH = 1440; +constexpr int MODEL_HEIGHT = 960; + +template +static inline T *get_buffer(std::vector &buf, const size_t size) { + if (buf.size() < size) buf.resize(size); + return buf.data(); +} + +void dmonitoring_init(DMonitoringModelState* s) { + +#ifdef USE_ONNX_MODEL + s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); +#else + s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); +#endif + + s->m->addCalib(s->calib, CALIB_LEN); +} + +void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) { + for (int i = 0; i < 3; ++i) { + ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE; + ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]); + } + for (int i = 0; i < 2; ++i) { + ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE; + ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]); + } + for (int i = 0; i < 4; ++i) { + ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]); + } + for (int i = 0; i < 2; ++i) { + ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]); + } + ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]); + ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]); + ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]); + ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]); + ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]); + ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]); + ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]); +} + +void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) { + ddata.setFaceOrientation(ds_res.face_orientation); + ddata.setFaceOrientationStd(ds_res.face_orientation_std); + ddata.setFacePosition(ds_res.face_position); + ddata.setFacePositionStd(ds_res.face_position_std); + ddata.setFaceProb(ds_res.face_prob); + ddata.setLeftEyeProb(ds_res.left_eye_prob); + ddata.setRightEyeProb(ds_res.right_eye_prob); + ddata.setLeftBlinkProb(ds_res.left_blink_prob); + ddata.setRightBlinkProb(ds_res.right_blink_prob); + ddata.setSunglassesProb(ds_res.sunglasses_prob); + ddata.setOccludedProb(ds_res.occluded_prob); + ddata.setReadyProb(ds_res.ready_prob); + ddata.setNotReadyProb(ds_res.not_ready_prob); +} + +DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) { + int v_off = height - MODEL_HEIGHT; + int h_off = (width - MODEL_WIDTH) / 2; + int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT; + + uint8_t *raw_buf = (uint8_t *) stream_buf; + // vertical crop free + uint8_t *raw_y_start = raw_buf + stride * v_off; + + uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); + + // here makes a uint8 copy + for (int r = 0; r < MODEL_HEIGHT; ++r) { + memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH); + } + + // printf("preprocess completed. %d \n", yuv_buf_len); + // FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); + // fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file); + // fclose(dump_yuv_file); + + double t1 = millis_since_boot(); + s->m->addImage((float*)net_input_buf, yuv_buf_len / 4); + for (int i = 0; i < CALIB_LEN; i++) { + s->calib[i] = calib[i]; + } + s->m->execute(); + double t2 = millis_since_boot(); + + DMonitoringModelResult model_res = {0}; + parse_driver_data(model_res.driver_state_lhd, s, 0); + parse_driver_data(model_res.driver_state_rhd, s, 41); + model_res.poor_vision_prob = sigmoid(s->output[82]); + model_res.wheel_on_right_prob = sigmoid(s->output[83]); + model_res.dsp_execution_time = (t2 - t1) / 1000.; + + return model_res; +} + +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred) { + // make msg + MessageBuilder msg; + auto framed = msg.initEvent().initDriverStateV2(); + framed.setFrameId(frame_id); + framed.setModelExecutionTime(execution_time); + framed.setDspExecutionTime(model_res.dsp_execution_time); + + framed.setPoorVisionProb(model_res.poor_vision_prob); + framed.setWheelOnRightProb(model_res.wheel_on_right_prob); + fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd); + fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd); + + if (send_raw_pred) { + framed.setRawPredictions(raw_pred.asBytes()); + } + + pm.send("driverStateV2", msg); +} + +void dmonitoring_free(DMonitoringModelState* s) { + delete s->m; +} diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h new file mode 100644 index 00000000000000..ae2bf053946861 --- /dev/null +++ b/selfdrive/modeld/models/dmonitoring.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include "cereal/messaging/messaging.h" +#include "common/util.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" + +#define CALIB_LEN 3 + +#define OUTPUT_SIZE 84 +#define REG_SCALE 0.25f + +typedef struct DriverStateResult { + float face_orientation[3]; + float face_orientation_std[3]; + float face_position[2]; + float face_position_std[2]; + float face_prob; + float left_eye_prob; + float right_eye_prob; + float left_blink_prob; + float right_blink_prob; + float sunglasses_prob; + float occluded_prob; + float ready_prob[4]; + float not_ready_prob[2]; +} DriverStateResult; + +typedef struct DMonitoringModelResult { + DriverStateResult driver_state_lhd; + DriverStateResult driver_state_rhd; + float poor_vision_prob; + float wheel_on_right_prob; + float dsp_execution_time; +} DMonitoringModelResult; + +typedef struct DMonitoringModelState { + RunModel *m; + float output[OUTPUT_SIZE]; + std::vector net_input_buf; + float calib[CALIB_LEN]; +} DMonitoringModelState; + +void dmonitoring_init(DMonitoringModelState* s); +DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib); +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred); +void dmonitoring_free(DMonitoringModelState* s); + diff --git a/selfdrive/modeld/models/dmonitoring_model.current b/selfdrive/modeld/models/dmonitoring_model.current new file mode 100644 index 00000000000000..d1e7d1136fa1e4 --- /dev/null +++ b/selfdrive/modeld/models/dmonitoring_model.current @@ -0,0 +1,2 @@ +ee8f830b-d6a1-42ef-9b1b-50fd0b2faae4 +cac8f7b69d420506707ff7a19d573d5011ef2533 \ No newline at end of file diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 4052a154818165..4cbd6bb7dd64b1 100644 --- a/selfdrive/modeld/models/dmonitoring_model.onnx +++ b/selfdrive/modeld/models/dmonitoring_model.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:35e4a5d4c4d481f915e42358af4665b2c92b8f5c1efd1c0731f21b876ad1d856 -size 6954249 +oid sha256:932e589e5cce66e5d9f48492426a33c74cd7f352a870d3ddafcede3e9156f30d +size 9157561 diff --git a/selfdrive/modeld/models/dmonitoring_model_q.dlc b/selfdrive/modeld/models/dmonitoring_model_q.dlc new file mode 100644 index 00000000000000..94632030edb80d --- /dev/null +++ b/selfdrive/modeld/models/dmonitoring_model_q.dlc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3587976a8b7e3be274fa86c2e2233e3e464cad713f5077c4394cd1ddd3c7c6c5 +size 2636965 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc new file mode 100644 index 00000000000000..8d02eb6b2f1d36 --- /dev/null +++ b/selfdrive/modeld/models/driving.cc @@ -0,0 +1,371 @@ +#include "selfdrive/modeld/models/driving.h" + +#include +#include + +#include +#include + +#include + +#include "common/clutil.h" +#include "common/params.h" +#include "common/timing.h" +#include "common/swaglog.h" + +constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15; +constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05; +constexpr float FCW_THRESHOLD_3MS2 = 0.7; + +std::array prev_brake_5ms2_probs = {0,0,0,0,0}; +std::array prev_brake_3ms2_probs = {0,0,0}; + +// #define DUMP_YUV + +template +constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { + return kj::ArrayPtr(arr.data(), arr.size()); +} + +void model_init(ModelState* s, cl_device_id device_id, cl_context context) { + s->frame = new ModelFrame(device_id, context); + s->wide_frame = new ModelFrame(device_id, context); + +#ifdef USE_THNEED + s->m = std::make_unique("models/supercombo.thneed", +#elif USE_ONNX_MODEL + s->m = std::make_unique("models/supercombo.onnx", +#else + s->m = std::make_unique("models/supercombo.dlc", +#endif + &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true, false, context); + +#ifdef TEMPORAL + s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); +#endif + +#ifdef DESIRE + s->m->addDesire(s->pulse_desire, DESIRE_LEN); +#endif + +#ifdef TRAFFIC_CONVENTION + s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); +#endif +} + +ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { +#ifdef DESIRE + if (desire_in != NULL) { + for (int i = 1; i < DESIRE_LEN; i++) { + // Model decides when action is completed + // so desire input is just a pulse triggered on rising edge + if (desire_in[i] - s->prev_desire[i] > .99) { + s->pulse_desire[i] = desire_in[i]; + } else { + s->pulse_desire[i] = 0.0; + } + s->prev_desire[i] = desire_in[i]; + } + } +#endif + + int rhd_idx = is_rhd; + s->traffic_convention[rhd_idx] = 1.0; + s->traffic_convention[1-rhd_idx] = 0.0; + + // if getInputBuf is not NULL, net_input_buf will be + auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, buf->stride, buf->uv_offset, transform, static_cast(s->m->getInputBuf())); + s->m->addImage(net_input_buf, s->frame->buf_size); + LOGT("Image added"); + + if (wbuf != nullptr) { + auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, wbuf->stride, wbuf->uv_offset, transform_wide, static_cast(s->m->getExtraBuf())); + s->m->addExtra(net_extra_buf, s->wide_frame->buf_size); + LOGT("Extra image added"); + } + + if (prepare_only) { + return nullptr; + } + + s->m->execute(); + LOGT("Execution finished"); + + return (ModelOutput*)&s->output; +} + +void model_free(ModelState* s) { + delete s->frame; + delete s->wide_frame; +} + +void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) { + std::array lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + const auto &best_prediction = leads.get_best_prediction(t_idx); + lead.setProb(sigmoid(leads.prob[t_idx])); + lead.setProbTime(prob_t); + std::array lead_x, lead_y, lead_v, lead_a; + std::array lead_x_std, lead_y_std, lead_v_std, lead_a_std; + for (int i=0; i desire_state_softmax; + softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN); + + std::array desire_pred_softmax; + for (int i=0; i lat_long_t = {2,4,6,8,10}; + std::array gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid, + brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid; + for (int i=0; i threshold; + } + for (int i=0; i FCW_THRESHOLD_3MS2; + } + + auto disengage = meta.initDisengagePredictions(); + disengage.setT(to_kj_array_ptr(lat_long_t)); + disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid)); + disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid)); + disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid)); + disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid)); + disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid)); + disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid)); + + meta.setEngagedProb(sigmoid(meta_data.engaged_prob)); + meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax)); + meta.setDesireState(to_kj_array_ptr(desire_state_softmax)); + meta.setHardBrakePredicted(above_fcw_threshold); +} + +template +void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array &t, + const std::array &x, const std::array &y, const std::array &z) { + xyzt.setT(to_kj_array_ptr(t)); + xyzt.setX(to_kj_array_ptr(x)); + xyzt.setY(to_kj_array_ptr(y)); + xyzt.setZ(to_kj_array_ptr(z)); +} + +template +void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array &t, + const std::array &x, const std::array &y, const std::array &z, + const std::array &x_std, const std::array &y_std, const std::array &z_std) { + fill_xyzt(xyzt, t, x, y, z); + xyzt.setXStd(to_kj_array_ptr(x_std)); + xyzt.setYStd(to_kj_array_ptr(y_std)); + xyzt.setZStd(to_kj_array_ptr(z_std)); +} + +void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) { + std::array pos_x, pos_y, pos_z; + std::array pos_x_std, pos_y_std, pos_z_std; + std::array vel_x, vel_y, vel_z; + std::array rot_x, rot_y, rot_z; + std::array acc_x, acc_y, acc_z; + std::array rot_rate_x, rot_rate_y, rot_rate_z; + + for(int i=0; i &plan_t, + const ModelOutputLaneLines &lanes) { + std::array left_far_y, left_far_z; + std::array left_near_y, left_near_z; + std::array right_near_y, right_near_z; + std::array right_far_y, right_far_z; + for (int j=0; j &plan_t, + const ModelOutputRoadEdges &edges) { + std::array left_y, left_z; + std::array right_y, right_z; + for (int j=0; j plan_t; + std::fill_n(plan_t.data(), plan_t.size(), NAN); + plan_t[0] = 0.0; + for (int xidx=1, tidx=0; xidx t_offsets = {0.0, 2.0, 4.0}; + for (int i=0; i raw_pred, const bool valid) { + const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + MessageBuilder msg; + auto framed = msg.initEvent(valid).initModelV2(); + framed.setFrameId(vipc_frame_id); + framed.setFrameIdExtra(vipc_frame_id_extra); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPredictions(raw_pred.asBytes()); + } + fill_model(framed, net_outputs); + pm.send("modelV2", msg); +} + +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, + const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid) { + MessageBuilder msg; + const auto &v_mean = net_outputs.pose.velocity_mean; + const auto &r_mean = net_outputs.pose.rotation_mean; + const auto &v_std = net_outputs.pose.velocity_std; + const auto &r_std = net_outputs.pose.rotation_std; + + auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry(); + posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z}); + posenetd.setRot({r_mean.x, r_mean.y, r_mean.z}); + posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)}); + posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)}); + + posenetd.setTimestampEof(timestamp_eof); + posenetd.setFrameId(vipc_frame_id); + + pm.send("cameraOdometry", msg); +} diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h new file mode 100644 index 00000000000000..e2ee812e44a0cf --- /dev/null +++ b/selfdrive/modeld/models/driving.h @@ -0,0 +1,277 @@ +#pragma once + +// gate this here +#define TEMPORAL +#define DESIRE +#define TRAFFIC_CONVENTION + +#include +#include + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_client.h" +#include "common/mat.h" +#include "common/modeldata.h" +#include "common/util.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" + +constexpr int DESIRE_LEN = 8; +constexpr int DESIRE_PRED_LEN = 4; +constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int MODEL_FREQ = 20; + +constexpr int DISENGAGE_LEN = 5; +constexpr int BLINKER_LEN = 6; +constexpr int META_STRIDE = 7; + +constexpr int PLAN_MHP_N = 5; +constexpr int STOP_LINE_MHP_N = 3; + +constexpr int LEAD_MHP_N = 2; +constexpr int LEAD_TRAJ_LEN = 6; +constexpr int LEAD_PRED_DIM = 4; +constexpr int LEAD_MHP_SELECTION = 3; + +struct ModelOutputXYZ { + float x; + float y; + float z; +}; +static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3); + +struct ModelOutputYZ { + float y; + float z; +}; +static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2); + +struct ModelOutputPlanElement { + ModelOutputXYZ position; + ModelOutputXYZ velocity; + ModelOutputXYZ acceleration; + ModelOutputXYZ rotation; + ModelOutputXYZ rotation_rate; +}; +static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5); + +struct ModelOutputPlanPrediction { + std::array mean; + std::array std; + float prob; +}; +static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float)); + +struct ModelOutputPlans { + std::array prediction; + + constexpr const ModelOutputPlanPrediction &get_best_prediction() const { + int max_idx = 0; + for (int i = 1; i < prediction.size(); i++) { + if (prediction[i].prob > prediction[max_idx].prob) { + max_idx = i; + } + } + return prediction[max_idx]; + } +}; +static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N); + +struct ModelOutputLinesXY { + std::array left_far; + std::array left_near; + std::array right_near; + std::array right_far; +}; +static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4); + +struct ModelOutputLineProbVal { + float val_deprecated; + float val; +}; +static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2); + +struct ModelOutputLinesProb { + ModelOutputLineProbVal left_far; + ModelOutputLineProbVal left_near; + ModelOutputLineProbVal right_near; + ModelOutputLineProbVal right_far; +}; +static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4); + +struct ModelOutputLaneLines { + ModelOutputLinesXY mean; + ModelOutputLinesXY std; + ModelOutputLinesProb prob; +}; +static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb)); + +struct ModelOutputEdgessXY { + std::array left; + std::array right; +}; +static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2); + +struct ModelOutputRoadEdges { + ModelOutputEdgessXY mean; + ModelOutputEdgessXY std; +}; +static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2)); + +struct ModelOutputLeadElement { + float x; + float y; + float velocity; + float acceleration; +}; +static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4); + +struct ModelOutputLeadPrediction { + std::array mean; + std::array std; + std::array prob; +}; +static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION)); + +struct ModelOutputLeads { + std::array prediction; + std::array prob; + + constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const { + int max_idx = 0; + for (int i = 1; i < prediction.size(); i++) { + if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) { + max_idx = i; + } + } + return prediction[max_idx]; + } +}; +static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION)); + +struct ModelOutputStopLineElement { + ModelOutputXYZ position; + ModelOutputXYZ rotation; + float speed; + float time; +}; +static_assert(sizeof(ModelOutputStopLineElement) == (sizeof(ModelOutputXYZ)*2 + sizeof(float)*2)); + +struct ModelOutputStopLinePrediction { + ModelOutputStopLineElement mean; + ModelOutputStopLineElement std; + float prob; +}; +static_assert(sizeof(ModelOutputStopLinePrediction) == (sizeof(ModelOutputStopLineElement)*2 + sizeof(float))); + +struct ModelOutputStopLines { + std::array prediction; + float prob; + + constexpr const ModelOutputStopLinePrediction &get_best_prediction(int t_idx) const { + int max_idx = 0; + for (int i = 1; i < prediction.size(); i++) { + if (prediction[i].prob > prediction[max_idx].prob) { + max_idx = i; + } + } + return prediction[max_idx]; + } +}; +static_assert(sizeof(ModelOutputStopLines) == (sizeof(ModelOutputStopLinePrediction)*STOP_LINE_MHP_N) + sizeof(float)); + +struct ModelOutputPose { + ModelOutputXYZ velocity_mean; + ModelOutputXYZ rotation_mean; + ModelOutputXYZ velocity_std; + ModelOutputXYZ rotation_std; +}; +static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4); + +struct ModelOutputDisengageProb { + float gas_disengage; + float brake_disengage; + float steer_override; + float brake_3ms2; + float brake_4ms2; + float brake_5ms2; + float gas_pressed; +}; +static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7); + +struct ModelOutputBlinkerProb { + float left; + float right; +}; +static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2); + +struct ModelOutputDesireProb { + union { + struct { + float none; + float turn_left; + float turn_right; + float lane_change_left; + float lane_change_right; + float keep_left; + float keep_right; + float null; + }; + struct { + std::array array; + }; + }; +}; +static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN); + +struct ModelOutputMeta { + ModelOutputDesireProb desire_state_prob; + float engaged_prob; + std::array disengage_prob; + std::array blinker_prob; + std::array desire_pred_prob; +}; +static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); + +struct ModelOutput { + const ModelOutputPlans plans; + const ModelOutputLaneLines lane_lines; + const ModelOutputRoadEdges road_edges; + const ModelOutputLeads leads; + const ModelOutputStopLines stop_lines; + const ModelOutputMeta meta; + const ModelOutputPose pose; +}; + +constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); +#ifdef TEMPORAL + constexpr int TEMPORAL_SIZE = 512; +#else + constexpr int TEMPORAL_SIZE = 0; +#endif +constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE; + +// TODO: convert remaining arrays to std::array and update model runners +struct ModelState { + ModelFrame *frame = nullptr; + ModelFrame *wide_frame = nullptr; + std::array output = {}; + std::unique_ptr m; +#ifdef DESIRE + float prev_desire[DESIRE_LEN] = {}; + float pulse_desire[DESIRE_LEN] = {}; +#endif +#ifdef TRAFFIC_CONVENTION + float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; +#endif +}; + +void model_init(ModelState* s, cl_device_id device_id, cl_context context); +ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only); +void model_free(ModelState* s); +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, + const ModelOutput &net_outputs, uint64_t timestamp_eof, + float model_execution_time, kj::ArrayPtr raw_pred, const bool valid); +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, + const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid); diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx deleted file mode 100644 index 611ae9fe85f837..00000000000000 --- a/selfdrive/modeld/models/driving_policy.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:78477124cbf3ffe30fa951ebada8410b43c4242c6054584d656f1d329b067e15 -size 14060847 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx deleted file mode 100644 index 6c9fc4c84d3632..00000000000000 --- a/selfdrive/modeld/models/driving_vision.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ee29ee5bce84d1ce23e9ff381280de9b4e4d96d2934cd751740354884e112c66 -size 46877473 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx new file mode 100644 index 00000000000000..7b11edbe0800ad --- /dev/null +++ b/selfdrive/modeld/models/supercombo.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:50c7fc8565ac69a4b9a0de122e961326820e78bf13659255a89d0ed04be030d5 +size 95167481 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py deleted file mode 100644 index 038f51ca9cf2f9..00000000000000 --- a/selfdrive/modeld/parse_model_outputs.py +++ /dev/null @@ -1,122 +0,0 @@ -import numpy as np -from openpilot.selfdrive.modeld.constants import ModelConstants - -def safe_exp(x, out=None): - # -11 is around 10**14, more causes float16 overflow - return np.exp(np.clip(x, -np.inf, 11), out=out) - -def sigmoid(x): - return 1. / (1. + safe_exp(-x)) - -def softmax(x, axis=-1): - x -= np.max(x, axis=axis, keepdims=True) - if x.dtype == np.float32 or x.dtype == np.float64: - safe_exp(x, out=x) - else: - x = safe_exp(x) - x /= np.sum(x, axis=axis, keepdims=True) - return x - -class Parser: - def __init__(self, ignore_missing=False): - self.ignore_missing = ignore_missing - - def check_missing(self, outs, name): - missing = name not in outs - if missing and not self.ignore_missing: - raise ValueError(f"Missing output {name}") - return missing - - def parse_categorical_crossentropy(self, name, outs, out_shape=None): - if self.check_missing(outs, name): - return - raw = outs[name] - if out_shape is not None: - raw = raw.reshape((raw.shape[0],) + out_shape) - outs[name] = softmax(raw, axis=-1) - - def parse_binary_crossentropy(self, name, outs): - if self.check_missing(outs, name): - return - raw = outs[name] - outs[name] = sigmoid(raw) - - def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None): - if self.check_missing(outs, name): - return - raw = outs[name] - raw = raw.reshape((raw.shape[0], max(in_N, 1), -1)) - - n_values = (raw.shape[2] - out_N)//2 - pred_mu = raw[:,:,:n_values] - pred_std = safe_exp(raw[:,:,n_values: 2*n_values]) - - if in_N > 1: - weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype) - for i in range(out_N): - weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1) - - if out_N == 1: - for fidx in range(weights.shape[0]): - idxs = np.argsort(weights[fidx][:,0])[::-1] - weights[fidx] = weights[fidx][idxs] - pred_mu[fidx] = pred_mu[fidx][idxs] - pred_std[fidx] = pred_std[fidx][idxs] - full_shape = tuple([raw.shape[0], in_N] + list(out_shape)) - outs[name + '_weights'] = weights - outs[name + '_hypotheses'] = pred_mu.reshape(full_shape) - outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape) - - pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) - pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) - for fidx in range(weights.shape[0]): - for hidx in range(out_N): - idxs = np.argsort(weights[fidx,:,hidx])[::-1] - pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]] - pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]] - else: - pred_mu_final = pred_mu - pred_std_final = pred_std - - if out_N > 1: - final_shape = tuple([raw.shape[0], out_N] + list(out_shape)) - else: - final_shape = tuple([raw.shape[0],] + list(out_shape)) - outs[name] = pred_mu_final.reshape(final_shape) - outs[name + '_stds'] = pred_std_final.reshape(final_shape) - - def is_mhp(self, outs, name, shape): - if self.check_missing(outs, name): - return False - if outs[name].shape[1] == 2 * shape: - return False - return True - - def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: - self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) - self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_binary_crossentropy('lane_lines_prob', outs) - self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) - self.parse_binary_crossentropy('meta', outs) - self.parse_binary_crossentropy('lead_prob', outs) - lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH) - lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0) - lead_out_shape = (ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) if lead_mhp else \ - (ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) - self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape) - return outs - - def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: - plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH) - plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0) - self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) - self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) - return outs - - def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: - outs = self.parse_vision_outputs(outs) - outs = self.parse_policy_outputs(outs) - return outs diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py new file mode 100755 index 00000000000000..ac7cc68814df22 --- /dev/null +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import os +import sys +import numpy as np + +os.environ["OMP_NUM_THREADS"] = "4" +os.environ["OMP_WAIT_POLICY"] = "PASSIVE" + +import onnxruntime as ort # pylint: disable=import-error + +def read(sz, tf8=False): + dd = [] + gt = 0 + szof = 1 if tf8 else 4 + while gt < sz * szof: + st = os.read(0, sz * szof - gt) + assert(len(st) > 0) + dd.append(st) + gt += len(st) + r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) + if tf8: + r = r / 255. + return r + +def write(d): + os.write(1, d.tobytes()) + +def run_loop(m, tf8_input=False): + ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] + keys = [x.name for x in m.get_inputs()] + + # run once to initialize CUDA provider + if "CUDAExecutionProvider" in m.get_providers(): + m.run(None, dict(zip(keys, [np.zeros(shp, dtype=np.float32) for shp in ishapes]))) + + print("ready to run onnx model", keys, ishapes, file=sys.stderr) + while 1: + inputs = [] + for k, shp in zip(keys, ishapes): + ts = np.product(shp) + #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) + inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) + ret = m.run(None, dict(zip(keys, inputs))) + #print(ret, file=sys.stderr) + for r in ret: + write(r) + + +if __name__ == "__main__": + print(sys.argv, file=sys.stderr) + print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr) + options = ort.SessionOptions() + options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL + if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: + provider = 'OpenVINOExecutionProvider' + elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: + options.intra_op_num_threads = 2 + provider = 'CUDAExecutionProvider' + else: + options.intra_op_num_threads = 2 + options.inter_op_num_threads = 8 + options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL + options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL + provider = 'CPUExecutionProvider' + + try: + print("Onnx selected provider: ", [provider], file=sys.stderr) + ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider]) + print("Onnx using ", ort_session.get_providers(), file=sys.stderr) + run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv)) + except KeyboardInterrupt: + pass diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc new file mode 100644 index 00000000000000..447d90fd7efeb5 --- /dev/null +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -0,0 +1,142 @@ +#include "selfdrive/modeld/runners/onnxmodel.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8, cl_context context) { + LOGD("loading model %s", path); + + output = _output; + output_size = _output_size; + use_extra = _use_extra; + use_tf8 = _use_tf8; + + int err = pipe(pipein); + assert(err == 0); + err = pipe(pipeout); + assert(err == 0); + + std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); + std::string onnx_runner = exe_dir + "/runners/onnx_runner.py"; + std::string tf8_arg = use_tf8 ? "--use_tf8" : ""; + + proc_pid = fork(); + if (proc_pid == 0) { + LOGD("spawning onnx process %s", onnx_runner.c_str()); + char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, (char*)tf8_arg.c_str(), nullptr}; + dup2(pipein[0], 0); + dup2(pipeout[1], 1); + close(pipein[0]); + close(pipein[1]); + close(pipeout[0]); + close(pipeout[1]); + execvp(onnx_runner.c_str(), argv); + } + + // parent + close(pipein[0]); + close(pipeout[1]); +} + +ONNXModel::~ONNXModel() { + close(pipein[1]); + close(pipeout[0]); + kill(proc_pid, SIGTERM); +} + +void ONNXModel::pwrite(float *buf, int size) { + char *cbuf = (char *)buf; + int tw = size*sizeof(float); + while (tw > 0) { + int err = write(pipein[1], cbuf, tw); + //printf("host write %d\n", err); + assert(err >= 0); + cbuf += err; + tw -= err; + } + LOGD("host write of size %d done", size); +} + +void ONNXModel::pread(float *buf, int size) { + char *cbuf = (char *)buf; + int tr = size*sizeof(float); + struct pollfd fds[1]; + fds[0].fd = pipeout[0]; + fds[0].events = POLLIN; + while (tr > 0) { + int err; + err = poll(fds, 1, 10000); // 10 second timeout + assert(err == 1 || (err == -1 && errno == EINTR)); + LOGD("host read remaining %d/%d poll %d", tr, size*sizeof(float), err); + err = read(pipeout[0], cbuf, tr); + assert(err > 0 || (err == 0 && errno == EINTR)); + cbuf += err; + tr -= err; + } + LOGD("host read done"); +} + +void ONNXModel::addRecurrent(float *state, int state_size) { + rnn_input_buf = state; + rnn_state_size = state_size; +} + +void ONNXModel::addDesire(float *state, int state_size) { + desire_input_buf = state; + desire_state_size = state_size; +} + +void ONNXModel::addTrafficConvention(float *state, int state_size) { + traffic_convention_input_buf = state; + traffic_convention_size = state_size; +} + +void ONNXModel::addCalib(float *state, int state_size) { + calib_input_buf = state; + calib_size = state_size; +} + +void ONNXModel::addImage(float *image_buf, int buf_size) { + image_input_buf = image_buf; + image_buf_size = buf_size; +} + +void ONNXModel::addExtra(float *image_buf, int buf_size) { + extra_input_buf = image_buf; + extra_buf_size = buf_size; +} + +void ONNXModel::execute() { + // order must be this + if (image_input_buf != NULL) { + pwrite(image_input_buf, image_buf_size); + } + if (extra_input_buf != NULL) { + pwrite(extra_input_buf, extra_buf_size); + } + if (desire_input_buf != NULL) { + pwrite(desire_input_buf, desire_state_size); + } + if (traffic_convention_input_buf != NULL) { + pwrite(traffic_convention_input_buf, traffic_convention_size); + } + if (calib_input_buf != NULL) { + pwrite(calib_input_buf, calib_size); + } + if (rnn_input_buf != NULL) { + pwrite(rnn_input_buf, rnn_state_size); + } + pread(output, output_size); +} + diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h new file mode 100644 index 00000000000000..d5b7bfecf0cc05 --- /dev/null +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "selfdrive/modeld/runners/runmodel.h" + +class ONNXModel : public RunModel { +public: + ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false, cl_context context = NULL); + ~ONNXModel(); + void addRecurrent(float *state, int state_size); + void addDesire(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); + void addCalib(float *state, int state_size); + void addImage(float *image_buf, int buf_size); + void addExtra(float *image_buf, int buf_size); + void execute(); +private: + int proc_pid; + + float *output; + size_t output_size; + + float *rnn_input_buf = NULL; + int rnn_state_size; + float *desire_input_buf = NULL; + int desire_state_size; + float *traffic_convention_input_buf = NULL; + int traffic_convention_size; + float *calib_input_buf = NULL; + int calib_size; + float *image_input_buf = NULL; + int image_buf_size; + bool use_tf8; + float *extra_input_buf = NULL; + int extra_buf_size; + bool use_extra; + + // pipe to communicate to keras subprocess + void pread(float *buf, int size); + void pwrite(float *buf, int size); + int pipein[2]; + int pipeout[2]; +}; + diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h new file mode 100644 index 00000000000000..c64f300fe2411b --- /dev/null +++ b/selfdrive/modeld/runners/run.h @@ -0,0 +1,10 @@ +#pragma once + +#include "runmodel.h" +#include "snpemodel.h" + +#if defined(USE_THNEED) +#include "thneedmodel.h" +#elif defined(USE_ONNX_MODEL) +#include "onnxmodel.h" +#endif diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h new file mode 100644 index 00000000000000..c6078114012586 --- /dev/null +++ b/selfdrive/modeld/runners/runmodel.h @@ -0,0 +1,16 @@ +#pragma once +#include "common/clutil.h" +class RunModel { +public: + virtual ~RunModel() {} + virtual void addRecurrent(float *state, int state_size) {} + virtual void addDesire(float *state, int state_size) {} + virtual void addTrafficConvention(float *state, int state_size) {} + virtual void addCalib(float *state, int state_size) {} + virtual void addImage(float *image_buf, int buf_size) {} + virtual void addExtra(float *image_buf, int buf_size) {} + virtual void execute() {} + virtual void* getInputBuf() { return nullptr; } + virtual void* getExtraBuf() { return nullptr; } +}; + diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc new file mode 100644 index 00000000000000..ff4adcd8d338b8 --- /dev/null +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -0,0 +1,199 @@ +#pragma clang diagnostic ignored "-Wexceptions" + +#include "selfdrive/modeld/runners/snpemodel.h" + +#include +#include +#include + +#include "common/util.h" +#include "common/timing.h" + +void PrintErrorStringAndExit() { + std::cerr << zdl::DlSystem::getLastErrorString() << std::endl; + std::exit(EXIT_FAILURE); +} + +SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8, cl_context context) { + output = loutput; + output_size = loutput_size; + use_extra = luse_extra; + use_tf8 = luse_tf8; +#ifdef QCOM2 + if (runtime==USE_GPU_RUNTIME) { + Runtime = zdl::DlSystem::Runtime_t::GPU; + } else if (runtime==USE_DSP_RUNTIME) { + Runtime = zdl::DlSystem::Runtime_t::DSP; + } else { + Runtime = zdl::DlSystem::Runtime_t::CPU; + } + assert(zdl::SNPE::SNPEFactory::isRuntimeAvailable(Runtime)); +#endif + model_data = util::read_file(path); + assert(model_data.size() > 0); + + // load model + std::unique_ptr container = zdl::DlContainer::IDlContainer::open((uint8_t*)model_data.data(), model_data.size()); + if (!container) { PrintErrorStringAndExit(); } + printf("loaded model with size: %lu\n", model_data.size()); + + // create model runner + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + while (!snpe) { +#ifdef QCOM2 + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(Runtime) + .setUseUserSuppliedBuffers(true) + .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE) + .build(); +#else + snpe = snpeBuilder.setOutputLayers({}) + .setUseUserSuppliedBuffers(true) + .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE) + .build(); +#endif + if (!snpe) std::cerr << zdl::DlSystem::getLastErrorString() << std::endl; + } + + // get input and output names + const auto &strListi_opt = snpe->getInputTensorNames(); + if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names"); + const auto &strListi = *strListi_opt; + //assert(strListi.size() == 1); + const char *input_tensor_name = strListi.at(0); + + const auto &strListo_opt = snpe->getOutputTensorNames(); + if (!strListo_opt) throw std::runtime_error("Error obtaining Output tensor names"); + const auto &strListo = *strListo_opt; + assert(strListo.size() == 1); + const char *output_tensor_name = strListo.at(0); + + printf("model: %s -> %s\n", input_tensor_name, output_tensor_name); + + zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; + zdl::DlSystem::UserBufferEncodingTf8 userBufferEncodingTf8(0, 1./255); // network takes 0-1 + zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); + size_t size_of_input = use_tf8 ? sizeof(uint8_t) : sizeof(float); + + // create input buffer + { + const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name); + const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt; + std::vector strides(bufferShape.rank()); + strides[strides.size() - 1] = size_of_input; + size_t product = 1; + for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i]; + size_t stride = strides[strides.size() - 1]; + for (size_t i = bufferShape.rank() - 1; i > 0; i--) { + stride *= bufferShape[i]; + strides[i-1] = stride; + } + printf("input product is %lu\n", product); + inputBuffer = ubFactory.createUserBuffer(NULL, + product*size_of_input, + strides, + use_tf8 ? (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingTf8 : (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingFloat); + + inputMap.add(input_tensor_name, inputBuffer.get()); + } + + if (use_extra) { + const char *extra_tensor_name = strListi.at(1); + const auto &extraDims_opt = snpe->getInputDimensions(extra_tensor_name); + const zdl::DlSystem::TensorShape& bufferShape = *extraDims_opt; + std::vector strides(bufferShape.rank()); + strides[strides.size() - 1] = sizeof(float); + size_t product = 1; + for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i]; + size_t stride = strides[strides.size() - 1]; + for (size_t i = bufferShape.rank() - 1; i > 0; i--) { + stride *= bufferShape[i]; + strides[i-1] = stride; + } + printf("extra product is %lu\n", product); + extraBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat); + + inputMap.add(extra_tensor_name, extraBuffer.get()); + } + + // create output buffer + { + const zdl::DlSystem::TensorShape& bufferShape = snpe->getInputOutputBufferAttributes(output_tensor_name)->getDims(); + if (output_size != 0) { + assert(output_size == bufferShape[1]); + } else { + output_size = bufferShape[1]; + } + + std::vector outputStrides = {output_size * sizeof(float), sizeof(float)}; + outputBuffer = ubFactory.createUserBuffer(output, output_size * sizeof(float), outputStrides, &userBufferEncodingFloat); + outputMap.add(output_tensor_name, outputBuffer.get()); + } + +#ifdef USE_THNEED + if (Runtime == zdl::DlSystem::Runtime_t::GPU) { + thneed.reset(new Thneed()); + } +#endif +} + +void SNPEModel::addRecurrent(float *state, int state_size) { + recurrent = state; + recurrent_size = state_size; + recurrentBuffer = this->addExtra(state, state_size, 3); +} + +void SNPEModel::addTrafficConvention(float *state, int state_size) { + trafficConvention = state; + trafficConventionBuffer = this->addExtra(state, state_size, 2); +} + +void SNPEModel::addDesire(float *state, int state_size) { + desire = state; + desireBuffer = this->addExtra(state, state_size, 1); +} + +void SNPEModel::addCalib(float *state, int state_size) { + calib = state; + calibBuffer = this->addExtra(state, state_size, 1); +} + +void SNPEModel::addImage(float *image_buf, int buf_size) { + input = image_buf; + input_size = buf_size; +} + +void SNPEModel::addExtra(float *image_buf, int buf_size) { + extra = image_buf; + extra_size = buf_size; +} + +std::unique_ptr SNPEModel::addExtra(float *state, int state_size, int idx) { + // get input and output names + const auto real_idx = idx + (use_extra ? 1 : 0); + const auto &strListi_opt = snpe->getInputTensorNames(); + if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names"); + const auto &strListi = *strListi_opt; + const char *input_tensor_name = strListi.at(real_idx); + printf("adding index %d: %s\n", real_idx, input_tensor_name); + + zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; + zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); + std::vector retStrides = {state_size * sizeof(float), sizeof(float)}; + auto ret = ubFactory.createUserBuffer(state, state_size * sizeof(float), retStrides, &userBufferEncodingFloat); + inputMap.add(input_tensor_name, ret.get()); + return ret; +} + +void SNPEModel::execute() { + bool ret = inputBuffer->setBufferAddress(input); + assert(ret == true); + if (use_extra) { + bool extra_ret = extraBuffer->setBufferAddress(extra); + assert(extra_ret == true); + } + if (!snpe->execute(inputMap, outputMap)) { + PrintErrorStringAndExit(); + } +} + diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h new file mode 100644 index 00000000000000..08ae16c2b1598f --- /dev/null +++ b/selfdrive/modeld/runners/snpemodel.h @@ -0,0 +1,80 @@ +#pragma once +#pragma clang diagnostic ignored "-Wdeprecated-declarations" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "runmodel.h" + +#define USE_CPU_RUNTIME 0 +#define USE_GPU_RUNTIME 1 +#define USE_DSP_RUNTIME 2 + +#ifdef USE_THNEED +#include "selfdrive/modeld/thneed/thneed.h" +#endif + +class SNPEModel : public RunModel { +public: + SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false, cl_context context = NULL); + void addRecurrent(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); + void addCalib(float *state, int state_size); + void addDesire(float *state, int state_size); + void addImage(float *image_buf, int buf_size); + void addExtra(float *image_buf, int buf_size); + void execute(); + +#ifdef USE_THNEED + std::unique_ptr thneed; + bool thneed_recorded = false; +#endif + +private: + std::string model_data; + +#ifdef QCOM2 + zdl::DlSystem::Runtime_t Runtime; +#endif + + // snpe model stuff + std::unique_ptr snpe; + + // snpe input stuff + zdl::DlSystem::UserBufferMap inputMap; + std::unique_ptr inputBuffer; + float *input; + size_t input_size; + bool use_tf8; + + // snpe output stuff + zdl::DlSystem::UserBufferMap outputMap; + std::unique_ptr outputBuffer; + float *output; + size_t output_size; + + // extra input stuff + std::unique_ptr extraBuffer; + float *extra; + size_t extra_size; + bool use_extra; + + // recurrent and desire + std::unique_ptr addExtra(float *state, int state_size, int idx); + float *recurrent; + size_t recurrent_size; + std::unique_ptr recurrentBuffer; + float *trafficConvention; + std::unique_ptr trafficConventionBuffer; + float *desire; + std::unique_ptr desireBuffer; + float *calib; + std::unique_ptr calibBuffer; +}; diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc new file mode 100644 index 00000000000000..67db01bb952cf1 --- /dev/null +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -0,0 +1,71 @@ +#include "selfdrive/modeld/runners/thneedmodel.h" + +#include + +ThneedModel::ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8, cl_context context) { + thneed = new Thneed(true, context); + thneed->load(path); + thneed->clexec(); + + recorded = false; + output = loutput; + use_extra = luse_extra; +} + +void ThneedModel::addRecurrent(float *state, int state_size) { + recurrent = state; +} + +void ThneedModel::addTrafficConvention(float *state, int state_size) { + trafficConvention = state; +} + +void ThneedModel::addDesire(float *state, int state_size) { + desire = state; +} + +void ThneedModel::addImage(float *image_input_buf, int buf_size) { + input = image_input_buf; +} + +void ThneedModel::addExtra(float *extra_input_buf, int buf_size) { + extra = extra_input_buf; +} + +void* ThneedModel::getInputBuf() { + if (use_extra && thneed->input_clmem.size() > 4) return &(thneed->input_clmem[4]); + else if (!use_extra && thneed->input_clmem.size() > 3) return &(thneed->input_clmem[3]); + else return nullptr; +} + +void* ThneedModel::getExtraBuf() { + if (thneed->input_clmem.size() > 3) return &(thneed->input_clmem[3]); + else return nullptr; +} + +void ThneedModel::execute() { + if (!recorded) { + thneed->record = true; + if (use_extra) { + float *inputs[5] = {recurrent, trafficConvention, desire, extra, input}; + thneed->copy_inputs(inputs); + } else { + float *inputs[4] = {recurrent, trafficConvention, desire, input}; + thneed->copy_inputs(inputs); + } + thneed->clexec(); + thneed->copy_output(output); + thneed->stop(); + + recorded = true; + } else { + if (use_extra) { + float *inputs[5] = {recurrent, trafficConvention, desire, extra, input}; + thneed->execute(inputs, output); + } else { + float *inputs[4] = {recurrent, trafficConvention, desire, input}; + thneed->execute(inputs, output); + } + } +} + diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h new file mode 100644 index 00000000000000..f3f34dc7f4bac4 --- /dev/null +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -0,0 +1,31 @@ +#pragma once + +#include "selfdrive/modeld/runners/runmodel.h" +#include "selfdrive/modeld/thneed/thneed.h" + +class ThneedModel : public RunModel { +public: + ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false, cl_context context = NULL); + void addRecurrent(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); + void addDesire(float *state, int state_size); + void addImage(float *image_buf, int buf_size); + void addExtra(float *image_buf, int buf_size); + void execute(); + void* getInputBuf(); + void* getExtraBuf(); +private: + Thneed *thneed = NULL; + bool recorded; + bool use_extra; + + float *input; + float *extra; + float *output; + + // recurrent and desire + float *recurrent; + float *trafficConvention; + float *desire; +}; + diff --git a/selfdrive/modeld/runners/tinygrad_helpers.py b/selfdrive/modeld/runners/tinygrad_helpers.py deleted file mode 100644 index 776381341cf373..00000000000000 --- a/selfdrive/modeld/runners/tinygrad_helpers.py +++ /dev/null @@ -1,8 +0,0 @@ - -from tinygrad.tensor import Tensor -from tinygrad.helpers import to_mv - -def qcom_tensor_from_opencl_address(opencl_address, shape, dtype): - cl_buf_desc_ptr = to_mv(opencl_address, 8).cast('Q')[0] - rawbuf_ptr = to_mv(cl_buf_desc_ptr, 0x100).cast('Q')[20] # offset 0xA0 is a raw gpu pointer. - return Tensor.from_blob(rawbuf_ptr, shape, dtype=dtype, device='QCOM') diff --git a/selfdrive/modeld/test/dmon_lag/repro.cc b/selfdrive/modeld/test/dmon_lag/repro.cc new file mode 100644 index 00000000000000..c4c1c65cbe86a7 --- /dev/null +++ b/selfdrive/modeld/test/dmon_lag/repro.cc @@ -0,0 +1,101 @@ +// clang++ -O2 repro.cc && ./a.out + +#include +#include +#include + +#include +#include +#include +#include +#include + +static inline double millis_since_boot() { + struct timespec t; + clock_gettime(CLOCK_BOOTTIME, &t); + return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6; +} + +#define MODEL_WIDTH 320 +#define MODEL_HEIGHT 640 + +// null function still breaks it +#define input_lambda(x) x + +// this is copied from models/dmonitoring.cc, and is the code that triggers the issue +void inner(uint8_t *resized_buf, float *net_input_buf) { + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + // one shot conversion, O(n) anyway + // yuvframe2tensor, normalize + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + // Y_ul + net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); + // Y_ur + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); + // Y_dl + net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); + // Y_dr + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); + // U + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); + // V + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); + } + } +} + +float trial() { + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v + + // allocate the buffers + uint8_t *resized_buf = (uint8_t*)malloc(resized_width*resized_height*3/2); + float *net_input_buf = (float*)malloc(yuv_buf_len*sizeof(float)); + printf("allocate -- %p 0x%x -- %p 0x%lx\n", resized_buf, resized_width*resized_height*3/2, net_input_buf, yuv_buf_len*sizeof(float)); + + // test for bad buffers + static int CNT = 20; + float avg = 0.0; + for (int i = 0; i < CNT; i++) { + double s4 = millis_since_boot(); + inner(resized_buf, net_input_buf); + double s5 = millis_since_boot(); + avg += s5-s4; + } + avg /= CNT; + + // once it's bad, it's reliably bad + if (avg > 10) { + printf("HIT %f\n", avg); + printf("BAD\n"); + + for (int i = 0; i < 200; i++) { + double s4 = millis_since_boot(); + inner(resized_buf, net_input_buf); + double s5 = millis_since_boot(); + printf("%.2f ", s5-s4); + } + printf("\n"); + + exit(0); + } + + // don't free so we get a different buffer each time + //free(resized_buf); + //free(net_input_buf); + + return avg; +} + +int main() { + while (true) { + float ret = trial(); + printf("got %f\n", ret); + } +} + diff --git a/selfdrive/modeld/test/snpe_benchmark/.gitignore b/selfdrive/modeld/test/snpe_benchmark/.gitignore new file mode 100644 index 00000000000000..d83a1b2ff5c336 --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/.gitignore @@ -0,0 +1 @@ +benchmark diff --git a/selfdrive/modeld/test/snpe_benchmark/benchmark.cc b/selfdrive/modeld/test/snpe_benchmark/benchmark.cc new file mode 100644 index 00000000000000..1e2072eea10fc4 --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/benchmark.cc @@ -0,0 +1,191 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p) { + return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec); +} + +void PrintErrorStringAndExit() { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + std::exit(EXIT_FAILURE); +} + + +zdl::DlSystem::Runtime_t checkRuntime() { + static zdl::DlSystem::Version_t Version = zdl::SNPE::SNPEFactory::getLibraryVersion(); + static zdl::DlSystem::Runtime_t Runtime; + std::cout << "SNPE Version: " << Version.asString().c_str() << std::endl; //Print Version number + if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::DSP)) { + std::cout << "Using DSP runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::DSP; + } else if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::GPU)) { + std::cout << "Using GPU runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::GPU; + } else { + std::cout << "Using cpu runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::CPU; + } + return Runtime; +} + +void test(char *filename) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(filename); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + + cout << "get input tensor names done" << endl; + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + vector > inputs; + for (int i = 0; i < strList.size(); i++) { + cout << "input name: " << strList.at(i) << endl; + + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(i)); + const auto &inputShape = *inputDims_opt; + inputs.push_back(zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape)); + inputTensorMap.add(strList.at(i), inputs[i].get()); + } + + struct timespec start, end; + cout << "**** starting benchmark ****" << endl; + for (int i = 0; i < 50; i++) { + clock_gettime(CLOCK_MONOTONIC, &start); + int err = snpe->execute(inputTensorMap, outputTensorMap); + assert(err == true); + clock_gettime(CLOCK_MONOTONIC, &end); + uint64_t timeElapsed = timespecDiff(&end, &start); + printf("time: %f ms\n", timeElapsed*1.0/1e6); + } +} + +void get_testframe(int index, std::unique_ptr &input) { + FILE * pFile; + string filepath="/data/ipt/quantize_samples/sample_input_"+std::to_string(index); + pFile = fopen(filepath.c_str(),"rb"); + int length = 1*6*160*320*4; + float * frame_buffer = new float[length/4]; // 32/8 + fread(frame_buffer, length, 1, pFile); + // std::cout << *(frame_buffer+length/4-1) << std::endl; + std::copy(frame_buffer, frame_buffer+(length/4), input->begin()); +} + +void SaveITensor(const std::string& path, const zdl::DlSystem::ITensor* tensor) +{ + std::ofstream os(path, std::ofstream::binary); + if (!os) + { + std::cerr << "Failed to open output file for writing: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + for ( auto it = tensor->cbegin(); it != tensor->cend(); ++it ) + { + float f = *it; + if (!os.write(reinterpret_cast(&f), sizeof(float))) + { + std::cerr << "Failed to write data to: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + } +} + +void testrun(char* modelfile) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(modelfile); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + cout << "get input tensor names done" << endl; + + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + + assert (strList.size() == 1); + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(0)); + const auto &inputShape = *inputDims_opt; + std::cout << "winkwink" << std::endl; + + for (int i=0;i<10000;i++) { + std::unique_ptr input; + input = zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape); + get_testframe(i,input); + snpe->execute(input.get(), outputTensorMap); + zdl::DlSystem::StringList tensorNames = outputTensorMap.getTensorNames(); + std::for_each( tensorNames.begin(), tensorNames.end(), [&](const char* name) { + std::ostringstream path; + path << "/data/opt/Result_" << std::to_string(i) << ".raw"; + auto tensorPtr = outputTensorMap.getTensor(name); + SaveITensor(path.str(), tensorPtr); + }); + } +} + +int main(int argc, char* argv[]) { + if (argc < 2) { + printf("usage: %s \n", argv[0]); + return -1; + } + + if (argc == 2) { + while (true) test(argv[1]); + } else if (argc == 3) { + testrun(argv[1]); + } + return 0; +} + diff --git a/selfdrive/modeld/test/snpe_benchmark/benchmark.sh b/selfdrive/modeld/test/snpe_benchmark/benchmark.sh new file mode 100755 index 00000000000000..a9d3f79786a2b0 --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/benchmark.sh @@ -0,0 +1,4 @@ +#!/bin/sh -e +clang++ -I /data/openpilot/third_party/snpe/include/ -L/data/pythonpath/third_party/snpe/aarch64 -lSNPE benchmark.cc -o benchmark +export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$HOME/openpilot/third_party/snpe/x86_64/:$LD_LIBRARY_PATH" +exec ./benchmark $1 diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py new file mode 100755 index 00000000000000..2fcff785a9dcae --- /dev/null +++ b/selfdrive/modeld/test/test_modeld.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +import time +import unittest +import numpy as np +import random + +import cereal.messaging as messaging +from cereal.visionipc import VisionIpcServer, VisionStreamType +from common.transformations.camera import tici_f_frame_size +from common.realtime import DT_MDL +from selfdrive.manager.process_config import managed_processes + + +VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, + "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD} + +IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8) +IMG_BYTES = IMG.flatten().tobytes() + +class TestModeld(unittest.TestCase): + + def setUp(self): + self.vipc_server = VisionIpcServer("camerad") + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) + self.vipc_server.start_listener() + + self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) + self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan']) + + managed_processes['modeld'].start() + time.sleep(0.2) + self.sm.update(1000) + + def tearDown(self): + managed_processes['modeld'].stop() + del self.vipc_server + + def _send_frames(self, frame_id, cams=None): + if cams is None: + cams = ('roadCameraState', 'wideRoadCameraState') + + cs = None + for cam in cams: + msg = messaging.new_message(cam) + cs = getattr(msg, cam) + cs.frameId = frame_id + cs.timestampSof = int((frame_id * DT_MDL) * 1e9) + cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9)) + + self.pm.send(msg.which(), msg) + self.vipc_server.send(VIPC_STREAM[msg.which()], IMG_BYTES, cs.frameId, + cs.timestampSof, cs.timestampEof) + return cs + + def _wait(self): + self.sm.update(5000) + if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId: + self.sm.update(1000) + + def test_modeld(self): + for n in range(1, 500): + cs = self._send_frames(n) + self._wait() + + mdl = self.sm['modelV2'] + self.assertEqual(mdl.frameId, n) + self.assertEqual(mdl.frameIdExtra, n) + self.assertEqual(mdl.timestampEof, cs.timestampEof) + self.assertEqual(mdl.frameAge, 0) + self.assertEqual(mdl.frameDropPerc, 0) + + odo = self.sm['cameraOdometry'] + self.assertEqual(odo.frameId, n) + self.assertEqual(odo.timestampEof, cs.timestampEof) + + def test_dropped_frames(self): + """ + modeld should only run on consecutive road frames + """ + frame_id = -1 + road_frames = list() + for n in range(1, 50): + if (random.random() < 0.1) and n > 3: + cams = random.choice([(), ('wideRoadCameraState', )]) + self._send_frames(n, cams) + else: + self._send_frames(n) + road_frames.append(n) + self._wait() + + if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1: + frame_id = road_frames[-1] + + mdl = self.sm['modelV2'] + odo = self.sm['cameraOdometry'] + self.assertEqual(mdl.frameId, frame_id) + self.assertEqual(mdl.frameIdExtra, frame_id) + self.assertEqual(odo.frameId, frame_id) + if n != frame_id: + self.assertFalse(self.sm.updated['modelV2']) + self.assertFalse(self.sm.updated['cameraOdometry']) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/modeld/test/tf_test/build.sh b/selfdrive/modeld/test/tf_test/build.sh new file mode 100755 index 00000000000000..4e92ca06981999 --- /dev/null +++ b/selfdrive/modeld/test/tf_test/build.sh @@ -0,0 +1,2 @@ +#!/bin/bash +clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow diff --git a/selfdrive/modeld/test/tf_test/main.cc b/selfdrive/modeld/test/tf_test/main.cc new file mode 100644 index 00000000000000..db1ef3d67edbec --- /dev/null +++ b/selfdrive/modeld/test/tf_test/main.cc @@ -0,0 +1,69 @@ +#include +#include +#include +#include "tensorflow/c/c_api.h" + +void* read_file(const char* path, size_t* out_len) { + FILE* f = fopen(path, "r"); + if (!f) { + return NULL; + } + fseek(f, 0, SEEK_END); + long f_len = ftell(f); + rewind(f); + + char* buf = (char*)calloc(f_len, 1); + assert(buf); + + size_t num_read = fread(buf, f_len, 1, f); + fclose(f); + + if (num_read != 1) { + free(buf); + return NULL; + } + + if (out_len) { + *out_len = f_len; + } + + return buf; +} + +static void DeallocateBuffer(void* data, size_t) { + free(data); +} + +int main(int argc, char* argv[]) { + TF_Buffer* buf; + TF_Graph* graph; + TF_Status* status; + char *path = argv[1]; + + // load model + { + size_t model_size; + char tmp[1024]; + snprintf(tmp, sizeof(tmp), "%s.pb", path); + printf("loading model %s\n", tmp); + uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size); + buf = TF_NewBuffer(); + buf->data = model_data; + buf->length = model_size; + buf->data_deallocator = DeallocateBuffer; + printf("loaded model of size %d\n", model_size); + } + + // import graph + status = TF_NewStatus(); + graph = TF_NewGraph(); + TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions(); + TF_GraphImportGraphDef(graph, buf, opts, status); + TF_DeleteImportGraphDefOptions(opts); + TF_DeleteBuffer(buf); + if (TF_GetCode(status) != TF_OK) { + printf("FAIL: %s\n", TF_Message(status)); + } else { + printf("SUCCESS\n"); + } +} diff --git a/selfdrive/modeld/test/tf_test/pb_loader.py b/selfdrive/modeld/test/tf_test/pb_loader.py new file mode 100755 index 00000000000000..78fd33aef222cb --- /dev/null +++ b/selfdrive/modeld/test/tf_test/pb_loader.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 +import sys +import tensorflow as tf # pylint: disable=import-error + +with open(sys.argv[1], "rb") as f: + graph_def = tf.compat.v1.GraphDef() + graph_def.ParseFromString(f.read()) + #tf.io.write_graph(graph_def, '', sys.argv[1]+".try") diff --git a/selfdrive/modeld/test/timing/benchmark.py b/selfdrive/modeld/test/timing/benchmark.py new file mode 100755 index 00000000000000..3c7ce5b70a8c4d --- /dev/null +++ b/selfdrive/modeld/test/timing/benchmark.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +# type: ignore +# pylint: skip-file + +import os +import time +import numpy as np + +import cereal.messaging as messaging +from selfdrive.manager.process_config import managed_processes + + +N = int(os.getenv("N", "5")) +TIME = int(os.getenv("TIME", "30")) + +if __name__ == "__main__": + sock = messaging.sub_sock('modelV2', conflate=False, timeout=1000) + + execution_times = [] + + for _ in range(N): + os.environ['LOGPRINT'] = 'debug' + managed_processes['modeld'].start() + time.sleep(5) + + t = [] + start = time.monotonic() + while time.monotonic() - start < TIME: + msgs = messaging.drain_sock(sock, wait_for_one=True) + for m in msgs: + t.append(m.modelV2.modelExecutionTime) + + execution_times.append(np.array(t[10:]) * 1000) + managed_processes['modeld'].stop() + + print("\n\n") + print(f"ran modeld {N} times for {TIME}s each") + for n, t in enumerate(execution_times): + print(f"\tavg: {sum(t)/len(t):0.2f}ms, min: {min(t):0.2f}ms, max: {max(t):0.2f}ms") + print("\n\n") diff --git a/selfdrive/modeld/thneed/README b/selfdrive/modeld/thneed/README new file mode 100644 index 00000000000000..f3bc66d8fc26ff --- /dev/null +++ b/selfdrive/modeld/thneed/README @@ -0,0 +1,8 @@ +thneed is an SNPE accelerator. I know SNPE is already an accelerator, but sometimes things need to go even faster.. + +It runs on the local device, and caches a single model run. Then it replays it, but fast. + +thneed slices through abstraction layers like a fish. + +You need a thneed. + diff --git a/selfdrive/modeld/thneed/__init__.py b/selfdrive/modeld/thneed/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/third_party/linux/include/msm_kgsl.h b/selfdrive/modeld/thneed/include/msm_kgsl.h similarity index 100% rename from third_party/linux/include/msm_kgsl.h rename to selfdrive/modeld/thneed/include/msm_kgsl.h diff --git a/selfdrive/modeld/thneed/lib.py b/selfdrive/modeld/thneed/lib.py new file mode 100644 index 00000000000000..38ef3f4262ad9d --- /dev/null +++ b/selfdrive/modeld/thneed/lib.py @@ -0,0 +1,31 @@ +import struct, json + +def load_thneed(fn): + with open(fn, "rb") as f: + json_len = struct.unpack("I", f.read(4))[0] + jdat = json.loads(f.read(json_len).decode('latin_1')) + weights = f.read() + ptr = 0 + for o in jdat['objects']: + if o['needs_load']: + nptr = ptr + o['size'] + o['data'] = weights[ptr:nptr] + ptr = nptr + for o in jdat['binaries']: + nptr = ptr + o['length'] + o['data'] = weights[ptr:nptr] + ptr = nptr + return jdat + +def save_thneed(jdat, fn): + new_weights = [] + for o in jdat['objects'] + jdat['binaries']: + if 'data' in o: + new_weights.append(o['data']) + del o['data'] + new_weights_bytes = b''.join(new_weights) + with open(fn, "wb") as f: + j = json.dumps(jdat, ensure_ascii=False).encode('latin_1') + f.write(struct.pack("I", len(j))) + f.write(j) + f.write(new_weights_bytes) diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc new file mode 100644 index 00000000000000..f789e5bf577f6d --- /dev/null +++ b/selfdrive/modeld/thneed/serialize.cc @@ -0,0 +1,154 @@ +#include +#include + +#include "json11.hpp" +#include "common/util.h" +#include "common/clutil.h" +#include "selfdrive/modeld/thneed/thneed.h" +using namespace json11; + +extern map g_program_source; + +void Thneed::load(const char *filename) { + printf("Thneed::load: loading from %s\n", filename); + + string buf = util::read_file(filename); + int jsz = *(int *)buf.data(); + string jsonerr; + string jj(buf.data() + sizeof(int), jsz); + Json jdat = Json::parse(jj, jsonerr); + + map real_mem; + real_mem[NULL] = NULL; + + int ptr = sizeof(int)+jsz; + for (auto &obj : jdat["objects"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + cl_mem clbuf = NULL; + + if (mobj["buffer_id"].string_value().size() > 0) { + // image buffer must already be allocated + clbuf = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + assert(mobj["needs_load"].bool_value() == false); + } else { + if (mobj["needs_load"].bool_value()) { + clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, &buf[ptr], NULL); + if (debug >= 1) printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr); + ptr += sz; + } else { + // TODO: is there a faster way to init zeroed out buffers? + void *host_zeros = calloc(sz, 1); + clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, host_zeros, NULL); + free(host_zeros); + } + } + assert(clbuf != NULL); + + if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") { + cl_image_desc desc = {0}; + desc.image_type = (mobj["arg_type"] == "image2d_t") ? CL_MEM_OBJECT_IMAGE2D : CL_MEM_OBJECT_IMAGE1D_BUFFER; + desc.image_width = mobj["width"].int_value(); + desc.image_height = mobj["height"].int_value(); + desc.image_row_pitch = mobj["row_pitch"].int_value(); + assert(sz == desc.image_height*desc.image_row_pitch); +#ifdef QCOM2 + desc.buffer = clbuf; +#else + // TODO: we are creating unused buffers on PC + clReleaseMemObject(clbuf); +#endif + cl_image_format format = {0}; + format.image_channel_order = CL_RGBA; + format.image_channel_data_type = mobj["float32"].bool_value() ? CL_FLOAT : CL_HALF_FLOAT; + + cl_int errcode; + +#ifndef QCOM2 + if (mobj["needs_load"].bool_value()) { + clbuf = clCreateImage(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, &format, &desc, &buf[ptr-sz], &errcode); + } else { + clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode); + } +#else + clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode); +#endif + if (clbuf == NULL) { + printf("clError: %s create image %zux%zu rp %zu with buffer %p\n", cl_get_error_string(errcode), + desc.image_width, desc.image_height, desc.image_row_pitch, desc.buffer + ); + } + assert(clbuf != NULL); + } + + real_mem[*(cl_mem*)(mobj["id"].string_value().data())] = clbuf; + } + + map g_programs; + for (const auto &[name, source] : jdat["programs"].object_items()) { + if (debug >= 1) printf("building %s with size %zu\n", name.c_str(), source.string_value().size()); + g_programs[name] = cl_program_from_source(context, device_id, source.string_value()); + } + + for (auto &obj : jdat["inputs"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + cl_mem aa = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + input_clmem.push_back(aa); + input_sizes.push_back(sz); + printf("Thneed::load: adding input %s with size %d\n", mobj["name"].string_value().data(), sz); + + cl_int cl_err; + void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &cl_err); + if (cl_err != CL_SUCCESS) printf("clError: %s map %p %d\n", cl_get_error_string(cl_err), aa, sz); + assert(cl_err == CL_SUCCESS); + inputs.push_back(ret); + } + + for (auto &obj : jdat["outputs"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + printf("Thneed::save: adding output with size %d\n", sz); + // TODO: support multiple outputs + output = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + assert(output != NULL); + } + + for (auto &obj : jdat["binaries"].array_items()) { + string name = obj["name"].string_value(); + size_t length = obj["length"].int_value(); + if (debug >= 1) printf("binary %s with size %zu\n", name.c_str(), length); + g_programs[name] = cl_program_from_binary(context, device_id, (const uint8_t*)&buf[ptr], length); + ptr += length; + } + + for (auto &obj : jdat["kernels"].array_items()) { + auto gws = obj["global_work_size"]; + auto lws = obj["local_work_size"]; + auto kk = shared_ptr(new CLQueuedKernel(this)); + + kk->name = obj["name"].string_value(); + kk->program = g_programs[kk->name]; + kk->work_dim = obj["work_dim"].int_value(); + for (int i = 0; i < kk->work_dim; i++) { + kk->global_work_size[i] = gws[i].int_value(); + kk->local_work_size[i] = lws[i].int_value(); + } + kk->num_args = obj["num_args"].int_value(); + for (int i = 0; i < kk->num_args; i++) { + string arg = obj["args"].array_items()[i].string_value(); + int arg_size = obj["args_size"].array_items()[i].int_value(); + kk->args_size.push_back(arg_size); + if (arg_size == 8) { + cl_mem val = *(cl_mem*)(arg.data()); + val = real_mem[val]; + kk->args.push_back(string((char*)&val, sizeof(val))); + } else { + kk->args.push_back(arg); + } + } + kq.push_back(kk); + } + + clFinish(command_queue); +} diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h new file mode 100644 index 00000000000000..65475ccf7f3366 --- /dev/null +++ b/selfdrive/modeld/thneed/thneed.h @@ -0,0 +1,133 @@ +#pragma once + +#ifndef __user +#define __user __attribute__(()) +#endif + +#include +#include +#include +#include +#include + +#include + +#include "selfdrive/modeld/thneed/include/msm_kgsl.h" + +using namespace std; + +cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value); + +namespace json11 { + class Json; +} +class Thneed; + +class GPUMalloc { + public: + GPUMalloc(int size, int fd); + ~GPUMalloc(); + void *alloc(int size); + private: + uint64_t base; + int remaining; +}; + +class CLQueuedKernel { + public: + CLQueuedKernel(Thneed *lthneed) { thneed = lthneed; } + CLQueuedKernel(Thneed *lthneed, + cl_kernel _kernel, + cl_uint _work_dim, + const size_t *_global_work_size, + const size_t *_local_work_size); + cl_int exec(); + void debug_print(bool verbose); + int get_arg_num(const char *search_arg_name); + cl_program program; + string name; + cl_uint num_args; + vector arg_names; + vector arg_types; + vector args; + vector args_size; + cl_kernel kernel = NULL; + json11::Json to_json() const; + + cl_uint work_dim; + size_t global_work_size[3] = {0}; + size_t local_work_size[3] = {0}; + private: + Thneed *thneed; +}; + +class CachedIoctl { + public: + virtual void exec() {} +}; + +class CachedSync: public CachedIoctl { + public: + CachedSync(Thneed *lthneed, string ldata) { thneed = lthneed; data = ldata; } + void exec(); + private: + Thneed *thneed; + string data; +}; + +class CachedCommand: public CachedIoctl { + public: + CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd); + void exec(); + private: + void disassemble(int cmd_index); + struct kgsl_gpu_command cache; + unique_ptr cmds; + unique_ptr objs; + Thneed *thneed; + vector > kq; +}; + +class Thneed { + public: + Thneed(bool do_clinit=false, cl_context _context = NULL); + void stop(); + void execute(float **finputs, float *foutput, bool slow=false); + void wait(); + + vector input_clmem; + vector inputs; + vector input_sizes; + cl_mem output = NULL; + + cl_context context = NULL; + cl_command_queue command_queue; + cl_device_id device_id; + int context_id; + + // protected? + bool record = false; + int debug; + int timestamp; + +#ifdef QCOM2 + unique_ptr ram; + vector > cmds; + int fd; +#endif + + // all CL kernels + void copy_inputs(float **finputs, bool internal=false); + void copy_output(float *foutput); + cl_int clexec(); + vector > kq; + + // pending CL kernels + vector > ckq; + + // loading + void load(const char *filename); + private: + void clinit(); +}; + diff --git a/selfdrive/modeld/thneed/thneed_common.cc b/selfdrive/modeld/thneed/thneed_common.cc new file mode 100644 index 00000000000000..21170b13a65712 --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_common.cc @@ -0,0 +1,216 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include +#include +#include + +#include "common/clutil.h" +#include "common/timing.h" + +map, string> g_args; +map, int> g_args_size; +map g_program_source; + +void Thneed::stop() { + printf("Thneed::stop: recorded %lu commands\n", cmds.size()); + record = false; +} + +void Thneed::clinit() { + device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + if (context == NULL) context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); + //cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0}; + cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0}; + command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); + printf("Thneed::clinit done\n"); +} + +cl_int Thneed::clexec() { + if (debug >= 1) printf("Thneed::clexec: running %lu queued kernels\n", kq.size()); + for (auto &k : kq) { + if (record) ckq.push_back(k); + cl_int ret = k->exec(); + assert(ret == CL_SUCCESS); + } + return clFinish(command_queue); +} + +void Thneed::copy_inputs(float **finputs, bool internal) { + for (int idx = 0; idx < inputs.size(); ++idx) { + if (debug >= 1) printf("copying %lu -- %p -> %p (cl %p)\n", input_sizes[idx], finputs[idx], inputs[idx], input_clmem[idx]); + + if (internal) { + // if it's internal, using memcpy is fine since the buffer sync is cached in the ioctl layer + if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]); + } else { + if (finputs[idx] != NULL) CL_CHECK(clEnqueueWriteBuffer(command_queue, input_clmem[idx], CL_TRUE, 0, input_sizes[idx], finputs[idx], 0, NULL, NULL)); + } + } +} + +void Thneed::copy_output(float *foutput) { + if (output != NULL) { + size_t sz; + clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput); + CL_CHECK(clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL)); + } else { + printf("CAUTION: model output is NULL, does it have no outputs?\n"); + } +} + +// *********** CLQueuedKernel *********** + +CLQueuedKernel::CLQueuedKernel(Thneed *lthneed, + cl_kernel _kernel, + cl_uint _work_dim, + const size_t *_global_work_size, + const size_t *_local_work_size) { + thneed = lthneed; + kernel = _kernel; + work_dim = _work_dim; + assert(work_dim <= 3); + for (int i = 0; i < work_dim; i++) { + global_work_size[i] = _global_work_size[i]; + local_work_size[i] = _local_work_size[i]; + } + + char _name[0x100]; + clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(_name), _name, NULL); + name = string(_name); + clGetKernelInfo(kernel, CL_KERNEL_NUM_ARGS, sizeof(num_args), &num_args, NULL); + + // get args + for (int i = 0; i < num_args; i++) { + char arg_name[0x100] = {0}; + clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); + arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); + + args.push_back(g_args[make_pair(kernel, i)]); + args_size.push_back(g_args_size[make_pair(kernel, i)]); + } + + // get program + clGetKernelInfo(kernel, CL_KERNEL_PROGRAM, sizeof(program), &program, NULL); +} + +int CLQueuedKernel::get_arg_num(const char *search_arg_name) { + for (int i = 0; i < num_args; i++) { + if (arg_names[i] == search_arg_name) return i; + } + printf("failed to find %s in %s\n", search_arg_name, name.c_str()); + assert(false); +} + +cl_int CLQueuedKernel::exec() { + if (kernel == NULL) { + kernel = clCreateKernel(program, name.c_str(), NULL); + arg_names.clear(); + arg_types.clear(); + + for (int j = 0; j < num_args; j++) { + char arg_name[0x100] = {0}; + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); + arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); + + cl_int ret; + if (args[j].size() != 0) { + assert(args[j].size() == args_size[j]); + ret = thneed_clSetKernelArg(kernel, j, args[j].size(), args[j].data()); + } else { + ret = thneed_clSetKernelArg(kernel, j, args_size[j], NULL); + } + assert(ret == CL_SUCCESS); + } + } + + if (thneed->debug >= 1) { + debug_print(thneed->debug >= 2); + } + + return clEnqueueNDRangeKernel(thneed->command_queue, + kernel, work_dim, NULL, global_work_size, local_work_size, 0, NULL, NULL); +} + +void CLQueuedKernel::debug_print(bool verbose) { + printf("%p %56s -- ", kernel, name.c_str()); + for (int i = 0; i < work_dim; i++) { + printf("%4zu ", global_work_size[i]); + } + printf(" -- "); + for (int i = 0; i < work_dim; i++) { + printf("%4zu ", local_work_size[i]); + } + printf("\n"); + + if (verbose) { + for (int i = 0; i < num_args; i++) { + string arg = args[i]; + printf(" %s %s", arg_types[i].c_str(), arg_names[i].c_str()); + void *arg_value = (void*)arg.data(); + int arg_size = arg.size(); + if (arg_size == 0) { + printf(" (size) %d", args_size[i]); + } else if (arg_size == 1) { + printf(" = %d", *((char*)arg_value)); + } else if (arg_size == 2) { + printf(" = %d", *((short*)arg_value)); + } else if (arg_size == 4) { + if (arg_types[i] == "float") { + printf(" = %f", *((float*)arg_value)); + } else { + printf(" = %d", *((int*)arg_value)); + } + } else if (arg_size == 8) { + cl_mem val = (cl_mem)(*((uintptr_t*)arg_value)); + printf(" = %p", val); + if (val != NULL) { + cl_mem_object_type obj_type; + clGetMemObjectInfo(val, CL_MEM_TYPE, sizeof(obj_type), &obj_type, NULL); + if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t" || obj_type == CL_MEM_OBJECT_IMAGE2D) { + cl_image_format format; + size_t width, height, depth, array_size, row_pitch, slice_pitch; + cl_mem buf; + clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL); + assert(format.image_channel_order == CL_RGBA); + assert(format.image_channel_data_type == CL_HALF_FLOAT || format.image_channel_data_type == CL_FLOAT); + clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); + clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); + clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); + clGetImageInfo(val, CL_IMAGE_DEPTH, sizeof(depth), &depth, NULL); + clGetImageInfo(val, CL_IMAGE_ARRAY_SIZE, sizeof(array_size), &array_size, NULL); + clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL); + assert(depth == 0); + assert(array_size == 0); + assert(slice_pitch == 0); + + clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL); + size_t sz = 0; + if (buf != NULL) clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + printf(" image %zu x %zu rp %zu @ %p buffer %zu", width, height, row_pitch, buf, sz); + } else { + size_t sz; + clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + printf(" buffer %zu", sz); + } + } + } + printf("\n"); + } + } +} + +cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) { + g_args_size[make_pair(kernel, arg_index)] = arg_size; + if (arg_value != NULL) { + g_args[make_pair(kernel, arg_index)] = string((char*)arg_value, arg_size); + } else { + g_args[make_pair(kernel, arg_index)] = string(""); + } + cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value); + return ret; +} diff --git a/selfdrive/modeld/thneed/thneed_pc.cc b/selfdrive/modeld/thneed/thneed_pc.cc new file mode 100644 index 00000000000000..8d0037628e2f3d --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_pc.cc @@ -0,0 +1,32 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include + +#include "common/clutil.h" +#include "common/timing.h" + +Thneed::Thneed(bool do_clinit, cl_context _context) { + context = _context; + if (do_clinit) clinit(); + char *thneed_debug_env = getenv("THNEED_DEBUG"); + debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; +} + +void Thneed::execute(float **finputs, float *foutput, bool slow) { + uint64_t tb, te; + if (debug >= 1) tb = nanos_since_boot(); + + // ****** copy inputs + copy_inputs(finputs); + + // ****** run commands + clexec(); + + // ****** copy outputs + copy_output(foutput); + + if (debug >= 1) { + te = nanos_since_boot(); + printf("model exec in %lu us\n", (te-tb)/1000); + } +} diff --git a/selfdrive/modeld/thneed/thneed_qcom2.cc b/selfdrive/modeld/thneed/thneed_qcom2.cc new file mode 100644 index 00000000000000..a29a82c8c87338 --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_qcom2.cc @@ -0,0 +1,283 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include +#include + +#include +#include +#include +#include +#include + +#include "common/clutil.h" +#include "common/timing.h" + +Thneed *g_thneed = NULL; +int g_fd = -1; + +void hexdump(uint8_t *d, int len) { + assert((len%4) == 0); + printf(" dumping %p len 0x%x\n", d, len); + for (int i = 0; i < len/4; i++) { + if (i != 0 && (i%0x10) == 0) printf("\n"); + printf("%8x ", d[i]); + } + printf("\n"); +} + +// *********** ioctl interceptor *********** + +extern "C" { + +int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; +#undef ioctl +int ioctl(int filedes, unsigned long request, void *argp) { + request &= 0xFFFFFFFF; // needed on QCOM2 + if (my_ioctl == NULL) my_ioctl = reinterpret_cast(dlsym(RTLD_NEXT, "ioctl")); + Thneed *thneed = g_thneed; + + // save the fd + if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes; + + // note that this runs always, even without a thneed object + if (request == IOCTL_KGSL_DRAWCTXT_CREATE) { + struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp; + create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK; + create->flags |= 1 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority + printf("IOCTL_KGSL_DRAWCTXT_CREATE: creating context with flags 0x%x\n", create->flags); + } + + if (thneed != NULL) { + if (request == IOCTL_KGSL_GPU_COMMAND) { + struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp; + if (thneed->record) { + thneed->timestamp = cmd->timestamp; + thneed->context_id = cmd->context_id; + thneed->cmds.push_back(unique_ptr(new CachedCommand(thneed, cmd))); + } + if (thneed->debug >= 1) { + printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx context_id: %u timestamp: %u numcmds: %d numobjs: %d\n", + thneed->cmds.size(), + cmd->flags, + cmd->context_id, cmd->timestamp, cmd->numcmds, cmd->numobjs); + } + } else if (request == IOCTL_KGSL_GPUOBJ_SYNC) { + struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp; + struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs); + + if (thneed->debug >= 2) { + printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count); + for (int i = 0; i < cmd->count; i++) { + printf(" -- offset:0x%lx len:0x%lx id:%d op:%d ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op); + } + printf("\n"); + } + + if (thneed->record) { + thneed->cmds.push_back(unique_ptr(new + CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count)))); + } + } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) { + struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp; + if (thneed->debug >= 1) { + printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d timestamp: %d timeout: %d\n", + cmd->context_id, cmd->timestamp, cmd->timeout); + } + } else if (request == IOCTL_KGSL_SETPROPERTY) { + if (thneed->debug >= 1) { + struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp; + printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes); + if (thneed->debug >= 2) { + hexdump((uint8_t *)prop->value, prop->sizebytes); + if (prop->type == KGSL_PROP_PWR_CONSTRAINT) { + struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value; + hexdump((uint8_t *)constraint->data, constraint->size); + } + } + } + } else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) { + // this happens + } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) { + // this happens + } else { + if (thneed->debug >= 1) { + printf("other ioctl %lx\n", request); + } + } + } + + int ret = my_ioctl(filedes, request, argp); + if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno); + return ret; +} + +} + +// *********** GPUMalloc *********** + +GPUMalloc::GPUMalloc(int size, int fd) { + struct kgsl_gpuobj_alloc alloc; + memset(&alloc, 0, sizeof(alloc)); + alloc.size = size; + alloc.flags = 0x10000a00; + ioctl(fd, IOCTL_KGSL_GPUOBJ_ALLOC, &alloc); + void *addr = mmap64(NULL, alloc.mmapsize, 0x3, 0x1, fd, alloc.id*0x1000); + assert(addr != MAP_FAILED); + + base = (uint64_t)addr; + remaining = size; +} + +GPUMalloc::~GPUMalloc() { + // TODO: free the GPU malloced area +} + +void *GPUMalloc::alloc(int size) { + void *ret = (void*)base; + size = (size+0xff) & (~0xFF); + assert(size <= remaining); + remaining -= size; + base += size; + return ret; +} + +// *********** CachedSync, at the ioctl layer *********** + +void CachedSync::exec() { + struct kgsl_gpuobj_sync cmd; + + cmd.objs = (uint64_t)data.data(); + cmd.obj_len = data.length(); + cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj); + + int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); + assert(ret == 0); +} + +// *********** CachedCommand, at the ioctl layer *********** + +CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { + thneed = lthneed; + assert(cmd->numsyncs == 0); + + memcpy(&cache, cmd, sizeof(cache)); + + if (cmd->numcmds > 0) { + cmds = make_unique(cmd->numcmds); + memcpy(cmds.get(), (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*cmd->numcmds); + cache.cmdlist = (uint64_t)cmds.get(); + for (int i = 0; i < cmd->numcmds; i++) { + void *nn = thneed->ram->alloc(cmds[i].size); + memcpy(nn, (void*)cmds[i].gpuaddr, cmds[i].size); + cmds[i].gpuaddr = (uint64_t)nn; + } + } + + if (cmd->numobjs > 0) { + objs = make_unique(cmd->numobjs); + memcpy(objs.get(), (void *)cmd->objlist, sizeof(struct kgsl_command_object)*cmd->numobjs); + cache.objlist = (uint64_t)objs.get(); + for (int i = 0; i < cmd->numobjs; i++) { + void *nn = thneed->ram->alloc(objs[i].size); + memset(nn, 0, objs[i].size); + objs[i].gpuaddr = (uint64_t)nn; + } + } + + kq = thneed->ckq; + thneed->ckq.clear(); +} + +void CachedCommand::exec() { + cache.timestamp = ++thneed->timestamp; + int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache); + + if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret); + + if (thneed->debug >= 2) { + for (auto &it : kq) { + it->debug_print(false); + } + } + + assert(ret == 0); +} + +// *********** Thneed *********** + +Thneed::Thneed(bool do_clinit, cl_context _context) { + // TODO: QCOM2 actually requires a different context + //context = _context; + if (do_clinit) clinit(); + assert(g_fd != -1); + fd = g_fd; + ram = make_unique(0x80000, fd); + timestamp = -1; + g_thneed = this; + char *thneed_debug_env = getenv("THNEED_DEBUG"); + debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; +} + +void Thneed::wait() { + struct kgsl_device_waittimestamp_ctxtid wait; + wait.context_id = context_id; + wait.timestamp = timestamp; + wait.timeout = -1; + + uint64_t tb = nanos_since_boot(); + int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); + uint64_t te = nanos_since_boot(); + + if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000); +} + +void Thneed::execute(float **finputs, float *foutput, bool slow) { + uint64_t tb, te; + if (debug >= 1) tb = nanos_since_boot(); + + // ****** copy inputs + copy_inputs(finputs, true); + + // ****** set power constraint + int ret; + struct kgsl_device_constraint_pwrlevel pwrlevel; + pwrlevel.level = KGSL_CONSTRAINT_PWR_MAX; + + struct kgsl_device_constraint constraint; + constraint.type = KGSL_CONSTRAINT_PWRLEVEL; + constraint.context_id = context_id; + constraint.data = (void*)&pwrlevel; + constraint.size = sizeof(pwrlevel); + + struct kgsl_device_getproperty prop; + prop.type = KGSL_PROP_PWR_CONSTRAINT; + prop.value = (void*)&constraint; + prop.sizebytes = sizeof(constraint); + ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); + assert(ret == 0); + + // ****** run commands + int i = 0; + for (auto &it : cmds) { + ++i; + if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000); + it->exec(); + if ((i == cmds.size()) || slow) wait(); + } + + // ****** copy outputs + copy_output(foutput); + + // ****** unset power constraint + constraint.type = KGSL_CONSTRAINT_NONE; + constraint.data = NULL; + constraint.size = 0; + + ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop); + assert(ret == 0); + + if (debug >= 1) { + te = nanos_since_boot(); + printf("model exec in %lu us\n", (te-tb)/1000); + } +} diff --git a/selfdrive/modeld/transforms/loadyuv.cc b/selfdrive/modeld/transforms/loadyuv.cc index c93f5cd038183d..39f404a897c0a1 100644 --- a/selfdrive/modeld/transforms/loadyuv.cc +++ b/selfdrive/modeld/transforms/loadyuv.cc @@ -15,7 +15,7 @@ void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int w "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d", width, height); - cl_program prg = cl_program_from_file(ctx, device_id, LOADYUV_PATH, args); + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/loadyuv.cl", args); s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err)); s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err)); @@ -33,8 +33,17 @@ void loadyuv_destroy(LoadYUVState* s) { void loadyuv_queue(LoadYUVState* s, cl_command_queue q, cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, - cl_mem out_cl) { + cl_mem out_cl, bool do_shift) { cl_int global_out_off = 0; + if (do_shift) { + // shift the image in slot 1 to slot 0, then place the new image in slot 1 + global_out_off += (s->width*s->height) + (s->width/2)*(s->height/2)*2; + CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &out_cl)); + CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_int), &global_out_off)); + const size_t copy_work_size = global_out_off/8; + CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL, + ©_work_size, NULL, 0, 0, NULL)); + } CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl)); CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl)); @@ -63,14 +72,3 @@ void loadyuv_queue(LoadYUVState* s, cl_command_queue q, CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, &loaduv_work_size, NULL, 0, 0, NULL)); } - -void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst, - size_t src_offset, size_t dst_offset, size_t size) { - CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &src)); - CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_mem), &dst)); - CL_CHECK(clSetKernelArg(s->copy_krnl, 2, sizeof(cl_int), &src_offset)); - CL_CHECK(clSetKernelArg(s->copy_krnl, 3, sizeof(cl_int), &dst_offset)); - const size_t copy_work_size = size/8; - CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL, - ©_work_size, NULL, 0, 0, NULL)); -} \ No newline at end of file diff --git a/selfdrive/modeld/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl index 970187a6d70129..7dd3d973a3ef69 100644 --- a/selfdrive/modeld/transforms/loadyuv.cl +++ b/selfdrive/modeld/transforms/loadyuv.cl @@ -1,7 +1,7 @@ #define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2)) __kernel void loadys(__global uchar8 const * const Y, - __global uchar * out, + __global float * out, int out_offset) { const int gid = get_global_id(0); @@ -10,12 +10,13 @@ __kernel void loadys(__global uchar8 const * const Y, const int ox = ois % TRANSFORMED_WIDTH; const uchar8 ys = Y[gid]; + const float8 ysf = convert_float8(ys); // 02 // 13 - __global uchar* outy0; - __global uchar* outy1; + __global float* outy0; + __global float* outy1; if ((oy & 1) == 0) { outy0 = out + out_offset; //y0 outy1 = out + out_offset + UV_SIZE*2; //y2 @@ -24,24 +25,23 @@ __kernel void loadys(__global uchar8 const * const Y, outy1 = out + out_offset + UV_SIZE*3; //y3 } - vstore4(ys.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); - vstore4(ys.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); + vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); + vstore4(ysf.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); } __kernel void loaduv(__global uchar8 const * const in, - __global uchar8 * out, + __global float8 * out, int out_offset) { const int gid = get_global_id(0); const uchar8 inv = in[gid]; - out[gid + out_offset / 8] = inv; + const float8 outv = convert_float8(inv); + out[gid + out_offset / 8] = outv; } -__kernel void copy(__global uchar8 * in, - __global uchar8 * out, - int in_offset, - int out_offset) +__kernel void copy(__global float8 * inout, + int in_offset) { const int gid = get_global_id(0); - out[gid + out_offset / 8] = in[gid + in_offset / 8]; + inout[gid] = inout[gid + in_offset / 8]; } diff --git a/selfdrive/modeld/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h index 659059cd25e610..7d27ef5d468dba 100644 --- a/selfdrive/modeld/transforms/loadyuv.h +++ b/selfdrive/modeld/transforms/loadyuv.h @@ -13,8 +13,4 @@ void loadyuv_destroy(LoadYUVState* s); void loadyuv_queue(LoadYUVState* s, cl_command_queue q, cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, - cl_mem out_cl); - - -void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst, - size_t src_offset, size_t dst_offset, size_t size); \ No newline at end of file + cl_mem out_cl, bool do_shift = false); diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index 305643cf42eaf6..f341314144ec02 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -8,7 +8,7 @@ void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) { memset(s, 0, sizeof(*s)); - cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, ""); + cl_program prg = cl_program_from_file(ctx, device_id, "transforms/transform.cl", ""); s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err)); // done with this CL_CHECK(clReleaseProgram(prg)); diff --git a/selfdrive/modeld/transforms/transform.cl b/selfdrive/modeld/transforms/transform.cl index 2ca25920cd19be..357ef87321371c 100644 --- a/selfdrive/modeld/transforms/transform.cl +++ b/selfdrive/modeld/transforms/transform.cl @@ -22,20 +22,20 @@ __kernel void warpPerspective(__global const uchar * src, W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f; int X = rint(X0 * W), Y = rint(Y0 * W); - int sx = convert_short_sat(X >> INTER_BITS); - int sy = convert_short_sat(Y >> INTER_BITS); - - short sx_clamp = clamp(sx, 0, src_cols - 1); - short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1); - short sy_clamp = clamp(sy, 0, src_rows - 1); - short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1); - int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]); - int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]); - int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]); - int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]); - + short sx = convert_short_sat(X >> INTER_BITS); + short sy = convert_short_sat(Y >> INTER_BITS); short ay = (short)(Y & (INTER_TAB_SIZE - 1)); short ax = (short)(X & (INTER_TAB_SIZE - 1)); + + int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ? + convert_int(src[mad24(sy, src_row_stride, src_offset + sx*src_px_stride)]) : 0; + int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ? + convert_int(src[mad24(sy, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; + int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? + convert_int(src[mad24(sy+1, src_row_stride, src_offset + sx*src_px_stride)]) : 0; + int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? + convert_int(src[mad24(sy+1, src_row_stride, src_offset + (sx+1)*src_px_stride)]) : 0; + float taby = 1.f/INTER_TAB_SIZE*ay; float tabx = 1.f/INTER_TAB_SIZE*ax; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1ac2c2dcbab497..35eee5b035bd07 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,50 +1,96 @@ #!/usr/bin/env python3 +import gc + import cereal.messaging as messaging -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process -from openpilot.selfdrive.monitoring.helpers import DriverMonitoring +from cereal import car +from common.params import Params, put_bool_nonblocking +from common.realtime import set_realtime_priority +from selfdrive.controls.lib.events import Events +from selfdrive.locationd.calibrationd import Calibration +from selfdrive.monitoring.driver_monitor import DriverStatus + + +def dmonitoringd_thread(sm=None, pm=None): + gc.disable() + set_realtime_priority(2) + if pm is None: + pm = messaging.PubMaster(['driverMonitoringState']) -def dmonitoringd_thread(): - config_realtime_process([0, 1, 2, 3], 5) + if sm is None: + sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2']) - params = Params() - pm = messaging.PubMaster(['driverMonitoringState']) - sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') + driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) - DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) - demo_mode=False + sm['liveCalibration'].calStatus = Calibration.INVALID + sm['liveCalibration'].rpyCalib = [0, 0, 0] + sm['carState'].buttonEvents = [] + sm['carState'].standstill = True - # 20Hz <- dmonitoringmodeld + v_cruise_last = 0 + driver_engaged = False + + # 10Hz <- dmonitoringmodeld while True: sm.update() + if not sm.updated['driverStateV2']: - # iterate when model has new output continue - valid = sm.all_checks() - if demo_mode and sm.valid['driverStateV2']: - DM.run_step(sm, demo=demo_mode) - elif valid: - DM.run_step(sm, demo=demo_mode) + # Get interaction + if sm.updated['carState']: + v_cruise = sm['carState'].cruiseState.speed + driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ + v_cruise != v_cruise_last or \ + sm['carState'].steeringPressed or \ + sm['carState'].gasPressed + v_cruise_last = v_cruise - # publish - dat = DM.get_state_packet(valid=valid) - pm.send('driverMonitoringState', dat) + if sm.updated['modelV2']: + driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo) + + # Get data from dmonitoringmodeld + events = Events() + driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + + # Block engaging after max number of distrations + if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ + driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION: + events.add(car.CarEvent.EventName.tooDistracted) - # load live always-on toggle - if sm['driverStateV2'].frameId % 40 == 1: - DM.always_on = params.get_bool("AlwaysOnDM") - demo_mode = params.get_bool("IsDriverViewEnabled") + # Update events from driver state + driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) + + # build driverMonitoringState packet + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { + "events": events.to_msg(), + "faceDetected": driver_status.face_detected, + "isDistracted": driver_status.driver_distracted, + "distractedType": sum(driver_status.distracted_types), + "awarenessStatus": driver_status.awareness, + "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), + "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, + "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), + "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, + "stepChange": driver_status.step_change, + "awarenessActive": driver_status.awareness_active, + "awarenessPassive": driver_status.awareness_passive, + "isLowStd": driver_status.pose.low_std, + "hiStdCount": driver_status.hi_stds, + "isActiveMode": driver_status.active_monitoring_mode, + "isRHD": driver_status.wheel_on_right, + } + pm.send('driverMonitoringState', dat) # save rhd virtual toggle every 5 mins - if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and - DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and - DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): - params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) + if (sm['driverStateV2'].frameId % 6000 == 0 and + driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and + driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)): + put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right) -def main(): - dmonitoringd_thread() +def main(sm=None, pm=None): + dmonitoringd_thread(sm, pm) if __name__ == '__main__': diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py new file mode 100644 index 00000000000000..9ff3125c15c893 --- /dev/null +++ b/selfdrive/monitoring/driver_monitor.py @@ -0,0 +1,332 @@ +from math import atan2 + +from cereal import car +from common.numpy_fast import interp +from common.realtime import DT_DMON +from common.filter_simple import FirstOrderFilter +from common.stat_live import RunningStatFilter +from common.transformations.camera import tici_d_frame_size + +EventName = car.CarEvent.EventName + +# ****************************************************************************************** +# NOTE: To fork maintainers. +# Disabling or nerfing safety features will get you and your users banned from our servers. +# We recommend that you do not change these numbers from the defaults. +# ****************************************************************************************** + +class DRIVER_MONITOR_SETTINGS(): + def __init__(self): + self._DT_DMON = DT_DMON + # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 + self._AWARENESS_TIME = 30. # passive wheeltouch total timeout + self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. + self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. + self._DISTRACTED_TIME = 11. # active monitoring total timeout + self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. + self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. + + self._FACE_THRESHOLD = 0.7 + self._EYE_THRESHOLD = 0.65 + self._SG_THRESHOLD = 0.9 + self._BLINK_THRESHOLD = 0.87 + + self._EE_THRESH11 = 0.75 + self._EE_THRESH12 = 3.25 + self._EE_THRESH21 = 0.01 + self._EE_THRESH22 = 0.35 + + self._POSE_PITCH_THRESHOLD = 0.3133 + self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 + self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD + self._POSE_YAW_THRESHOLD = 0.4020 + self._POSE_YAW_THRESHOLD_SLACK = 0.5042 + self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD + self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned + self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned + self._PITCH_MAX_OFFSET = 0.124 + self._PITCH_MIN_OFFSET = -0.0881 + self._YAW_MAX_OFFSET = 0.289 + self._YAW_MIN_OFFSET = -0.0246 + + self._POSESTD_THRESHOLD = 0.3 + self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s + self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz + + self._POSE_CALIB_MIN_SPEED = 13 # 30 mph + self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative + self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" + + self._WHEELPOS_CALIB_MIN_SPEED = 11 + self._WHEELPOS_THRESHOLD = 0.5 + self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side + + self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change + self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change + + self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts + self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts + + +# model output refers to center of undistorted+leveled image +EFL = 598.0 # focal length in K +W, H = tici_d_frame_size # corrected image has same size as raw + +class DistractedType: + NOT_DISTRACTED = 0 + DISTRACTED_POSE = 1 + DISTRACTED_BLINK = 2 + DISTRACTED_E2E = 4 + +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): + # the output of these angles are in device frame + # so from driver's perspective, pitch is up and yaw is right + + pitch_net, yaw_net, roll_net = angles_desc + + face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) + yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) + pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) + + pitch = pitch_net + pitch_focal_angle + yaw = -yaw_net + yaw_focal_angle + + # no calib for roll + pitch -= rpy_calib[1] + yaw -= rpy_calib[2] + return roll_net, pitch, yaw + +class DriverPose(): + def __init__(self, max_trackable): + self.yaw = 0. + self.pitch = 0. + self.roll = 0. + self.yaw_std = 0. + self.pitch_std = 0. + self.roll_std = 0. + self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) + self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) + self.low_std = True + self.cfactor_pitch = 1. + self.cfactor_yaw = 1. + +class DriverBlink(): + def __init__(self): + self.left_blink = 0. + self.right_blink = 0. + +class DriverStatus(): + def __init__(self, rhd_saved=False, settings=DRIVER_MONITOR_SETTINGS()): + # init policy settings + self.settings = settings + + # init driver status + self.wheelpos_learner = RunningStatFilter() + self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) + self.pose_calibrated = False + self.blink = DriverBlink() + self.eev1 = 0. + self.eev2 = 1. + self.ee1_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT) + self.ee2_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT) + self.ee1_calibrated = False + self.ee2_calibrated = False + + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + self.distracted_types = [] + self.driver_distracted = False + self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) + self.wheel_on_right = False + self.wheel_on_right_last = None + self.wheel_on_right_default = rhd_saved + self.face_detected = False + self.terminal_alert_cnt = 0 + self.terminal_time = 0 + self.step_change = 0. + self.active_monitoring_mode = True + self.is_model_uncertain = False + self.hi_stds = 0 + self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + + self._set_timers(active_monitoring=True) + + def _set_timers(self, active_monitoring): + if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: + if active_monitoring: + self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME + else: + self.step_change = 0. + return # no exploit after orange alert + elif self.awareness <= 0.: + return + + if active_monitoring: + # when falling back from passive mode to active mode, reset awareness to avoid false alert + if not self.active_monitoring_mode: + self.awareness_passive = self.awareness + self.awareness = self.awareness_active + + self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME + self.active_monitoring_mode = True + else: + if self.active_monitoring_mode: + self.awareness_active = self.awareness + self.awareness = self.awareness_passive + + self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME + self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME + self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME + self.active_monitoring_mode = False + + def _get_distracted_types(self): + distracted_types = [] + + if not self.pose_calibrated: + pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET + yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET + else: + pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(), + self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) + yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(), + self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) + pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit + yaw_error = abs(yaw_error) + if pitch_error > self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch or \ + yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: + distracted_types.append(DistractedType.DISTRACTED_POSE) + + if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD: + distracted_types.append(DistractedType.DISTRACTED_BLINK) + + if self.ee1_calibrated: + ee1_dist = self.eev1 > self.ee1_offseter.filtered_stat.M * self.settings._EE_THRESH12 + else: + ee1_dist = self.eev1 > self.settings._EE_THRESH11 + if self.ee2_calibrated: + ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22 + else: + ee2_dist = self.eev2 < self.settings._EE_THRESH21 + if ee1_dist or ee2_dist: + distracted_types.append(DistractedType.DISTRACTED_E2E) + + return distracted_types + + def set_policy(self, model_data, car_speed): + bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) + bp_normal = max(min(bp / k1, 0.5),0) + self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], + [self.settings._POSE_PITCH_THRESHOLD_SLACK, + self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD + self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5], + [self.settings._POSE_YAW_THRESHOLD_SLACK, + self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD + + def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): + rhd_pred = driver_state.wheelOnRightProb + # calibrates only when there's movement and either face detected + if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or + driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD): + self.wheelpos_learner.push_and_update(rhd_pred) + if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: + self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + else: + self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished + # make sure no switching when engaged + if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right: + self.wheel_on_right = self.wheel_on_right_last + driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData + if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, + driver_data.faceOrientationStd, driver_data.facePositionStd, + driver_data.readyProb, driver_data.notReadyProb)): + return + + self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + if self.wheel_on_right: + self.pose.yaw *= -1 + self.wheel_on_right_last = self.wheel_on_right + self.pose.pitch_std = driver_data.faceOrientationStd[0] + self.pose.yaw_std = driver_data.faceOrientationStd[1] + model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) + self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD + self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.eev1 = driver_data.notReadyProb[1] + self.eev2 = driver_data.readyProb[0] + + self.distracted_types = self._get_distracted_types() + self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or + DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ + driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + self.driver_distraction_filter.update(self.driver_distracted) + + # update offseter + # only update when driver is actively driving the car above a certain speed + if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): + self.pose.pitch_offseter.push_and_update(self.pose.pitch) + self.pose.yaw_offseter.push_and_update(self.pose.yaw) + self.ee1_offseter.push_and_update(self.eev1) + self.ee2_offseter.push_and_update(self.eev2) + + self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + self.ee1_calibrated = self.ee1_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + self.ee2_calibrated = self.ee2_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + + self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME + self._set_timers(self.face_detected and not self.is_model_uncertain) + if self.face_detected and not self.pose.low_std and not self.driver_distracted: + self.hi_stds += 1 + elif self.face_detected and self.pose.low_std: + self.hi_stds = 0 + + def update_events(self, events, driver_engaged, ctrl_active, standstill): + if (driver_engaged and self.awareness > 0) or not ctrl_active: + # reset only when on disengagement if red reached + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + return + + driver_attentive = self.driver_distraction_filter.x < 0.37 + awareness_prev = self.awareness + + if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): + # only restore awareness when paying attention and alert is not red + self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) + if self.awareness == 1.: + self.awareness_passive = min(self.awareness_passive + self.step_change, 1.) + # don't display alert banner when awareness is recovering and has cleared orange + if self.awareness > self.threshold_prompt: + return + + standstill_exemption = standstill and self.awareness - self.step_change <= self.threshold_prompt + certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected + maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected + if certainly_distracted or maybe_distracted: + # should always be counting if distracted unless at standstill and reaching orange + if not standstill_exemption: + self.awareness = max(self.awareness - self.step_change, -0.1) + + alert = None + if self.awareness <= 0.: + # terminal red alert: disengagement required + alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive + self.terminal_time += 1 + if awareness_prev > 0.: + self.terminal_alert_cnt += 1 + elif self.awareness <= self.threshold_prompt: + # prompt orange alert + alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive + elif self.awareness <= self.threshold_pre: + # pre green alert + alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive + + if alert is not None: + events.add(alert) diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py deleted file mode 100644 index 0b54504b647296..00000000000000 --- a/selfdrive/monitoring/helpers.py +++ /dev/null @@ -1,463 +0,0 @@ -from math import atan2 -import numpy as np - -from cereal import car, log -import cereal.messaging as messaging -from openpilot.selfdrive.selfdrived.events import Events -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.common.realtime import DT_DMON -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.common.stat_live import RunningStatFilter -from openpilot.common.transformations.camera import DEVICE_CAMERAS -from openpilot.system.hardware import HARDWARE - -EventName = log.OnroadEvent.EventName - -# ****************************************************************************************** -# NOTE: To fork maintainers. -# Disabling or nerfing safety features will get you and your users banned from our servers. -# We recommend that you do not change these numbers from the defaults. -# ****************************************************************************************** - -class DRIVER_MONITOR_SETTINGS: - def __init__(self, device_type): - self._DT_DMON = DT_DMON - # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - self._AWARENESS_TIME = 30. # passive wheeltouch total timeout - self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. - self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. - self._DISTRACTED_TIME = 11. # active monitoring total timeout - self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. - self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. - - self._FACE_THRESHOLD = 0.7 - self._EYE_THRESHOLD = 0.65 - self._SG_THRESHOLD = 0.9 - self._BLINK_THRESHOLD = 0.865 - self._PHONE_THRESH = 0.5 - - self._POSE_PITCH_THRESHOLD = 0.3133 - self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 - self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD - self._POSE_YAW_THRESHOLD = 0.4020 - self._POSE_YAW_THRESHOLD_SLACK = 0.5042 - self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD - self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned - self._PITCH_NATURAL_THRESHOLD = 0.449 - self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned - self._PITCH_NATURAL_VAR = 3*0.01 - self._YAW_NATURAL_VAR = 3*0.05 - self._PITCH_MAX_OFFSET = 0.124 - self._PITCH_MIN_OFFSET = -0.0881 - self._YAW_MAX_OFFSET = 0.289 - self._YAW_MIN_OFFSET = -0.0246 - - self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1 - self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / self._DT_DMON) - self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON) - self._POSESTD_THRESHOLD = 0.3 - self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s - self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz - self._ALWAYS_ON_ALERT_MIN_SPEED = 11 - - self._POSE_CALIB_MIN_SPEED = 13 # 30 mph - self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative - self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" - - self._WHEELPOS_CALIB_MIN_SPEED = 11 - self._WHEELPOS_THRESHOLD = 0.5 - self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side - self._WHEELPOS_DATA_AVG = 0.03 - self._WHEELPOS_DATA_VAR = 3*5.5e-5 - self._WHEELPOS_MAX_COUNT = -1 - - self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change - self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change - - self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts - self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts - -class DistractedType: - - NOT_DISTRACTED = 0 - DISTRACTED_POSE = 1 << 0 - DISTRACTED_BLINK = 1 << 1 - DISTRACTED_PHONE = 1 << 2 - -class DriverPose: - def __init__(self, settings): - pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2) - yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2) - self.yaw = 0. - self.pitch = 0. - self.roll = 0. - self.yaw_std = 0. - self.pitch_std = 0. - self.roll_std = 0. - self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.calibrated = False - self.low_std = True - self.cfactor_pitch = 1. - self.cfactor_yaw = 1. - -class DriverProb: - def __init__(self, raw_priors, max_trackable): - self.prob = 0. - self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable) - self.prob_calibrated = False - -class DriverBlink: - def __init__(self): - self.left = 0. - self.right = 0. - - -# model output refers to center of undistorted+leveled image -EFL = 598.0 # focal length in K -cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw -W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw - -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): - # the output of these angles are in device frame - # so from driver's perspective, pitch is up and yaw is right - - pitch_net, yaw_net, roll_net = angles_desc - - face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) - yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) - pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) - - pitch = pitch_net + pitch_focal_angle - yaw = -yaw_net + yaw_focal_angle - - # no calib for roll - pitch -= rpy_calib[1] - yaw -= rpy_calib[2] - return roll_net, pitch, yaw - - -class DriverMonitoring: - def __init__(self, rhd_saved=False, settings=None, always_on=False): - # init policy settings - self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) - - # init driver status - wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) - self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) - self.pose = DriverPose(settings=self.settings) - self.blink = DriverBlink() - self.phone_prob = 0. - - self.always_on = always_on - self.distracted_types = [] - self.driver_distracted = False - self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) - self.wheel_on_right = False - self.wheel_on_right_last = None - self.wheel_on_right_default = rhd_saved - self.face_detected = False - self.terminal_alert_cnt = 0 - self.terminal_time = 0 - self.step_change = 0. - self.active_monitoring_mode = True - self.is_model_uncertain = False - self.hi_stds = 0 - self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.dcam_uncertain_cnt = 0 - self.dcam_uncertain_alerted = False # once per drive - self.dcam_reset_cnt = 0 - - self.params = Params() - self.too_distracted = self.params.get_bool("DriverTooDistracted") - - self._reset_awareness() - self._set_timers(active_monitoring=True) - self._reset_events() - - def _reset_awareness(self): - self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. - - def _reset_events(self): - self.current_events = Events() - - def _set_timers(self, active_monitoring): - if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: - if active_monitoring: - self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME - else: - self.step_change = 0. - return # no exploit after orange alert - elif self.awareness <= 0.: - return - - if active_monitoring: - # when falling back from passive mode to active mode, reset awareness to avoid false alert - if not self.active_monitoring_mode: - self.awareness_passive = self.awareness - self.awareness = self.awareness_active - - self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME - self.active_monitoring_mode = True - else: - if self.active_monitoring_mode: - self.awareness_active = self.awareness - self.awareness = self.awareness_passive - - self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME - self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME - self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME - self.active_monitoring_mode = False - - def _set_policy(self, brake_disengage_prob, car_speed): - bp = brake_disengage_prob - k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) - bp_normal = max(min(bp / k1, 0.5),0) - self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_PITCH_THRESHOLD_SLACK, - self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD - - def _get_distracted_types(self): - distracted_types = [] - - if not self.pose.calibrated: - pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET - yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET - else: - pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(), - self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) - yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(), - self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) - pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit - yaw_error = abs(yaw_error) - - pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD - yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw - - if pitch_error > pitch_threshold or yaw_error > yaw_threshold: - distracted_types.append(DistractedType.DISTRACTED_POSE) - - if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: - distracted_types.append(DistractedType.DISTRACTED_BLINK) - - if self.phone_prob > self.settings._PHONE_THRESH: - distracted_types.append(DistractedType.DISTRACTED_PHONE) - - return distracted_types - - def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False): - rhd_pred = driver_state.wheelOnRightProb - # calibrates only when there's movement and either face detected - if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or - driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD): - self.wheelpos.prob_offseter.push_and_update(rhd_pred) - - self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT - - if self.wheelpos.prob_calibrated or demo_mode: - self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD - else: - self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished - # make sure no switching when engaged - if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode: - self.wheel_on_right = self.wheel_on_right_last - driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData - if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, - driver_data.faceOrientationStd, driver_data.facePositionStd)): - return - - self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) - if self.wheel_on_right: - self.pose.yaw *= -1 - self.wheel_on_right_last = self.wheel_on_right - self.pose.pitch_std = driver_data.faceOrientationStd[0] - self.pose.yaw_std = driver_data.faceOrientationStd[1] - model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD - self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.phone_prob = driver_data.phoneProb - - self.distracted_types = self._get_distracted_types() - self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types - or DistractedType.DISTRACTED_POSE in self.distracted_types - or DistractedType.DISTRACTED_BLINK in self.distracted_types) \ - and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std - self.driver_distraction_filter.update(self.driver_distracted) - - # update offseter - # only update when driver is actively driving the car above a certain speed - if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): - self.pose.pitch_offseter.push_and_update(self.pose.pitch) - self.pose.yaw_offseter.push_and_update(self.pose.yaw) - - self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ - self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT - - if self.face_detected and not self.driver_distracted: - if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD: - if not standstill: - self.dcam_uncertain_cnt += 1 - self.dcam_reset_cnt = 0 - else: - self.dcam_reset_cnt += 1 - if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT: - self.dcam_uncertain_cnt = 0 - - self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME - self._set_timers(self.face_detected and not self.is_model_uncertain) - if self.face_detected and not self.pose.low_std and not self.driver_distracted: - self.hi_stds += 1 - elif self.face_detected and self.pose.low_std: - self.hi_stds = 0 - - def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed): - self._reset_events() - # Block engaging until ignition cycle after max number or time of distractions - if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ - self.terminal_time >= self.settings._MAX_TERMINAL_DURATION: - if not self.too_distracted: - self.params.put_bool_nonblocking("DriverTooDistracted", True) - self.too_distracted = True - - # Always-on distraction lockout is temporary - if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt): - self.current_events.add(EventName.tooDistracted) - - always_on_valid = self.always_on and not wrong_gear - if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \ - (not always_on_valid and not op_engaged) or \ - (always_on_valid and not op_engaged and self.awareness <= 0): - # always reset on disengage with normal mode; disengage resets only on red if always on - self._reset_awareness() - return - - driver_attentive = self.driver_distraction_filter.x < 0.37 - awareness_prev = self.awareness - - if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): - if driver_engaged: - self._reset_awareness() - return - # only restore awareness when paying attention and alert is not red - self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)* - (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) - if self.awareness == 1.: - self.awareness_passive = min(self.awareness_passive + self.step_change, 1.) - # don't display alert banner when awareness is recovering and has cleared orange - if self.awareness > self.threshold_prompt: - return - - _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt - _reaching_terminal = self.awareness - self.step_change <= 0 - standstill_orange_exemption = standstill and _reaching_audible - always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal - always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED - - certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected - maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected - - if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange - # also will not be reaching 0 if DM is active when not engaged - if not (standstill_orange_exemption or always_on_red_exemption or (always_on_lowspeed_exemption and _reaching_audible)): - self.awareness = max(self.awareness - self.step_change, -0.1) - - alert = None - if self.awareness <= 0.: - # terminal red alert: disengagement required - alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive - self.terminal_time += 1 - if awareness_prev > 0.: - self.terminal_alert_cnt += 1 - elif self.awareness <= self.threshold_prompt: - # prompt orange alert - alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive - elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: - # pre green alert - alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive - - if alert is not None: - self.current_events.add(alert) - - if self.dcam_uncertain_cnt > self.settings._DCAM_UNCERTAIN_ALERT_COUNT and not self.dcam_uncertain_alerted: - set_offroad_alert("Offroad_DriverMonitoringUncertain", True) - self.dcam_uncertain_alerted = True - - - def get_state_packet(self, valid=True): - # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState', valid=valid) - dat.driverMonitoringState = { - "events": self.current_events.to_msg(), - "faceDetected": self.face_detected, - "isDistracted": self.driver_distracted, - "distractedType": sum(self.distracted_types), - "awarenessStatus": self.awareness, - "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), - "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, - "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), - "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, - "stepChange": self.step_change, - "awarenessActive": self.awareness_active, - "awarenessPassive": self.awareness_passive, - "isLowStd": self.pose.low_std, - "hiStdCount": self.hi_stds, - "isActiveMode": self.active_monitoring_mode, - "isRHD": self.wheel_on_right, - "uncertainCount": self.dcam_uncertain_cnt, - } - return dat - - def run_step(self, sm, demo=False): - if demo: - highway_speed = 30 - enabled = True - wrong_gear = False - standstill = False - driver_engaged = False - brake_disengage_prob = 1.0 - rpyCalib = [0., 0., 0.] - else: - highway_speed = sm['carState'].vEgo - enabled = sm['selfdriveState'].enabled - wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) - standstill = sm['carState'].standstill - driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed - brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - rpyCalib = sm['liveCalibration'].rpyCalib - self._set_policy( - brake_disengage_prob=brake_disengage_prob, - car_speed=highway_speed, - ) - - # Parse data from dmonitoringmodeld - self._update_states( - driver_state=sm['driverStateV2'], - cal_rpy=rpyCalib, - car_speed=highway_speed, - op_engaged=enabled, - standstill=standstill, - demo_mode=demo, - ) - - # Update distraction events - self._update_events( - driver_engaged=driver_engaged, - op_engaged=enabled, - standstill=standstill, - wrong_gear=wrong_gear, - car_speed=highway_speed - ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py old mode 100644 new mode 100755 index 6ea9b80283fc7d..43b5e7747e66ba --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,12 +1,14 @@ +#!/usr/bin/env python3 +import unittest import numpy as np -from cereal import log -from openpilot.common.realtime import DT_DMON -from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS -from openpilot.system.hardware import HARDWARE +from cereal import car, log +from common.realtime import DT_DMON +from selfdrive.controls.lib.events import Events +from selfdrive.monitoring.driver_monitor import DriverStatus, DRIVER_MONITOR_SETTINGS -EventName = log.OnroadEvent.EventName -dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) +EventName = car.CarEvent.EventName +dm_settings = DRIVER_MONITOR_SETTINGS() TEST_TIMESPAN = 120 # seconds DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 @@ -26,7 +28,8 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] # TODO: test both separately when e2e is used - ds.leftDriverData.phoneProb = 0. + ds.leftDriverData.readyProb = [0., 0., 0., 0.] + ds.leftDriverData.notReadyProb = [0., 0.] return ds @@ -49,22 +52,25 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): always_true = [True] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON) -class TestMonitoring: +# TODO: this only tests DriverStatus +class TestMonitoring(unittest.TestCase): + # pylint: disable=no-member def _run_seq(self, msgs, interaction, engaged, standstill): - DM = DriverMonitoring() + DS = DriverStatus() events = [] for idx in range(len(msgs)): - DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx]) + e = Events() + DS.update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0) - events.append(DM.current_events) + DS.update_events(e, interaction[idx], engaged[idx], standstill[idx]) + events.append(e) assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" - return events, DM + return events, DS def _assert_no_events(self, events): - assert all(not len(e) for e in events) + self.assertTrue(all(not len(e) for e in events)) # engaged, driver is attentive all the time def test_fully_aware_driver(self): @@ -74,44 +80,40 @@ def test_fully_aware_driver(self): # engaged, driver is distracted and does nothing def test_fully_distracted_driver(self): events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \ - ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ - EventName.preDriverDistracted - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \ - ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted - assert events[int((d_status.settings._DISTRACTED_TIME + \ - ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted - assert isinstance(d_status.awareness, float) + self.assertEqual(len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]), 0) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + + ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.preDriverDistracted) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + + ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME + + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0], EventName.driverDistracted) + self.assertIs(type(d_status.awareness), float) # engaged, no face detected the whole time, no action def test_fully_invisible_driver(self): events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 - assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \ - ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ - EventName.preDriverUnresponsive - assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \ - ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive - assert events[int((d_status.settings._AWARENESS_TIME + \ - ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive + self.assertTrue(len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + + ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.preDriverUnresponsive) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + + ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((d_status.settings._AWARENESS_TIME + + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0], EventName.driverUnresponsive) # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel - # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention + # - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch def test_normal_driver(self): ds_vector = [msg_DISTRACTED] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ [msg_ATTENTIVE] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ - [msg_DISTRACTED] * int((DISTRACTED_SECONDS_TO_ORANGE+2)/DT_DMON) + \ - [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) + [msg_DISTRACTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON)) interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted - assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0 - assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted - assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted - assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0 + self.assertEqual(len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]), 0) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]), 0) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)]), 0) # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # driver dodges, and then touches wheel to no avail, disengages and reengages @@ -121,19 +123,15 @@ def test_biggest_comma_fan(self): ds_vector = always_distracted[:] interaction_vector = always_false[:] op_vector = always_true[:] - ds_vector[int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] \ - = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) - ds_vector[int((DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] \ - = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) - interaction_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] \ - = [True] * int(1/DT_DMON) - op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ - = [False] * int(0.5/DT_DMON) + ds_vector[int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + ds_vector[int((DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + interaction_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON) + op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON) events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted - assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted - assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted - assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0 + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0], EventName.promptDriverDistracted) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0], EventName.driverDistracted) + self.assertEqual(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0], EventName.driverDistracted) + self.assertTrue(len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0) # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # - both actions should clear the alert, but momentary appearance should not @@ -141,19 +139,18 @@ def test_sometimes_transparent_commuter(self): _visible_time = np.random.choice([0.5, 10]) ds_vector = always_no_face[:]*2 interaction_vector = always_false[:]*2 - ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ - [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) - assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive - assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0 + self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0) if _visible_time == 0.5: - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive) elif _visible_time == 10: - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive - assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0 + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0) # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # - only disengage will clear the alert @@ -166,18 +163,18 @@ def test_last_second_responder(self): interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive - assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive - assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive - assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive - assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0 + self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertEqual(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0], EventName.driverUnresponsive) + self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0) # disengaged, always distracted driver # - dm should stay quiet when not engaged def test_pure_dashcam_user(self): events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) - assert sum(len(event) for event in events) == 0 + self.assertTrue(sum(len(event) for event in events) == 0) # engaged, car stops at traffic light, down to orange, no action, then car starts moving # - should only reach green when stopped, but continues counting down on launch @@ -186,10 +183,9 @@ def test_long_traffic_light_victim(self): standstill_vector = always_true[:] standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ - EventName.preDriverDistracted - assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted - assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted + self.assertEqual(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0], EventName.preDriverDistracted) + self.assertEqual(events[int((_redlight_time-0.1)/DT_DMON)].names[0], EventName.preDriverDistracted) + self.assertEqual(events[int((_redlight_time+0.5)/DT_DMON)].names[0], EventName.promptDriverDistracted) # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert @@ -197,10 +193,10 @@ def test_somehow_indecisive_model(self): ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) interaction_vector = always_false[:] events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert EventName.preDriverUnresponsive in \ - events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names - assert EventName.promptDriverUnresponsive in \ - events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names - assert EventName.driverUnresponsive in \ - events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + self.assertTrue(EventName.preDriverUnresponsive in events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names) + self.assertTrue(EventName.promptDriverUnresponsive in events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) + self.assertTrue(EventName.driverUnresponsive in events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/navd/.gitignore b/selfdrive/navd/.gitignore new file mode 100644 index 00000000000000..a070fe32bb58dd --- /dev/null +++ b/selfdrive/navd/.gitignore @@ -0,0 +1,5 @@ +moc_* +*.moc + +map_renderer +libmap_renderer.so diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript new file mode 100644 index 00000000000000..4fbe41e80b46b6 --- /dev/null +++ b/selfdrive/navd/SConscript @@ -0,0 +1,20 @@ +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') + +base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', + 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] + +if arch == 'larch64': + base_libs.append('EGL') + +if arch in ['larch64', 'x86_64']: + if arch == 'x86_64': + rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath] + qt_env["RPATH"] += rpath + + qt_libs = ["qt_widgets", "qt_util", "qmapboxgl"] + base_libs + + nav_src = ["main.cc", "map_renderer.cc"] + qt_env.Program("map_renderer", nav_src, LIBS=qt_libs + ['common', 'json11']) + + if GetOption('extras'): + qt_env.SharedLibrary("map_renderer", ["map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) diff --git a/selfdrive/navd/__init__.py b/selfdrive/navd/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py new file mode 100644 index 00000000000000..eda813154adbc6 --- /dev/null +++ b/selfdrive/navd/helpers.py @@ -0,0 +1,174 @@ +from __future__ import annotations + +import json +import math +from typing import Any, Dict, List, Optional, Tuple, Union, cast + +from common.conversions import Conversions +from common.numpy_fast import clip +from common.params import Params + +EARTH_MEAN_RADIUS = 6371007.2 +SPEED_CONVERSIONS = { + 'km/h': Conversions.KPH_TO_MS, + 'mph': Conversions.MPH_TO_MS, + } + + +class Coordinate: + def __init__(self, latitude: float, longitude: float) -> None: + self.latitude = latitude + self.longitude = longitude + self.annotations: Dict[str, float] = {} + + @classmethod + def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: + return cls(t[1], t[0]) + + def as_dict(self) -> Dict[str, float]: + return {'latitude': self.latitude, 'longitude': self.longitude} + + def __str__(self) -> str: + return f"({self.latitude}, {self.longitude})" + + def __eq__(self, other) -> bool: + if not isinstance(other, Coordinate): + return False + return (self.latitude == other.latitude) and (self.longitude == other.longitude) + + def __sub__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude) + + def __add__(self, other: Coordinate) -> Coordinate: + return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude) + + def __mul__(self, c: float) -> Coordinate: + return Coordinate(self.latitude * c, self.longitude * c) + + def dot(self, other: Coordinate) -> float: + return self.latitude * other.latitude + self.longitude * other.longitude + + def distance_to(self, other: Coordinate) -> float: + # Haversine formula + dlat = math.radians(other.latitude - self.latitude) + dlon = math.radians(other.longitude - self.longitude) + + haversine_dlat = math.sin(dlat / 2.0) + haversine_dlat *= haversine_dlat + haversine_dlon = math.sin(dlon / 2.0) + haversine_dlon *= haversine_dlon + + y = haversine_dlat \ + + math.cos(math.radians(self.latitude)) \ + * math.cos(math.radians(other.latitude)) \ + * haversine_dlon + x = 2 * math.asin(math.sqrt(y)) + return x * EARTH_MEAN_RADIUS + + +def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): + if a.distance_to(b) < 0.01: + return a.distance_to(p) + + ap = p - a + ab = b - a + t = clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0) + projection = a + ab * t + return projection.distance_to(p) + + +def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: + if len(geometry) <= 2: + return geometry[0].distance_to(pos) + + # 1. Find segment that is closest to current position + # 2. Total distance is sum of distance to start of closest segment + # + all previous segments + total_distance = 0.0 + total_distance_closest = 0.0 + closest_distance = 1e9 + + for i in range(len(geometry) - 1): + d = minimum_distance(geometry[i], geometry[i + 1], pos) + + if d < closest_distance: + closest_distance = d + total_distance_closest = total_distance + geometry[i].distance_to(pos) + + total_distance += geometry[i].distance_to(geometry[i + 1]) + + return total_distance_closest + + +def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: + if params is None: + params = Params() + + json_str = params.get(param) + if json_str is None: + return None + + pos = json.loads(json_str) + if 'latitude' not in pos or 'longitude' not in pos: + return None + + return Coordinate(pos['latitude'], pos['longitude']) + + +def string_to_direction(direction: str) -> str: + for d in ['left', 'right', 'straight']: + if d in direction: + return d + return 'none' + + +def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: + unit = cast(str, maxspeed['unit']) + speed = cast(float, maxspeed['speed']) + return SPEED_CONVERSIONS[unit] * speed + + +def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: + if not len(banners): + return + + current_banner = banners[0] + + # A segment can contain multiple banners, find one that we need to show now + for banner in banners: + if distance_to_maneuver < banner['distanceAlongGeometry']: + current_banner = banner + + # Only show banner when close enough to maneuver + instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry'] + + # Primary + p = current_banner['primary'] + if 'text' in p: + instruction.maneuverPrimaryText = p['text'] + if 'type' in p: + instruction.maneuverType = p['type'] + if 'modifier' in p: + instruction.maneuverModifier = p['modifier'] + + # Secondary + if 'secondary' in current_banner: + instruction.maneuverSecondaryText = current_banner['secondary']['text'] + + # Lane lines + if 'sub' in current_banner: + lanes = [] + for component in current_banner['sub']['components']: + if component['type'] != 'lane': + continue + + lane = { + 'active': component['active'], + 'directions': [string_to_direction(d) for d in component['directions']], + } + + if 'active_direction' in component: + lane['activeDirection'] = string_to_direction(component['active_direction']) + + lanes.append(lane) + instruction.lanes = lanes diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc new file mode 100644 index 00000000000000..b6eec10328bc10 --- /dev/null +++ b/selfdrive/navd/main.cc @@ -0,0 +1,31 @@ +#include +#include +#include + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/navd/map_renderer.h" +#include "system/hardware/hw.h" + + + +void sigHandler(int s) { + qInfo() << "Shutting down"; + std::signal(s, SIG_DFL); + + qApp->quit(); +} + + +int main(int argc, char *argv[]) { + qInstallMessageHandler(swagLogMessageHandler); + + QApplication app(argc, argv); + std::signal(SIGINT, sigHandler); + std::signal(SIGTERM, sigHandler); + + MapRenderer * m = new MapRenderer(get_mapbox_settings()); + assert(m); + + return app.exec(); +} diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc new file mode 100644 index 00000000000000..d0770cfb48ef69 --- /dev/null +++ b/selfdrive/navd/map_renderer.cc @@ -0,0 +1,241 @@ +#include "selfdrive/navd/map_renderer.h" + +#include +#include +#include + +#include "common/timing.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" + +const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear +const int WIDTH = 256; +const int HEIGHT = WIDTH; + +const int NUM_VIPC_BUFFERS = 4; + +MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) { + QSurfaceFormat fmt; + fmt.setRenderableType(QSurfaceFormat::OpenGLES); + + ctx = std::make_unique(); + ctx->setFormat(fmt); + ctx->create(); + assert(ctx->isValid()); + + surface = std::make_unique(); + surface->setFormat(ctx->format()); + surface->create(); + + ctx->makeCurrent(surface.get()); + assert(QOpenGLContext::currentContext() == ctx.get()); + + gl_functions.reset(ctx->functions()); + gl_functions->initializeOpenGLFunctions(); + + QOpenGLFramebufferObjectFormat fbo_format; + fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); + + m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); + m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM); + m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr"); + m_map->createRenderer(); + + m_map->resize(fbo->size()); + m_map->setFramebufferObject(fbo->handle(), fbo->size()); + gl_functions->glViewport(0, 0, WIDTH, HEIGHT); + + if (online) { + vipc_server.reset(new VisionIpcServer("navd")); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); + vipc_server->start_listener(); + + pm.reset(new PubMaster({"navThumbnail"})); + sm.reset(new SubMaster({"liveLocationKalman", "navRoute"})); + + timer = new QTimer(this); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); + timer->start(50); + } +} + +void MapRenderer::msgUpdate() { + sm->update(0); + + if (sm->updated("liveLocationKalman")) { + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + auto pos = location.getPositionGeodetic(); + auto orientation = location.getCalibratedOrientationNED(); + + bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); + if (localizer_valid) { + updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2])); + } + } + + if (sm->updated("navRoute")) { + QList route; + auto coords = (*sm)["navRoute"].getNavRoute().getCoordinates(); + for (auto const &c : coords) { + route.push_back(QGeoCoordinate(c.getLatitude(), c.getLongitude())); + } + updateRoute(route); + } +} + +void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { + if (m_map.isNull()) { + return; + } + + m_map->setCoordinate(position); + m_map->setBearing(bearing); + update(); +} + +bool MapRenderer::loaded() { + return m_map->isFullyLoaded(); +} + +void MapRenderer::update() { + gl_functions->glClear(GL_COLOR_BUFFER_BIT); + m_map->render(); + gl_functions->glFlush(); + + sendVipc(); +} + +void MapRenderer::sendVipc() { + if (!vipc_server || !loaded()) { + return; + } + + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + uint64_t ts = nanos_since_boot(); + VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); + VisionIpcBufExtra extra = { + .frame_id = frame_id, + .timestamp_sof = ts, + .timestamp_eof = ts, + }; + + assert(cap.sizeInBytes() >= buf->len); + uint8_t* dst = (uint8_t*)buf->addr; + uint8_t* src = cap.bits(); + + // RGB to greyscale + memset(dst, 128, buf->len); + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + vipc_server->send(buf, &extra); + + if (frame_id % 100 == 0) { + // Write jpeg into buffer + QByteArray buffer_bytes; + QBuffer buffer(&buffer_bytes); + buffer.open(QIODevice::WriteOnly); + cap.save(&buffer, "JPG", 50); + + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); + + // Send thumbnail + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initNavThumbnail(); + thumbnaild.setFrameId(frame_id); + thumbnaild.setTimestampEof(ts); + thumbnaild.setThumbnail(buffer_kj); + pm->send("navThumbnail", msg); + } + + frame_id++; +} + +uint8_t* MapRenderer::getImage() { + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + + uint8_t* src = cap.bits(); + uint8_t* dst = new uint8_t[WIDTH * HEIGHT]; + + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + return dst; +} + +void MapRenderer::updateRoute(QList coordinates) { + if (m_map.isNull()) return; + initLayers(); + + auto route_points = coordinate_list_to_collection(coordinates); + QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); + QVariantMap navSource; + navSource["type"] = "geojson"; + navSource["data"] = QVariant::fromValue(feature); + m_map->updateSource("navSource", navSource); + m_map->setLayoutProperty("navLayer", "visibility", "visible"); +} + +void MapRenderer::initLayers() { + if (!m_map->layerExists("navLayer")) { + QVariantMap nav; + nav["id"] = "navLayer"; + nav["type"] = "line"; + nav["source"] = "navSource"; + m_map->addLayer(nav, "road-intersection"); + m_map->setPaintProperty("navLayer", "line-color", QColor("grey")); + m_map->setPaintProperty("navLayer", "line-width", 3); + m_map->setLayoutProperty("navLayer", "line-cap", "round"); + } +} + +MapRenderer::~MapRenderer() { +} + +extern "C" { + MapRenderer* map_renderer_init(char *maps_host = nullptr, char *token = nullptr) { + char *argv[] = { + (char*)"navd", + nullptr + }; + int argc = 0; + QApplication *app = new QApplication(argc, argv); + assert(app); + + QMapboxGLSettings settings; + settings.setApiBaseUrl(maps_host == nullptr ? MAPS_HOST : maps_host); + settings.setAccessToken(token == nullptr ? get_mapbox_token() : token); + + return new MapRenderer(settings, false); + } + + void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) { + inst->updatePosition({lat, lon}, bearing); + QApplication::processEvents(); + } + + void map_renderer_update_route(MapRenderer *inst, char* polyline) { + inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline))); + } + + void map_renderer_update(MapRenderer *inst) { + inst->update(); + } + + void map_renderer_process(MapRenderer *inst) { + QApplication::processEvents(); + } + + bool map_renderer_loaded(MapRenderer *inst) { + return inst->loaded(); + } + + uint8_t * map_renderer_get_image(MapRenderer *inst) { + return inst->getImage(); + } + + void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) { + delete[] buf; + } +} diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h new file mode 100644 index 00000000000000..855dc918946f68 --- /dev/null +++ b/selfdrive/navd/map_renderer.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cereal/visionipc/visionipc_server.h" +#include "cereal/messaging/messaging.h" + + +class MapRenderer : public QObject { + Q_OBJECT + +public: + MapRenderer(const QMapboxGLSettings &, bool online=true); + uint8_t* getImage(); + void update(); + bool loaded(); + ~MapRenderer(); + + +private: + std::unique_ptr ctx; + std::unique_ptr surface; + std::unique_ptr gl_functions; + std::unique_ptr fbo; + + std::unique_ptr vipc_server; + std::unique_ptr pm; + std::unique_ptr sm; + void sendVipc(); + + QMapboxGLSettings m_settings; + QScopedPointer m_map; + + void initLayers(); + + uint32_t frame_id = 0; + + QTimer* timer; + +public slots: + void updatePosition(QMapbox::Coordinate position, float bearing); + void updateRoute(QList coordinates); + void msgUpdate(); +}; diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py new file mode 100755 index 00000000000000..90006229282ea0 --- /dev/null +++ b/selfdrive/navd/map_renderer.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +# You might need to uninstall the PyQt5 pip package to avoid conflicts + +import os +import time +import numpy as np +from cffi import FFI + +from common.ffi_wrapper import suffix +from common.basedir import BASEDIR + +HEIGHT = WIDTH = 256 + + +def get_ffi(): + lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmap_renderer" + suffix()) + + ffi = FFI() + ffi.cdef(""" +void* map_renderer_init(char *maps_host, char *token); +void map_renderer_update_position(void *inst, float lat, float lon, float bearing); +void map_renderer_update_route(void *inst, char *polyline); +void map_renderer_update(void *inst); +void map_renderer_process(void *inst); +bool map_renderer_loaded(void *inst); +uint8_t* map_renderer_get_image(void *inst); +void map_renderer_free_image(void *inst, uint8_t *buf); +""") + return ffi, ffi.dlopen(lib) + + +def wait_ready(lib, renderer): + while not lib.map_renderer_loaded(renderer): + lib.map_renderer_update(renderer) + + # The main qt app is not execed, so we need to periodically process events for e.g. network requests + lib.map_renderer_process(renderer) + + time.sleep(0.01) + + +def get_image(lib, renderer): + buf = lib.map_renderer_get_image(renderer) + r = list(buf[0:WIDTH * HEIGHT]) + lib.map_renderer_free_image(renderer, buf) + + # Convert to numpy + r = np.asarray(r) + return r.reshape((WIDTH, HEIGHT)) + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + + ffi, lib = get_ffi() + renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL) + wait_ready(lib, renderer) + + geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" + lib.map_renderer_update_route(renderer, geometry.encode()) + + POSITIONS = [ + (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45), # San Diego + (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45), # Amsterdam + ] + plt.figure() + + for i, pos in enumerate(POSITIONS): + t = time.time() + lib.map_renderer_update_position(renderer, *pos) + wait_ready(lib, renderer) + + print(f"{pos} took {time.time() - t:.2f} s") + + plt.subplot(2, 2, i + 1) + plt.imshow(get_image(lib, renderer), cmap='gray') + + plt.show() diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py new file mode 100755 index 00000000000000..72874b2113d19f --- /dev/null +++ b/selfdrive/navd/navd.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python3 +import math +import os +import threading + +import requests +import numpy as np + +import cereal.messaging as messaging +from cereal import log +from common.api import Api +from common.params import Params +from common.realtime import Ratekeeper +from common.transformations.coordinates import ecef2geodetic +from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, + distance_along_geometry, maxspeed_to_ms, + minimum_distance, + parse_banner_instructions) +from system.swaglog import cloudlog + +REROUTE_DISTANCE = 25 +MANEUVER_TRANSITION_THRESHOLD = 10 +VALID_POS_STD = 50.0 + + +class RouteEngine: + def __init__(self, sm, pm): + self.sm = sm + self.pm = pm + + self.params = Params() + + # Get last gps position from params + self.last_position = coordinate_from_param("LastGPSPosition", self.params) + self.last_bearing = None + + self.gps_ok = False + self.localizer_valid = False + + self.nav_destination = None + self.step_idx = None + self.route = None + self.route_geometry = None + + self.recompute_backoff = 0 + self.recompute_countdown = 0 + + self.ui_pid = None + + if "MAPBOX_TOKEN" in os.environ: + self.mapbox_token = os.environ["MAPBOX_TOKEN"] + self.mapbox_host = "https://api.mapbox.com" + else: + try: + self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + except FileNotFoundError: + cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") + self.mapbox_token = "" + self.mapbox_host = "https://maps.comma.ai" + + def update(self): + self.sm.update(0) + + if self.sm.updated["managerState"]: + ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running] + if ui_pid: + if self.ui_pid and self.ui_pid != ui_pid[0]: + cloudlog.warning("UI restarting, sending route") + threading.Timer(5.0, self.send_route).start() + self.ui_pid = ui_pid[0] + + self.update_location() + self.recompute_route() + self.send_instruction() + + def update_location(self): + location = self.sm['liveLocationKalman'] + laikad = self.sm['gnssMeasurements'] + + locationd_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + laikad_valid = laikad.positionECEF.valid and np.linalg.norm(laikad.positionECEF.std) < VALID_POS_STD + + self.localizer_valid = locationd_valid or laikad_valid + self.gps_ok = location.gpsOK or laikad_valid + + if locationd_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + elif laikad_valid: + geodetic = ecef2geodetic(laikad.positionECEF.value) + self.last_position = Coordinate(geodetic[0], geodetic[1]) + self.last_bearing = None + + def recompute_route(self): + if self.last_position is None: + return + + new_destination = coordinate_from_param("NavDestination", self.params) + if new_destination is None: + self.clear_route() + return + + should_recompute = self.should_recompute() + if new_destination != self.nav_destination: + cloudlog.warning(f"Got new destination from NavDestination param {new_destination}") + should_recompute = True + + # Don't recompute when GPS drifts in tunnels + if not self.gps_ok and self.step_idx is not None: + return + + if self.recompute_countdown == 0 and should_recompute: + self.recompute_countdown = 2**self.recompute_backoff + self.recompute_backoff = min(6, self.recompute_backoff + 1) + self.calculate_route(new_destination) + else: + self.recompute_countdown = max(0, self.recompute_countdown - 1) + + def calculate_route(self, destination): + cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") + self.nav_destination = destination + + params = { + 'access_token': self.mapbox_token, + 'annotations': 'maxspeed', + 'geometries': 'geojson', + 'overview': 'full', + 'steps': 'true', + 'banner_instructions': 'true', + 'alternatives': 'false', + } + + if self.last_bearing is not None: + params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" + + url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' + try: + resp = requests.get(url, params=params, timeout=10) + resp.raise_for_status() + + r = resp.json() + if len(r['routes']): + self.route = r['routes'][0]['legs'][0]['steps'] + self.route_geometry = [] + + maxspeed_idx = 0 + maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed'] + + # Convert coordinates + for step in self.route: + coords = [] + + for c in step['geometry']['coordinates']: + coord = Coordinate.from_mapbox_tuple(c) + + # Last step does not have maxspeed + if (maxspeed_idx < len(maxspeeds)): + maxspeed = maxspeeds[maxspeed_idx] + if ('unknown' not in maxspeed) and ('none' not in maxspeed): + coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeed) + + coords.append(coord) + maxspeed_idx += 1 + + self.route_geometry.append(coords) + maxspeed_idx -= 1 # Every segment ends with the same coordinate as the start of the next + + self.step_idx = 0 + else: + cloudlog.warning("Got empty route response") + self.clear_route() + + except requests.exceptions.RequestException: + cloudlog.exception("failed to get route") + self.clear_route() + + self.send_route() + + def send_instruction(self): + msg = messaging.new_message('navInstruction') + + if self.step_idx is None: + msg.valid = False + self.pm.send('navInstruction', msg) + return + + step = self.route[self.step_idx] + geometry = self.route_geometry[self.step_idx] + along_geometry = distance_along_geometry(geometry, self.last_position) + distance_to_maneuver_along_geometry = step['distance'] - along_geometry + + # Current instruction + msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry + parse_banner_instructions(msg.navInstruction, step['bannerInstructions'], distance_to_maneuver_along_geometry) + + # Compute total remaining time and distance + remaining = 1.0 - along_geometry / max(step['distance'], 1) + total_distance = step['distance'] * remaining + total_time = step['duration'] * remaining + total_time_typical = step['duration_typical'] * remaining + + # Add up totals for future steps + for i in range(self.step_idx + 1, len(self.route)): + total_distance += self.route[i]['distance'] + total_time += self.route[i]['duration'] + total_time_typical += self.route[i]['duration_typical'] + + msg.navInstruction.distanceRemaining = total_distance + msg.navInstruction.timeRemaining = total_time + msg.navInstruction.timeRemainingTypical = total_time_typical + + # Speed limit + closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position)) + if closest_idx > 0: + # If we are not past the closest point, show previous + if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): + closest = geometry[closest_idx - 1] + + if ('maxspeed' in closest.annotations) and self.localizer_valid: + msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + + # Speed limit sign type + if 'speedLimitSign' in step: + if step['speedLimitSign'] == 'mutcd': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd + elif step['speedLimitSign'] == 'vienna': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna + + self.pm.send('navInstruction', msg) + + # Transition to next route segment + if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD: + if self.step_idx + 1 < len(self.route): + self.step_idx += 1 + self.recompute_backoff = 0 + self.recompute_countdown = 0 + else: + cloudlog.warning("Destination reached") + Params().remove("NavDestination") + + # Clear route if driving away from destination + dist = self.nav_destination.distance_to(self.last_position) + if dist > REROUTE_DISTANCE: + self.clear_route() + + def send_route(self): + coords = [] + + if self.route is not None: + for path in self.route_geometry: + coords += [c.as_dict() for c in path] + + msg = messaging.new_message('navRoute') + msg.navRoute.coordinates = coords + self.pm.send('navRoute', msg) + + def clear_route(self): + self.route = None + self.route_geometry = None + self.step_idx = None + self.nav_destination = None + + def should_recompute(self): + if self.step_idx is None or self.route is None: + return True + + # Don't recompute in last segment, assume destination is reached + if self.step_idx == len(self.route) - 1: + return False + + # Compute closest distance to all line segments in the current path + min_d = REROUTE_DISTANCE + 1 + path = self.route_geometry[self.step_idx] + for i in range(len(path) - 1): + a = path[i] + b = path[i + 1] + + if a.distance_to(b) < 1.0: + continue + + min_d = min(min_d, minimum_distance(a, b, self.last_position)) + + return min_d > REROUTE_DISTANCE + + # TODO: Check for going wrong way in segment + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'gnssMeasurements', 'managerState']) + if pm is None: + pm = messaging.PubMaster(['navInstruction', 'navRoute']) + + rk = Ratekeeper(1.0) + route_engine = RouteEngine(sm, pm) + while True: + route_engine.update() + rk.keep_time() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/pandad/.gitignore b/selfdrive/pandad/.gitignore deleted file mode 100644 index f7226cdb8760a9..00000000000000 --- a/selfdrive/pandad/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -pandad -pandad_api_impl.cpp -tests/test_pandad_usbprotocol diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript deleted file mode 100644 index 5e0b782c1e1472..00000000000000 --- a/selfdrive/pandad/SConscript +++ /dev/null @@ -1,9 +0,0 @@ -Import('env', 'common', 'messaging') - -libs = ['usb-1.0', common, messaging, 'pthread'] -panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) - -env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) - -if GetOption('extras'): - env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/pandad/__init__.py b/selfdrive/pandad/__init__.py deleted file mode 100644 index 0c17e886a2ed30..00000000000000 --- a/selfdrive/pandad/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp, can_capnp_to_list -assert can_list_to_can_capnp -assert can_capnp_to_list diff --git a/selfdrive/pandad/main.cc b/selfdrive/pandad/main.cc deleted file mode 100644 index b63d884a45e3f5..00000000000000 --- a/selfdrive/pandad/main.cc +++ /dev/null @@ -1,22 +0,0 @@ -#include - -#include "selfdrive/pandad/pandad.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "system/hardware/hw.h" - -int main(int argc, char *argv[]) { - LOGW("starting pandad"); - - if (!Hardware::PC()) { - int err; - err = util::set_realtime_priority(54); - assert(err == 0); - err = util::set_core_affinity({3}); - assert(err == 0); - } - - std::vector serials(argv + 1, argv + argc); - pandad_main_thread(serials); - return 0; -} diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc deleted file mode 100644 index 93e139f0ec173d..00000000000000 --- a/selfdrive/pandad/panda.cc +++ /dev/null @@ -1,312 +0,0 @@ -#include "selfdrive/pandad/panda.h" - -#include - -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/swaglog.h" -#include "common/util.h" - -const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr; - -Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // try USB first, then SPI - try { - handle = std::make_unique(serial); - LOGW("connected to %s over USB", serial.c_str()); - } catch (std::exception &e) { -#ifndef __APPLE__ - handle = std::make_unique(serial); - LOGW("connected to %s over SPI", serial.c_str()); -#else - throw e; -#endif - } - - hw_type = get_hw_type(); - can_reset_communications(); -} - -bool Panda::connected() { - return handle->connected; -} - -bool Panda::comms_healthy() { - return handle->comms_healthy; -} - -std::string Panda::hw_serial() { - return handle->hw_serial; -} - -std::vector Panda::list(bool usb_only) { - std::vector serials = PandaUsbHandle::list(); - -#ifndef __APPLE__ - if (!usb_only) { - for (const auto &s : PandaSpiHandle::list()) { - if (std::find(serials.begin(), serials.end(), s) == serials.end()) { - serials.push_back(s); - } - } - } -#endif - - return serials; -} - -void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { - handle->control_write(0xdc, (uint16_t)safety_model, safety_param); -} - -void Panda::set_alternative_experience(uint16_t alternative_experience) { - handle->control_write(0xdf, alternative_experience, 0); -} - -std::string Panda::serial_read(int port_number) { - std::string ret; - char buffer[USBPACKET_MAX_SIZE] = {}; - - while (true) { - int bytes_read = handle->control_read(0xe0, port_number, 0, (unsigned char *)buffer, USBPACKET_MAX_SIZE); - if (bytes_read <= 0) { - break; - } - ret.append(buffer, bytes_read); - } - - return ret; -} - -void Panda::set_uart_baud(int uart, int rate) { - handle->control_write(0xe4, uart, int(rate / 300)); -} - -cereal::PandaState::PandaType Panda::get_hw_type() { - unsigned char hw_query[1] = {0}; - - handle->control_read(0xc1, 0, 0, hw_query, 1); - return (cereal::PandaState::PandaType)(hw_query[0]); -} - -void Panda::set_fan_speed(uint16_t fan_speed) { - handle->control_write(0xb1, fan_speed, 0); -} - -uint16_t Panda::get_fan_speed() { - uint16_t fan_speed_rpm = 0; - handle->control_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); - return fan_speed_rpm; -} - -void Panda::set_ir_pwr(uint16_t ir_pwr) { - handle->control_write(0xb0, ir_pwr, 0); -} - -std::optional Panda::get_state() { - health_t health {0}; - int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); - return err >= 0 ? std::make_optional(health) : std::nullopt; -} - -std::optional Panda::get_can_state(uint16_t can_number) { - can_health_t can_health {0}; - int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health)); - return err >= 0 ? std::make_optional(can_health) : std::nullopt; -} - -void Panda::set_loopback(bool loopback) { - handle->control_write(0xe5, loopback, 0); -} - -std::optional> Panda::get_firmware_version() { - std::vector fw_sig_buf(128); - int read_1 = handle->control_read(0xd3, 0, 0, &fw_sig_buf[0], 64); - int read_2 = handle->control_read(0xd4, 0, 0, &fw_sig_buf[64], 64); - return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt; -} - -std::optional Panda::get_serial() { - char serial_buf[17] = {'\0'}; - int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); - return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; -} - -bool Panda::up_to_date() { - if (auto fw_sig = get_firmware_version()) { - for (auto fn : { "panda.bin.signed", "panda_h7.bin.signed" }) { - auto content = util::read_file(std::string("../../panda/board/obj/") + fn); - if (content.size() >= fw_sig->size() && - memcmp(content.data() + content.size() - fw_sig->size(), fw_sig->data(), fw_sig->size()) == 0) { - return true; - } - } - } - return false; -} - -void Panda::set_power_saving(bool power_saving) { - handle->control_write(0xe7, power_saving, 0); -} - -void Panda::enable_deepsleep() { - handle->control_write(0xfb, 0, 0); -} - -void Panda::send_heartbeat(bool engaged) { - handle->control_write(0xf3, engaged, 0); -} - -void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { - handle->control_write(0xde, bus, (speed * 10)); -} - -void Panda::set_can_fd_auto(uint16_t bus, bool enabled) { - handle->control_write(0xe8, bus, enabled); -} - -void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { - handle->control_write(0xf9, bus, (speed * 10)); -} - -void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) { - handle->control_write(0xfc, bus, non_iso); -} - -static uint8_t len_to_dlc(uint8_t len) { - if (len <= 8) { - return len; - } - if (len <= 24) { - return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0); - } else { - return 11 + (len / 16) + ((len % 16) ? 1 : 0); - } -} - -void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, - std::function write_func) { - int32_t pos = 0; - uint8_t send_buf[2 * USB_TX_SOFT_LIMIT]; - - for (const auto &cmsg : can_data_list) { - // check if the message is intended for this panda - uint8_t bus = cmsg.getSrc(); - if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_OFFSET)) { - continue; - } - auto can_data = cmsg.getDat(); - uint8_t data_len_code = len_to_dlc(can_data.size()); - assert(can_data.size() <= 64); - assert(can_data.size() == dlc_to_len[data_len_code]); - - can_header header = {}; - header.addr = cmsg.getAddress(); - header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; - header.data_len_code = data_len_code; - header.bus = bus - bus_offset; - header.checksum = 0; - - memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header)); - memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size()); - uint32_t msg_size = sizeof(can_header) + can_data.size(); - - // set checksum - ((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size); - - pos += msg_size; - - if (pos >= USB_TX_SOFT_LIMIT) { - write_func(send_buf, pos); - pos = 0; - } - } - - // send remaining packets - if (pos > 0) write_func(send_buf, pos); -} - -void Panda::can_send(const capnp::List::Reader &can_data_list) { - pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) { - handle->bulk_write(3, data, size, 5); - }); -} - -bool Panda::can_receive(std::vector& out_vec) { - // Check if enough space left in buffer to store RECV_SIZE data - assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); - - int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); - if (!comms_healthy()) { - return false; - } - - if (PANDAD_MAXOUT) { - static uint8_t junk[RECV_SIZE]; - handle->bulk_read(0xab, junk, RECV_SIZE - recv); - } - - bool ret = true; - if (recv > 0) { - receive_buffer_size += recv; - ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); - } - return ret; -} - -void Panda::can_reset_communications() { - handle->control_write(0xc0, 0, 0); -} - -bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec) { - int pos = 0; - - while (pos <= size - sizeof(can_header)) { - can_header header; - memcpy(&header, &data[pos], sizeof(can_header)); - - const uint8_t data_len = dlc_to_len[header.data_len_code]; - if (pos + sizeof(can_header) + data_len > size) { - // we don't have all the data for this message yet - break; - } - - if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) { - LOGE("Panda CAN checksum failed"); - size = 0; - can_reset_communications(); - return false; - } - - can_frame &canData = out_vec.emplace_back(); - canData.address = header.addr; - canData.src = header.bus + bus_offset; - if (header.rejected) { - canData.src += CAN_REJECTED_BUS_OFFSET; - } - if (header.returned) { - canData.src += CAN_RETURNED_BUS_OFFSET; - } - - canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); - - pos += sizeof(can_header) + data_len; - } - - // move the overflowing data to the beginning of the buffer for the next round - memmove(data, &data[pos], size - pos); - size -= pos; - - return true; -} - -uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) { - uint8_t checksum = 0U; - for (uint32_t i = 0U; i < len; i++) { - checksum ^= data[i]; - } - return checksum; -} diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h deleted file mode 100644 index 5cbce44f281cbe..00000000000000 --- a/selfdrive/pandad/panda.h +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/gen/cpp/car.capnp.h" -#include "cereal/gen/cpp/log.capnp.h" -#include "panda/board/health.h" -#include "panda/board/can.h" -#include "selfdrive/pandad/panda_comms.h" - -#define USB_TX_SOFT_LIMIT (0x100U) -#define USBPACKET_MAX_SIZE (0x40) - -#define RECV_SIZE (0x4000U) - -#define CAN_REJECTED_BUS_OFFSET 0xC0U -#define CAN_RETURNED_BUS_OFFSET 0x80U - -#define PANDA_BUS_OFFSET 4 - -struct __attribute__((packed)) can_header { - uint8_t reserved : 1; - uint8_t bus : 3; - uint8_t data_len_code : 4; - uint8_t rejected : 1; - uint8_t returned : 1; - uint8_t extended : 1; - uint32_t addr : 29; - uint8_t checksum : 8; -}; - -struct can_frame { - long address; - std::string dat; - long src; -}; - - -class Panda { -private: - std::unique_ptr handle; - -public: - Panda(std::string serial="", uint32_t bus_offset=0); - - cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; - const uint32_t bus_offset; - - bool connected(); - bool comms_healthy(); - std::string hw_serial(); - - // Static functions - static std::vector list(bool usb_only=false); - - // Panda functionality - cereal::PandaState::PandaType get_hw_type(); - void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); - void set_alternative_experience(uint16_t alternative_experience); - std::string serial_read(int port_number = 0); - void set_uart_baud(int uart, int rate); - void set_fan_speed(uint16_t fan_speed); - uint16_t get_fan_speed(); - void set_ir_pwr(uint16_t ir_pwr); - std::optional get_state(); - std::optional get_can_state(uint16_t can_number); - void set_loopback(bool loopback); - std::optional> get_firmware_version(); - bool up_to_date(); - std::optional get_serial(); - void set_power_saving(bool power_saving); - void enable_deepsleep(); - void send_heartbeat(bool engaged); - void set_can_speed_kbps(uint16_t bus, uint16_t speed); - void set_can_fd_auto(uint16_t bus, bool enabled); - void set_data_speed_kbps(uint16_t bus, uint16_t speed); - void set_canfd_non_iso(uint16_t bus, bool non_iso); - void can_send(const capnp::List::Reader &can_data_list); - bool can_receive(std::vector& out_vec); - void can_reset_communications(); - -protected: - // for unit tests - uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64]; - uint32_t receive_buffer_size = 0; - - Panda(uint32_t bus_offset) : bus_offset(bus_offset) {} - void pack_can_buffer(const capnp::List::Reader &can_data_list, - std::function write_func); - bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); - uint8_t calculate_checksum(uint8_t *data, uint32_t len); -}; diff --git a/selfdrive/pandad/panda_comms.cc b/selfdrive/pandad/panda_comms.cc deleted file mode 100644 index 8a20f397d31d32..00000000000000 --- a/selfdrive/pandad/panda_comms.cc +++ /dev/null @@ -1,227 +0,0 @@ -#include "selfdrive/pandad/panda.h" - -#include -#include -#include - -#include "common/swaglog.h" - -static libusb_context *init_usb_ctx() { - libusb_context *context = nullptr; - int err = libusb_init(&context); - if (err != 0) { - LOGE("libusb initialization error"); - return nullptr; - } - -#if LIBUSB_API_VERSION >= 0x01000106 - libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); -#else - libusb_set_debug(context, 3); -#endif - return context; -} - -PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - int err = 0; - ctx = init_usb_ctx(); - if (!ctx) { goto fail; } - - // connect by serial - num_devices = libusb_get_device_list(ctx, &dev_list); - if (num_devices < 0) { goto fail; } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device_descriptor desc; - libusb_get_device_descriptor(dev_list[i], &desc); - if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { - int ret = libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL || ret < 0) { goto fail; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - if (ret < 0) { goto fail; } - - hw_serial = std::string((char *)desc_serial, ret); - if (serial.empty() || serial == hw_serial) { - break; - } - libusb_close(dev_handle); - dev_handle = NULL; - } - } - if (dev_handle == NULL) goto fail; - libusb_free_device_list(dev_list, 1); - dev_list = nullptr; - - if (libusb_kernel_driver_active(dev_handle, 0) == 1) { - libusb_detach_kernel_driver(dev_handle, 0); - } - - err = libusb_set_configuration(dev_handle, 1); - if (err != 0) { goto fail; } - - err = libusb_claim_interface(dev_handle, 0); - if (err != 0) { goto fail; } - - return; - -fail: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - cleanup(); - throw std::runtime_error("Error connecting to panda"); -} - -PandaUsbHandle::~PandaUsbHandle() { - std::lock_guard lk(hw_lock); - cleanup(); - connected = false; -} - -void PandaUsbHandle::cleanup() { - if (dev_handle) { - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - } - - if (ctx) { - libusb_exit(ctx); - } -} - -std::vector PandaUsbHandle::list() { - static std::unique_ptr context(init_usb_ctx(), libusb_exit); - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - std::vector serials; - if (!context) { return serials; } - - num_devices = libusb_get_device_list(context.get(), &dev_list); - if (num_devices < 0) { - LOGE("libusb can't get device list"); - goto finish; - } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device *device = dev_list[i]; - libusb_device_descriptor desc; - libusb_get_device_descriptor(device, &desc); - if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { - libusb_device_handle *handle = NULL; - int ret = libusb_open(device, &handle); - if (ret < 0) { goto finish; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - libusb_close(handle); - if (ret < 0) { goto finish; } - - serials.push_back(std::string((char *)desc_serial, ret)); - } - } - -finish: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - return serials; -} - -void PandaUsbHandle::handle_usb_issue(int err, const char func[]) { - LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); - if (err == LIBUSB_ERROR_NO_DEVICE) { - LOGE("lost connection"); - connected = false; - } - // TODO: check other errors, is simply retrying okay? -} - -int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(hw_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(hw_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(hw_lock); - do { - // Try sending can messages. If the receive buffer on the panda is full it will NAK - // and libusb will try again. After 5ms, it will time out. We will drop the messages. - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - LOGW("Transmit buffer full"); - break; - } else if (err != 0 || length != transferred) { - handle_usb_issue(err, __func__); - } - } while (err != 0 && connected); - - return transferred; -} - -int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(hw_lock); - - do { - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - break; // timeout is okay to exit, recv still happened - } else if (err == LIBUSB_ERROR_OVERFLOW) { - comms_healthy = false; - LOGE_100("overflow got 0x%x", transferred); - } else if (err != 0) { - handle_usb_issue(err, __func__); - } - - } while (err != 0 && connected); - - return transferred; -} diff --git a/selfdrive/pandad/panda_comms.h b/selfdrive/pandad/panda_comms.h deleted file mode 100644 index 9c452faf6dad13..00000000000000 --- a/selfdrive/pandad/panda_comms.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#ifndef __APPLE__ -#include -#endif - -#include - - -#define TIMEOUT 0 -#define SPI_BUF_SIZE 2048 - - -// comms base class -class PandaCommsHandle { -public: - PandaCommsHandle(std::string serial) {} - virtual ~PandaCommsHandle() {} - virtual void cleanup() = 0; - - std::string hw_serial; - std::atomic connected = true; - std::atomic comms_healthy = true; - static std::vector list(); - - // HW communication - virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0; - virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; - virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; - virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; -}; - -class PandaUsbHandle : public PandaCommsHandle { -public: - PandaUsbHandle(std::string serial); - ~PandaUsbHandle(); - int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); - int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); - int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - void cleanup(); - - static std::vector list(); - -private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::recursive_mutex hw_lock; - void handle_usb_issue(int err, const char func[]); -}; - -#ifndef __APPLE__ -struct __attribute__((packed)) spi_header { - uint8_t sync; - uint8_t endpoint; - uint16_t tx_len; - uint16_t max_rx_len; -}; - -class PandaSpiHandle : public PandaCommsHandle { -public: - PandaSpiHandle(std::string serial); - ~PandaSpiHandle(); - int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); - int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); - int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - void cleanup(); - - static std::vector list(); - -private: - int spi_fd = -1; - uint8_t tx_buf[SPI_BUF_SIZE]; - uint8_t rx_buf[SPI_BUF_SIZE]; - inline static std::recursive_mutex hw_lock; - - int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length); - int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout); - int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); - int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); - int lltransfer(spi_ioc_transfer &t); - - spi_header header; - uint32_t xfer_count = 0; -}; -#endif diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc deleted file mode 100644 index b089503417d646..00000000000000 --- a/selfdrive/pandad/panda_safety.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include "selfdrive/pandad/pandad.h" -#include "cereal/messaging/messaging.h" -#include "common/swaglog.h" - -void PandaSafety::configureSafetyMode(bool is_onroad) { - if (is_onroad && !safety_configured_) { - updateMultiplexingMode(); - - auto car_params = fetchCarParams(); - if (!car_params.empty()) { - LOGW("got %lu bytes CarParams", car_params.size()); - setSafetyMode(car_params); - safety_configured_ = true; - } - } else if (!is_onroad) { - initialized_ = false; - safety_configured_ = false; - log_once_ = false; - } -} - -void PandaSafety::updateMultiplexingMode() { - // Initialize to ELM327 without OBD multiplexing for initial fingerprinting - if (!initialized_) { - prev_obd_multiplexing_ = false; - for (int i = 0; i < pandas_.size(); ++i) { - pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); - } - initialized_ = true; - } - - // Switch between multiplexing modes based on the OBD multiplexing request - bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled"); - if (obd_multiplexing_requested != prev_obd_multiplexing_) { - for (int i = 0; i < pandas_.size(); ++i) { - const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; - pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); - } - prev_obd_multiplexing_ = obd_multiplexing_requested; - params_.putBool("ObdMultiplexingChanged", true); - } -} - -std::string PandaSafety::fetchCarParams() { - if (!params_.getBool("FirmwareQueryDone")) { - return {}; - } - - if (!log_once_) { - LOGW("Finished FW query, Waiting for params to set safety model"); - log_once_ = true; - } - - if (!params_.getBool("ControlsReady")) { - return {}; - } - return params_.get("CarParams"); -} - -void PandaSafety::setSafetyMode(const std::string ¶ms_string) { - AlignedBuffer aligned_buf; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - - auto safety_configs = car_params.getSafetyConfigs(); - uint16_t alternative_experience = car_params.getAlternativeExperience(); - - for (int i = 0; i < pandas_.size(); ++i) { - // Default to SILENT safety model if not specified - cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT; - uint16_t safety_param = 0U; - if (i < safety_configs.size()) { - safety_model = safety_configs[i].getSafetyModel(); - safety_param = safety_configs[i].getSafetyParam(); - } - - LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); - pandas_[i]->set_alternative_experience(alternative_experience); - pandas_[i]->set_safety_model(safety_model, safety_param); - } -} diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc deleted file mode 100644 index 2fd4a4def24e84..00000000000000 --- a/selfdrive/pandad/pandad.cc +++ /dev/null @@ -1,538 +0,0 @@ -#include "selfdrive/pandad/pandad.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/gen/cpp/car.capnp.h" -#include "cereal/messaging/messaging.h" -#include "cereal/services.h" -#include "common/ratekeeper.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" -#include "system/hardware/hw.h" - -// -- Multi-panda conventions -- -// Ordering: -// - The internal panda will always be the first panda -// - Consecutive pandas will be sorted based on panda type, and then serial number -// Connecting: -// - If a panda connection is dropped, pandad will reconnect to all pandas -// - If a panda is added, we will only reconnect when we are offroad -// CAN buses: -// - Each panda will have its block of 4 buses. E.g.: the second panda will use -// bus numbers 4, 5, 6 and 7 -// - The internal panda will always be used for accessing the OBD2 port, -// and thus firmware queries -// Safety: -// - SafetyConfig is a list, which is mapped to the connected pandas -// - If there are more pandas connected than there are SafetyConfigs, -// the excess pandas will remain in "silent" or "noOutput" mode -// Ignition: -// - If any of the ignition sources in any panda is high, ignition is high - -#define MAX_IR_PANDA_VAL 50 -#define CUTOFF_IL 400 -#define SATURATE_IL 1000 - -ExitHandler do_exit; - -bool check_all_connected(const std::vector &pandas) { - for (const auto& panda : pandas) { - if (!panda->connected()) { - do_exit = true; - return false; - } - } - return true; -} - -Panda *connect(std::string serial="", uint32_t index=0) { - std::unique_ptr panda; - try { - panda = std::make_unique(serial, (index * PANDA_BUS_OFFSET)); - } catch (std::exception &e) { - return nullptr; - } - - // common panda config - if (getenv("BOARDD_LOOPBACK")) { - panda->set_loopback(true); - } - //panda->enable_deepsleep(); - - for (int i = 0; i < PANDA_CAN_CNT; i++) { - panda->set_can_fd_auto(i, true); - } - - if (!panda->up_to_date() && !getenv("BOARDD_SKIP_FW_CHECK")) { - throw std::runtime_error("Panda firmware out of date. Run pandad.py to update."); - } - - return panda.release(); -} - -void can_send_thread(std::vector pandas, bool fake_send) { - util::set_thread_name("pandad_can_send"); - - AlignedBuffer aligned_buf; - std::unique_ptr context(Context::create()); - std::unique_ptr subscriber(SubSocket::create(context.get(), "sendcan", "127.0.0.1", false, true, services.at("sendcan").queue_size)); - assert(subscriber != NULL); - subscriber->setTimeout(100); - - // run as fast as messages come in - while (!do_exit && check_all_connected(pandas)) { - std::unique_ptr msg(subscriber->receive()); - if (!msg) { - continue; - } - - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); - cereal::Event::Reader event = cmsg.getRoot(); - - // Don't send if older than 1 second - if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { - for (const auto& panda : pandas) { - LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); - panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); - } - } else { - LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime()); - } - } -} - -void can_recv(std::vector &pandas, PubMaster *pm) { - static std::vector raw_can_data; - { - bool comms_healthy = true; - raw_can_data.clear(); - for (const auto& panda : pandas) { - comms_healthy &= panda->can_receive(raw_can_data); - } - - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(comms_healthy); - auto canData = evt.initCan(raw_can_data.size()); - for (size_t i = 0; i < raw_can_data.size(); ++i) { - canData[i].setAddress(raw_can_data[i].address); - canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size())); - canData[i].setSrc(raw_can_data[i].src); - } - pm->send("can", msg); - } -} - -void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) { - ps.setVoltage(health.voltage_pkt); - ps.setCurrent(health.current_pkt); - ps.setUptime(health.uptime_pkt); - ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); - ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); - ps.setIgnitionLine(health.ignition_line_pkt); - ps.setIgnitionCan(health.ignition_can_pkt); - ps.setControlsAllowed(health.controls_allowed_pkt); - ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); - ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); - ps.setPandaType(hw_type); - ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); - ps.setSafetyParam(health.safety_param_pkt); - ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); - ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); - ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); - ps.setAlternativeExperience(health.alternative_experience_pkt); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); - ps.setInterruptLoad(health.interrupt_load_pkt); - ps.setFanPower(health.fan_power); - ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt)); - ps.setSpiErrorCount(health.spi_error_count_pkt); - ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); - ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); -} - -void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) { - cs.setBusOff((bool)can_health.bus_off); - cs.setBusOffCnt(can_health.bus_off_cnt); - cs.setErrorWarning((bool)can_health.error_warning); - cs.setErrorPassive((bool)can_health.error_passive); - cs.setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error)); - cs.setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error)); - cs.setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error)); - cs.setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error)); - cs.setReceiveErrorCnt(can_health.receive_error_cnt); - cs.setTransmitErrorCnt(can_health.transmit_error_cnt); - cs.setTotalErrorCnt(can_health.total_error_cnt); - cs.setTotalTxLostCnt(can_health.total_tx_lost_cnt); - cs.setTotalRxLostCnt(can_health.total_rx_lost_cnt); - cs.setTotalTxCnt(can_health.total_tx_cnt); - cs.setTotalRxCnt(can_health.total_rx_cnt); - cs.setTotalFwdCnt(can_health.total_fwd_cnt); - cs.setCanSpeed(can_health.can_speed); - cs.setCanDataSpeed(can_health.can_data_speed); - cs.setCanfdEnabled(can_health.canfd_enabled); - cs.setBrsEnabled(can_health.brs_enabled); - cs.setCanfdNonIso(can_health.canfd_non_iso); - cs.setIrq0CallRate(can_health.irq0_call_rate); - cs.setIrq1CallRate(can_health.irq1_call_rate); - cs.setIrq2CallRate(can_health.irq2_call_rate); - cs.setCanCoreResetCnt(can_health.can_core_reset_cnt); -} - -std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool is_onroad, bool spoofing_started) { - bool ignition_local = false; - const uint32_t pandas_cnt = pandas.size(); - - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - auto pss = evt.initPandaStates(pandas_cnt); - - std::vector pandaStates; - pandaStates.reserve(pandas_cnt); - - std::vector> pandaCanStates; - pandaCanStates.reserve(pandas_cnt); - - const bool red_panda_comma_three = (pandas.size() == 2) && - (pandas[0]->hw_type == cereal::PandaState::PandaType::DOS) && - (pandas[1]->hw_type == cereal::PandaState::PandaType::RED_PANDA); - - for (const auto& panda : pandas){ - auto health_opt = panda->get_state(); - if (!health_opt) { - return std::nullopt; - } - - health_t health = *health_opt; - - std::array can_health{}; - for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) { - auto can_health_opt = panda->get_can_state(i); - if (!can_health_opt) { - return std::nullopt; - } - can_health[i] = *can_health_opt; - } - pandaCanStates.push_back(can_health); - - if (spoofing_started) { - health.ignition_line_pkt = 1; - } - - // on comma three setups with a red panda, the dos can - // get false positive ignitions due to the harness box - // without a harness connector, so ignore it - if (red_panda_comma_three && (panda->hw_type == cereal::PandaState::PandaType::DOS)) { - health.ignition_line_pkt = 0; - } - - ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); - - pandaStates.push_back(health); - } - - for (uint32_t i = 0; i < pandas_cnt; i++) { - auto panda = pandas[i]; - const auto &health = pandaStates[i]; - - // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node - if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } - - bool power_save_desired = !ignition_local; - if (health.power_save_enabled_pkt != power_save_desired) { - panda->set_power_saving(power_save_desired); - } - - // set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect - bool should_close_relay = !ignition_local || !is_onroad; - if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } - - if (!panda->comms_healthy()) { - evt.setValid(false); - } - - auto ps = pss[i]; - fill_panda_state(ps, panda->hw_type, health); - - auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; - for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) { - fill_panda_can_state(cs[j], pandaCanStates[i][j]); - } - - // Convert faults bitset to capnp list - std::bitset fault_bits(health.faults_pkt); - auto faults = ps.initFaults(fault_bits.count()); - - size_t j = 0; - for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) { - if (fault_bits.test(f)) { - faults.set(j, cereal::PandaState::FaultType(f)); - j++; - } - } - } - - pm->send("pandaStates", msg); - return ignition_local; -} - -void send_peripheral_state(Panda *panda, PubMaster *pm) { - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy()); - - auto ps = evt.initPeripheralState(); - ps.setPandaType(panda->hw_type); - - double read_time = millis_since_boot(); - ps.setVoltage(Hardware::get_voltage()); - ps.setCurrent(Hardware::get_current()); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } - - // fall back to panda's voltage and current measurement - if (ps.getVoltage() == 0 && ps.getCurrent() == 0) { - auto health_opt = panda->get_state(); - if (health_opt) { - health_t health = *health_opt; - ps.setVoltage(health.voltage_pkt); - ps.setCurrent(health.current_pkt); - } - } - - uint16_t fan_speed_rpm = panda->get_fan_speed(); - ps.setFanSpeedRpm(fan_speed_rpm); - - pm->send("peripheralState", msg); -} - -void process_panda_state(std::vector &pandas, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) { - std::vector connected_serials; - for (Panda *p : pandas) { - connected_serials.push_back(p->hw_serial()); - } - - { - auto ignition_opt = send_panda_states(pm, pandas, is_onroad, spoofing_started); - if (!ignition_opt) { - LOGE("Failed to get ignition_opt"); - return; - } - - // check if we should have pandad reconnect - if (!ignition_opt.value()) { - bool comms_healthy = true; - for (const auto &panda : pandas) { - comms_healthy &= panda->comms_healthy(); - } - - if (!comms_healthy) { - LOGE("Reconnecting, communication to pandas not healthy"); - do_exit = true; - - } else { - // check for new pandas - for (std::string &s : Panda::list(true)) { - if (!std::count(connected_serials.begin(), connected_serials.end(), s)) { - LOGW("Reconnecting to new panda: %s", s.c_str()); - do_exit = true; - break; - } - } - } - } - - for (const auto &panda : pandas) { - panda->send_heartbeat(engaged); - } - } -} - -void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { - static Params params; - static SubMaster sm({"deviceState", "driverCameraState"}); - - static uint64_t last_driver_camera_t = 0; - static uint16_t prev_fan_speed = 999; - static int ir_pwr = 0; - static int prev_ir_pwr = 999; - static uint32_t prev_frame_id = UINT32_MAX; - static bool driver_view = false; - - // TODO: can we merge these? - static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); - static FirstOrderFilter integ_lines_filter_driver_view(0, 5.0, 0.05); - - { - sm.update(0); - if (sm.updated("deviceState") && !no_fan_control) { - // Fan speed - uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); - if (fan_speed != prev_fan_speed || sm.frame % 100 == 0) { - panda->set_fan_speed(fan_speed); - prev_fan_speed = fan_speed; - } - } - - if (sm.updated("driverCameraState")) { - auto event = sm["driverCameraState"]; - int cur_integ_lines = event.getDriverCameraState().getIntegLines(); - - // reset the filter when camerad restarts - if (event.getDriverCameraState().getFrameId() < prev_frame_id) { - integ_lines_filter.reset(0); - integ_lines_filter_driver_view.reset(0); - driver_view = params.getBool("IsDriverViewEnabled"); - } - prev_frame_id = event.getDriverCameraState().getFrameId(); - - cur_integ_lines = (driver_view ? integ_lines_filter_driver_view : integ_lines_filter).update(cur_integ_lines); - last_driver_camera_t = event.getLogMonoTime(); - - if (cur_integ_lines <= CUTOFF_IL) { - ir_pwr = 0; - } else if (cur_integ_lines > SATURATE_IL) { - ir_pwr = 100; - } else { - ir_pwr = 100 * (cur_integ_lines - CUTOFF_IL) / (SATURATE_IL - CUTOFF_IL); - } - } - - // Disable IR on input timeout - if (nanos_since_boot() - last_driver_camera_t > 1e9) { - ir_pwr = 0; - } - - if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0) { - int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL); - panda->set_ir_pwr(ir_panda); - Hardware::set_ir_power(ir_pwr); - prev_ir_pwr = ir_pwr; - } - } -} - -void pandad_run(std::vector &pandas) { - const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr; - const bool spoofing_started = getenv("STARTED") != nullptr; - const bool fake_send = getenv("FAKESEND") != nullptr; - - // Start the CAN send thread - std::thread send_thread(can_send_thread, pandas, fake_send); - - Params params; - RateKeeper rk("pandad", 100); - SubMaster sm({"selfdriveState"}); - PubMaster pm({"can", "pandaStates", "peripheralState"}); - PandaSafety panda_safety(pandas); - Panda *peripheral_panda = pandas[0]; - bool engaged = false; - bool is_onroad = false; - - // Main loop: receive CAN data and process states - while (!do_exit && check_all_connected(pandas)) { - can_recv(pandas, &pm); - - // Process peripheral state at 20 Hz - if (rk.frame() % 5 == 0) { - process_peripheral_state(peripheral_panda, &pm, no_fan_control); - } - - // Process panda state at 10 Hz - if (rk.frame() % 10 == 0) { - sm.update(0); - engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled(); - is_onroad = params.getBool("IsOnroad"); - process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started); - panda_safety.configureSafetyMode(is_onroad); - } - - // Send out peripheralState at 2Hz - if (rk.frame() % 50 == 0) { - send_peripheral_state(peripheral_panda, &pm); - } - - // Forward logs from pandas to cloudlog if available - for (auto *panda : pandas) { - std::string log = panda->serial_read(); - if (!log.empty()) { - if (log.find("Register 0x") != std::string::npos) { - // Log register divergent faults as errors - LOGE("%s", log.c_str()); - } else { - LOGD("%s", log.c_str()); - } - } - } - - rk.keepTime(); - } - - // Close relay on exit to prevent a fault - if (is_onroad && !engaged) { - for (auto &p : pandas) { - if (p->connected()) { - p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } - } - } - - send_thread.join(); -} - -void pandad_main_thread(std::vector serials) { - if (serials.size() == 0) { - serials = Panda::list(); - - if (serials.size() == 0) { - LOGW("no pandas found, exiting"); - return; - } - } - - std::string serials_str; - for (int i = 0; i < serials.size(); i++) { - serials_str += serials[i]; - if (i < serials.size() - 1) serials_str += ", "; - } - LOGW("connecting to pandas: %s", serials_str.c_str()); - - // connect to all provided serials - std::vector pandas; - for (int i = 0; i < serials.size() && !do_exit; /**/) { - Panda *p = connect(serials[i], i); - if (!p) { - util::sleep_for(100); - continue; - } - - pandas.push_back(p); - ++i; - } - - if (!do_exit) { - LOGW("connected to all pandas"); - pandad_run(pandas); - } - - for (Panda *panda : pandas) { - delete panda; - } -} diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h deleted file mode 100644 index 637807e0749f98..00000000000000 --- a/selfdrive/pandad/pandad.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include - -#include "common/params.h" -#include "selfdrive/pandad/panda.h" - -void pandad_main_thread(std::vector serials); - -class PandaSafety { -public: - PandaSafety(const std::vector &pandas) : pandas_(pandas) {} - void configureSafetyMode(bool is_onroad); - -private: - void updateMultiplexingMode(); - std::string fetchCarParams(); - void setSafetyMode(const std::string ¶ms_string); - - bool initialized_ = false; - bool log_once_ = false; - bool safety_configured_ = false; - bool prev_obd_multiplexing_ = false; - std::vector pandas_; - Params params_; -}; diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py deleted file mode 100755 index d75af283f2c9c3..00000000000000 --- a/selfdrive/pandad/pandad.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# simple pandad wrapper that updates the panda first -import os -import usb1 -import time -import signal -import subprocess - -from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.system.hardware import HARDWARE -from openpilot.common.swaglog import cloudlog - - -def get_expected_signature(panda: Panda) -> bytes: - try: - fn = os.path.join(FW_PATH, panda.get_mcu_type().config.app_fn) - return Panda.get_signature_from_firmware(fn) - except Exception: - cloudlog.exception("Error computing expected signature") - return b"" - -def flash_panda(panda_serial: str) -> Panda: - try: - panda = Panda(panda_serial) - except PandaProtocolMismatch: - cloudlog.warning("detected protocol mismatch, reflashing panda") - HARDWARE.recover_internal_panda() - raise - - fw_signature = get_expected_signature(panda) - internal_panda = panda.is_internal() - - panda_version = "bootstub" if panda.bootstub else panda.get_version() - 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]}") - - if panda.bootstub or panda_signature != fw_signature: - cloudlog.info("Panda firmware out of date, update required") - panda.flash() - cloudlog.info("Done flashing") - - if panda.bootstub: - bootstub_version = panda.get_version() - cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. {bootstub_version=}, {internal_panda=}") - if internal_panda: - HARDWARE.recover_internal_panda() - panda.recover(reset=(not internal_panda)) - cloudlog.info("Done flashing bootstub") - - if panda.bootstub: - cloudlog.info("Panda still not booting, exiting") - raise AssertionError - - panda_signature = panda.get_signature() - if panda_signature != fw_signature: - cloudlog.info("Version mismatch after flashing, exiting") - raise AssertionError - - return panda - - -def main() -> None: - # signal pandad to close the relay and exit - def signal_handler(signum, frame): - cloudlog.info(f"Caught signal {signum}, exiting") - nonlocal do_exit - do_exit = True - if process is not None: - process.send_signal(signal.SIGINT) - - process = None - do_exit = False - signal.signal(signal.SIGINT, signal_handler) - - count = 0 - first_run = True - params = Params() - no_internal_panda_count = 0 - - while not do_exit: - try: - count += 1 - cloudlog.event("pandad.flash_and_connect", count=count) - params.remove("PandaSignatures") - - # Handle missing internal panda - if no_internal_panda_count > 0: - if no_internal_panda_count == 3: - cloudlog.info("No pandas found, putting internal panda into DFU") - HARDWARE.recover_internal_panda() - else: - cloudlog.info("No pandas found, resetting internal panda") - HARDWARE.reset_internal_panda() - time.sleep(3) # wait to come back up - - # Flash all Pandas in DFU mode - dfu_serials = PandaDFU.list() - if len(dfu_serials) > 0: - for serial in dfu_serials: - cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}") - PandaDFU(serial).recover() - time.sleep(1) - - panda_serials = Panda.list() - if len(panda_serials) == 0: - no_internal_panda_count += 1 - continue - - cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}") - - # Flash pandas - pandas: list[Panda] = [] - for serial in panda_serials: - pandas.append(flash_panda(serial)) - - # Ensure internal panda is present if expected - internal_pandas = [panda for panda in pandas if panda.is_internal()] - if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: - cloudlog.error("Internal panda is missing, trying again") - no_internal_panda_count += 1 - continue - no_internal_panda_count = 0 - - # sort pandas to have deterministic order - # * the internal one is always first - # * then sort by hardware type - # * as a last resort, sort by serial number - pandas.sort(key=lambda x: (not x.is_internal(), x.get_type(), x.get_usb_serial())) - panda_serials = [p.get_usb_serial() for p in pandas] - - # log panda fw versions - params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) - - for panda in pandas: - # check health for lost heartbeat - health = panda.health() - if health["heartbeat_lost"]: - params.put_bool("PandaHeartbeatLost", True) - cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) - if health["som_reset_triggered"]: - params.put_bool("PandaSomResetTriggered", True) - cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial()) - - if first_run: - # reset panda to ensure we're in a good state - cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - panda.reset(reconnect=True) - - for p in pandas: - p.close() - # TODO: wrap all panda exceptions in a base panda exception - except (usb1.USBErrorNoDevice, usb1.USBErrorPipe): - # a panda was disconnected while setting everything up. let's try again - cloudlog.exception("Panda USB exception while setting up") - continue - except PandaProtocolMismatch: - cloudlog.exception("pandad.protocol_mismatch") - continue - except Exception: - cloudlog.exception("pandad.uncaught_exception") - continue - - first_run = False - - # run pandad with all connected serials as arguments - os.environ['MANAGER_DAEMON'] = 'pandad' - process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "selfdrive/pandad")) - process.wait() - - -if __name__ == "__main__": - main() diff --git a/selfdrive/pandad/pandad_api_impl.py b/selfdrive/pandad/pandad_api_impl.py deleted file mode 100644 index 75a7ba484e15f0..00000000000000 --- a/selfdrive/pandad/pandad_api_impl.py +++ /dev/null @@ -1,88 +0,0 @@ -import time -from cereal import log - -NO_TRAVERSAL_LIMIT = 2**64 - 1 - -# Cache schema fields for faster access (avoids string lookup on each field access) -_cached_reader_fields = None # (address_field, dat_field, src_field) for reading -_cached_writer_fields = None # (address_field, dat_field, src_field) for writing - - -def _get_reader_fields(schema): - """Get cached schema field objects for reading.""" - global _cached_reader_fields - if _cached_reader_fields is None: - fields = schema.fields - _cached_reader_fields = (fields['address'], fields['dat'], fields['src']) - return _cached_reader_fields - - -def _get_writer_fields(schema): - """Get cached schema field objects for writing.""" - global _cached_writer_fields - if _cached_writer_fields is None: - fields = schema.fields - _cached_writer_fields = (fields['address'], fields['dat'], fields['src']) - return _cached_writer_fields - - -def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): - """Convert list of CAN messages to Cap'n Proto serialized bytes. - - Args: - can_msgs: List of tuples [(address, data_bytes, src), ...] - msgtype: 'can' or 'sendcan' - valid: Whether the event is valid - - Returns: - Cap'n Proto serialized bytes - """ - global _cached_writer_fields - - dat = log.Event.new_message(valid=valid, logMonoTime=int(time.monotonic() * 1e9)) - can_data = dat.init(msgtype, len(can_msgs)) - - # Cache schema fields on first call - if _cached_writer_fields is None and len(can_msgs) > 0: - _cached_writer_fields = _get_writer_fields(can_data[0].schema) - - if _cached_writer_fields is not None: - addr_f, dat_f, src_f = _cached_writer_fields - for i, msg in enumerate(can_msgs): - f = can_data[i] - f._set_by_field(addr_f, msg[0]) - f._set_by_field(dat_f, msg[1]) - f._set_by_field(src_f, msg[2]) - - return dat.to_bytes() - - -def can_capnp_to_list(strings, msgtype='can'): - """Convert Cap'n Proto serialized bytes to list of CAN messages. - - Args: - strings: Tuple/list of serialized Cap'n Proto bytes - msgtype: 'can' or 'sendcan' - - Returns: - List of tuples [(nanos, [(address, data, src), ...]), ...] - """ - global _cached_reader_fields - result = [] - - for s in strings: - with log.Event.from_bytes(s, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as event: - frames = getattr(event, msgtype) - - # Cache schema fields on first frame for faster access - if _cached_reader_fields is None and len(frames) > 0: - _cached_reader_fields = _get_reader_fields(frames[0].schema) - - if _cached_reader_fields is not None: - addr_f, dat_f, src_f = _cached_reader_fields - frame_list = [(f._get_by_field(addr_f), f._get_by_field(dat_f), f._get_by_field(src_f)) for f in frames] - else: - frame_list = [] - - result.append((event.logMonoTime, frame_list)) - return result diff --git a/selfdrive/pandad/spi.cc b/selfdrive/pandad/spi.cc deleted file mode 100644 index b6ee57801a31b7..00000000000000 --- a/selfdrive/pandad/spi.cc +++ /dev/null @@ -1,410 +0,0 @@ -#ifndef __APPLE__ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "common/util.h" -#include "common/timing.h" -#include "common/swaglog.h" -#include "panda/board/comms_definitions.h" -#include "selfdrive/pandad/panda_comms.h" - - -#define SPI_SYNC 0x5AU -#define SPI_HACK 0x79U -#define SPI_DACK 0x85U -#define SPI_NACK 0x1FU -#define SPI_CHECKSUM_START 0xABU - - -enum SpiError { - NACK = -2, - ACK_TIMEOUT = -3, -}; - -const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds -const std::string SPI_DEVICE = "/dev/spidev0.0"; - -class LockEx { -public: - LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) { - m.lock(); - flock(fd, LOCK_EX); - } - - ~LockEx() { - flock(fd, LOCK_UN); - m.unlock(); - } - -private: - int fd; - std::recursive_mutex &m; -}; - -#define SPILOG(fn, fmt, ...) do { \ - fn(fmt, ## __VA_ARGS__); \ - fn(" %d / 0x%x / %d / %d / tx: %s", \ - xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \ - util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \ - } while (0) - -PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { - int ret; - const int uid_len = 12; - uint8_t uid[uid_len] = {0}; - - uint32_t spi_mode = SPI_MODE_0; - uint8_t spi_bits_per_word = 8; - - // 50MHz is the max of the 845. note that some older - // revs of the comma three may not support this speed - uint32_t spi_speed = 50000000; - try { - if (!util::file_exists(SPI_DEVICE)) { - throw std::runtime_error("Error connecting to panda: SPI device not found"); - } - - spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); - if (spi_fd < 0) { - LOGE("failed opening SPI device %d", spi_fd); - throw std::runtime_error("Error connecting to panda: failed to open SPI device"); - } - - // SPI settings - util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode, "failed setting SPI mode"); - util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed, "failed setting SPI speed"); - util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word"); - - // get hw UID/serial - ret = control_read(0xc3, 0, 0, uid, uid_len, 100); - if (ret == uid_len) { - std::stringstream stream; - for (int i = 0; i < uid_len; i++) { - stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); - } - hw_serial = stream.str(); - } else { - LOGD("failed to get serial %d", ret); - throw std::runtime_error("Error connecting to panda: failed to get serial"); - } - - if (!serial.empty() && (serial != hw_serial)) { - throw std::runtime_error("Error connecting to panda: serial mismatch"); - } - - } catch (...) { - cleanup(); - throw; - } - return; -} - -PandaSpiHandle::~PandaSpiHandle() { - std::lock_guard lk(hw_lock); - cleanup(); -} - -void PandaSpiHandle::cleanup() { - if (spi_fd != -1) { - close(spi_fd); - spi_fd = -1; - } -} - - - -int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) { - ControlPacket_t packet = { - .request = request, - .param1 = param1, - .param2 = param2, - .length = 0 - }; - return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0, timeout); -} - -int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) { - ControlPacket_t packet = { - .request = request, - .param1 = param1, - .param2 = param2, - .length = length - }; - return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length, timeout); -} - -int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return bulk_transfer(endpoint, data, length, NULL, 0, timeout); -} -int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return bulk_transfer(endpoint, NULL, 0, data, length, timeout); -} - -int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) { - const int xfer_size = SPI_BUF_SIZE - 0x40; - - int ret = 0; - uint16_t length = (tx_data != NULL) ? tx_len : rx_len; - for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) { - int d; - if (tx_data != NULL) { - int len = std::min(xfer_size, tx_len - (xfer_size * i)); - d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0, timeout); - } else { - uint16_t to_read = std::min(xfer_size, rx_len - ret); - d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout); - } - - if (d < 0) { - SPILOG(LOGE, "SPI: bulk transfer failed with %d", d); - comms_healthy = false; - return d; - } - - ret += d; - if ((rx_data != NULL) && d < xfer_size) { - break; - } - } - - return ret; -} - -std::vector PandaSpiHandle::list() { - try { - PandaSpiHandle sh(""); - return {sh.hw_serial}; - } catch (std::exception &e) { - // no panda on SPI - } - return {}; -} - -void add_checksum(uint8_t *data, int data_len) { - data[data_len] = SPI_CHECKSUM_START; - for (int i=0; i < data_len; i++) { - data[data_len] ^= data[i]; - } -} - -bool check_checksum(uint8_t *data, int data_len) { - uint8_t checksum = SPI_CHECKSUM_START; - for (uint16_t i = 0U; i < data_len; i++) { - checksum ^= data[i]; - } - return checksum == 0U; -} - - -int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) { - int ret; - int nack_count = 0; - int timeout_count = 0; - bool timed_out = false; - double start_time = millis_since_boot(); - - do { - ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout); - - if (ret < 0) { - timed_out = (timeout != 0) && (timeout_count > 5); - timeout_count += ret == SpiError::ACK_TIMEOUT; - - // give other threads a chance to run - std::this_thread::yield(); - - if (ret == SpiError::NACK) { - // prevent busy waiting while the panda is NACK'ing - // due to full TX buffers - nack_count += 1; - if (nack_count > 3) { - SPILOG(LOGD, "NACK sleep %d", nack_count); - usleep(std::clamp(nack_count*10, 200, 2000)); - } - } - } - } while (ret < 0 && connected && !timed_out); - - if (ret < 0) { - SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time); - } - - return ret; -} - -int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) { - double start_millis = millis_since_boot(); - if (timeout == 0) { - timeout = SPI_ACK_TIMEOUT; - } - timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT); - - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf, - .len = length, - }; - memset(tx_buf, tx, length); - - while (true) { - int ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send ACK request"); - return ret; - } - - if (rx_buf[0] == ack) { - break; - } else if (rx_buf[0] == SPI_NACK) { - SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack); - return SpiError::NACK; - } - - // handle timeout - if (millis_since_boot() - start_millis > timeout) { - SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack); - return SpiError::ACK_TIMEOUT; - } - } - - return 0; -} - -int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) { - static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1")); - - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob) { - printf("transfer len error\n"); - t.len = rand() % SPI_BUF_SIZE; - } - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) { - printf("corrupting TX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } - } - - int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t); - - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) { - printf("corrupting RX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } - } - - return ret; -} - -int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) { - int ret; - uint16_t rx_data_len; - LockEx lock(spi_fd, hw_lock); - - // needs to be less, since we need to have space for the checksum - assert(tx_len < SPI_BUF_SIZE); - assert(max_rx_len < SPI_BUF_SIZE); - - xfer_count++; - header = { - .sync = SPI_SYNC, - .endpoint = endpoint, - .tx_len = tx_len, - .max_rx_len = max_rx_len - }; - - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf - }; - - // Send header - memcpy(tx_buf, &header, sizeof(header)); - add_checksum(tx_buf, sizeof(header)); - transfer.len = sizeof(header) + 1; - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send header"); - goto fail; - } - - // Wait for (N)ACK - ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1); - if (ret < 0) { - goto fail; - } - - // Send data - if (tx_data != NULL) { - memcpy(tx_buf, tx_data, tx_len); - } - add_checksum(tx_buf, tx_len); - transfer.len = tx_len + 1; - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send data"); - goto fail; - } - - // Wait for (N)ACK - ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3); - if (ret < 0) { - goto fail; - } - - // Read data - rx_data_len = *(uint16_t *)(rx_buf+1); - if (rx_data_len >= SPI_BUF_SIZE) { - SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len); - goto fail; - } - - transfer.len = rx_data_len + 1; - transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1); - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to read rx data"); - goto fail; - } - if (!check_checksum(rx_buf, rx_data_len + 4)) { - SPILOG(LOGE, "SPI: bad checksum"); - goto fail; - } - - if (rx_data != NULL) { - memcpy(rx_data, rx_buf + 3, rx_data_len); - } - - return rx_data_len; - -fail: - // ensure slave is in a consistent state - // and ready for the next transfer - int nack_cnt = 0; - while (nack_cnt < 3) { - if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) { - nack_cnt += 1; - } else { - nack_cnt = 0; - } - } - - if (ret >= 0) ret = -1; - return ret; -} -#endif diff --git a/selfdrive/pandad/tests/bootstub.panda_h7.bin b/selfdrive/pandad/tests/bootstub.panda_h7.bin deleted file mode 100755 index 1d9445004e33e5..00000000000000 Binary files a/selfdrive/pandad/tests/bootstub.panda_h7.bin and /dev/null differ diff --git a/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin b/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin deleted file mode 100755 index 5cf2fa45196484..00000000000000 Binary files a/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin and /dev/null differ diff --git a/selfdrive/pandad/tests/test_pandad.py b/selfdrive/pandad/tests/test_pandad.py deleted file mode 100644 index 88d3939a6ad8dd..00000000000000 --- a/selfdrive/pandad/tests/test_pandad.py +++ /dev/null @@ -1,114 +0,0 @@ -import os -import pytest -import time - -import cereal.messaging as messaging -from cereal import log -from openpilot.common.gpio import gpio_set, gpio_init -from panda import Panda, PandaDFU -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.hardware import HARDWARE -from openpilot.system.hardware.tici.pins import GPIO - -HERE = os.path.dirname(os.path.realpath(__file__)) - - -@pytest.mark.tici -class TestPandad: - - def setup_method(self): - # ensure panda is up - if len(Panda.list()) == 0: - self._run_test(60) - - def teardown_method(self): - managed_processes['pandad'].stop() - - def _run_test(self, timeout=30) -> float: - st = time.monotonic() - sm = messaging.SubMaster(['pandaStates']) - - managed_processes['pandad'].start() - while (time.monotonic() - st) < timeout: - sm.update(100) - if len(sm['pandaStates']) and sm['pandaStates'][0].pandaType != log.PandaState.PandaType.unknown: - break - dt = time.monotonic() - st - managed_processes['pandad'].stop() - - if len(sm['pandaStates']) == 0 or sm['pandaStates'][0].pandaType == log.PandaState.PandaType.unknown: - raise Exception("pandad failed to start") - - return dt - - def _go_to_dfu(self): - HARDWARE.recover_internal_panda() - assert Panda.wait_for_dfu(None, 10) - - def _assert_no_panda(self): - assert not Panda.wait_for_dfu(None, 3) - assert not Panda.wait_for_panda(None, 3) - - def _flash_bootstub(self, fn): - self._go_to_dfu() - pd = PandaDFU(None) - if fn is None: - fn = os.path.join(HERE, pd.get_mcu_type().config.bootstub_fn) - with open(fn, "rb") as f: - pd.program_bootstub(f.read()) - pd.reset() - HARDWARE.reset_internal_panda() - - def test_in_dfu(self): - HARDWARE.recover_internal_panda() - self._run_test(60) - - def test_in_bootstub(self): - with Panda() as p: - p.reset(enter_bootstub=True) - assert p.bootstub - self._run_test() - - def test_internal_panda_reset(self): - gpio_init(GPIO.STM_RST_N, True) - gpio_set(GPIO.STM_RST_N, 1) - time.sleep(0.5) - assert all(not Panda(s).is_internal() for s in Panda.list()) - self._run_test() - - assert any(Panda(s).is_internal() for s in Panda.list()) - - def test_best_case_startup_time(self): - # run once so we're up to date - self._run_test(60) - - ts = [] - for _ in range(10): - # should be nearly instant this time - dt = self._run_test(5) - ts.append(dt) - - # 5s for USB (due to enumeration) - # - 0.2s pandad -> pandad - # - plus some buffer - print("startup times", ts, sum(ts) / len(ts)) - assert 0.1 < (sum(ts)/len(ts)) < 0.7 - - def test_old_spi_protocol(self): - # flash firmware with old SPI protocol - self._flash_bootstub(os.path.join(HERE, "bootstub.panda_h7_spiv0.bin")) - self._run_test(45) - - def test_release_to_devel_bootstub(self): - self._flash_bootstub(None) - self._run_test(45) - - def test_recover_from_bad_bootstub(self): - self._go_to_dfu() - with PandaDFU(None) as pd: - pd.program_bootstub(b"\x00"*1024) - pd.reset() - HARDWARE.reset_internal_panda() - self._assert_no_panda() - - self._run_test(60) diff --git a/selfdrive/pandad/tests/test_pandad_loopback.py b/selfdrive/pandad/tests/test_pandad_loopback.py deleted file mode 100644 index eff70d2544da49..00000000000000 --- a/selfdrive/pandad/tests/test_pandad_loopback.py +++ /dev/null @@ -1,113 +0,0 @@ -import os -import copy -import random -import time -import pytest -from collections import defaultdict -from pprint import pprint - -import cereal.messaging as messaging -from cereal import car, log -from opendbc.car.can_definitions import CanData -from openpilot.common.utils import retry -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.selfdrive.pandad import can_list_to_can_capnp -from openpilot.system.hardware import TICI -from openpilot.selfdrive.test.helpers import with_processes - - -@retry(attempts=3) -def setup_pandad(num_pandas): - params = Params() - params.clear_all() - params.put_bool("IsOnroad", False) - - sm = messaging.SubMaster(['pandaStates']) - with Timeout(90, "pandad didn't start"): - while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ - any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): - sm.update(1000) - - found_pandas = len(sm['pandaStates']) - assert num_pandas == found_pandas, "connected pandas ({found_pandas}) doesn't match expected panda count ({num_pandas}). \ - connect another panda for multipanda tests." - - # pandad safety setting relies on these params - cp = car.CarParams.new_message() - - safety_config = car.CarParams.SafetyConfig.new_message() - safety_config.safetyModel = car.CarParams.SafetyModel.allOutput - cp.safetyConfigs = [safety_config]*num_pandas - - params.put_bool("IsOnroad", True) - params.put_bool("FirmwareQueryDone", True) - params.put_bool("ControlsReady", True) - params.put("CarParams", cp.to_bytes()) - - with Timeout(90, "pandad didn't set safety mode"): - while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']): - sm.update(1000) - -def send_random_can_messages(sendcan, count, num_pandas=1): - sent_msgs = defaultdict(set) - for _ in range(count): - to_send = [] - for __ in range(random.randrange(20)): - bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) - addr = random.randrange(1, 1<<29) - dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) - if (addr, dat) in sent_msgs[bus]: - continue - sent_msgs[bus].add((addr, dat)) - to_send.append(CanData(addr, dat, bus)) - sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) - return sent_msgs - - -@pytest.mark.tici -class TestBoarddLoopback: - @classmethod - def setup_class(cls): - os.environ['STARTED'] = '1' - os.environ['BOARDD_LOOPBACK'] = '1' - - @with_processes(['pandad']) - def test_loopback(self): - num_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1 - setup_pandad(num_pandas) - - sendcan = messaging.pub_sock('sendcan') - can = messaging.sub_sock('can', conflate=False, timeout=100) - sm = messaging.SubMaster(['pandaStates']) - time.sleep(1) - - n = 200 - for i in range(n): - print(f"pandad loopback {i}/{n}") - - sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100), num_pandas) - - sent_loopback = copy.deepcopy(sent_msgs) - sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()}) - sent_total = {k: len(v) for k, v in sent_loopback.items()} - for _ in range(100 * 5): - sm.update(0) - recvd = messaging.drain_sock(can, wait_for_one=True) - for msg in recvd: - for m in msg.can: - key = (m.address, m.dat) - assert key in sent_loopback[m.src], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}" - sent_loopback[m.src].discard(key) - - if all(len(v) == 0 for v in sent_loopback.values()): - break - - # if a set isn't empty, messages got dropped - pprint(sent_msgs) - pprint(sent_loopback) - print({k: len(x) for k, x in sent_loopback.items()}) - print(sum([len(x) for x in sent_loopback.values()])) - pprint(sm['pandaStates']) # may drop messages due to RX buffer overflow - for bus in sent_loopback.keys(): - assert not len(sent_loopback[bus]), f"loop {i}: bus {bus} missing {len(sent_loopback[bus])} out of {sent_total[bus]} messages" diff --git a/selfdrive/pandad/tests/test_pandad_spi.py b/selfdrive/pandad/tests/test_pandad_spi.py deleted file mode 100644 index da4b181993dd0e..00000000000000 --- a/selfdrive/pandad/tests/test_pandad_spi.py +++ /dev/null @@ -1,102 +0,0 @@ -import os -import time -import numpy as np -import pytest -import random - -import cereal.messaging as messaging -from cereal.services import SERVICE_LIST -from openpilot.selfdrive.test.helpers import with_processes -from openpilot.selfdrive.pandad.tests.test_pandad_loopback import setup_pandad, send_random_can_messages - -JUNGLE_SPAM = "JUNGLE_SPAM" in os.environ - -@pytest.mark.tici -class TestBoarddSpi: - @classmethod - def setup_class(cls): - os.environ['STARTED'] = '1' - os.environ['SPI_ERR_PROB'] = '0.001' - if not JUNGLE_SPAM: - os.environ['BOARDD_LOOPBACK'] = '1' - - @with_processes(['pandad']) - def test_spi_corruption(self, subtests): - setup_pandad(1) - - sendcan = messaging.pub_sock('sendcan') - socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')} - time.sleep(2) - for s in socks.values(): - messaging.drain_sock_raw(s) - - total_recv_count = 0 - total_sent_count = 0 - sent_msgs = {bus: list() for bus in range(3)} - - st = time.monotonic() - ts = {s: list() for s in socks.keys()} - for _ in range(int(os.getenv("TEST_TIME", "20"))): - # send some CAN messages - if not JUNGLE_SPAM: - sent = send_random_can_messages(sendcan, random.randrange(2, 20)) - for k, v in sent.items(): - sent_msgs[k].extend(list(v)) - total_sent_count += len(v) - - for service, sock in socks.items(): - for m in messaging.drain_sock(sock): - ts[service].append(m.logMonoTime) - - # sanity check for corruption - assert m.valid or (service == "can") - if service == "can": - for msg in m.can: - if JUNGLE_SPAM: - # PandaJungle.set_generated_can(True) - i = msg.address - 0x200 - assert msg.address >= 0x200 - assert msg.src == (i%3) - assert msg.dat == b"\xff"*(i%8) - total_recv_count += 1 - continue - - if msg.src > 4: - continue - key = (msg.address, msg.dat) - assert key in sent_msgs[msg.src], f"got unexpected msg: {msg.src=} {msg.address=} {msg.dat=}" - # TODO: enable this - #sent_msgs[msg.src].remove(key) - total_recv_count += 1 - elif service == "pandaStates": - assert len(m.pandaStates) == 1 - ps = m.pandaStates[0] - assert ps.uptime < 1000 - assert ps.pandaType == "tres" - assert ps.ignitionLine - assert not ps.ignitionCan - assert 4000 < ps.voltage < 14000 - elif service == "peripheralState": - ps = m.peripheralState - assert ps.pandaType == "tres" - assert 4000 < ps.voltage < 14000 - assert 50 < ps.current < 1000 - assert ps.fanSpeedRpm < 10000 - - time.sleep(0.5) - et = time.monotonic() - st - - print("\n======== timing report ========") - for service, times in ts.items(): - dts = np.diff(times)/1e6 - print(service.ljust(17), f"{np.mean(dts):7.2f} {np.min(dts):7.2f} {np.max(dts):7.2f}") - with subtests.test(msg="timing check", service=service): - edt = 1e3 / SERVICE_LIST[service].frequency - assert edt*0.9 < np.mean(dts) < edt*1.1 - assert np.max(dts) < edt*8 - assert np.min(dts) < edt - assert len(dts) >= ((et-0.5)*SERVICE_LIST[service].frequency*0.8) - - with subtests.test(msg="CAN traffic"): - print(f"Sent {total_sent_count} CAN messages, got {total_recv_count} back. {total_recv_count/(total_sent_count+1e-4):.2%} received") - assert total_recv_count > 20 diff --git a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc b/selfdrive/pandad/tests/test_pandad_usbprotocol.cc deleted file mode 100644 index 11f7184efdba2f..00000000000000 --- a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc +++ /dev/null @@ -1,135 +0,0 @@ -#define CATCH_CONFIG_MAIN -#define CATCH_CONFIG_ENABLE_BENCHMARKING - -#include "catch2/catch.hpp" -#include "cereal/messaging/messaging.h" -#include "common/util.h" -#include "selfdrive/pandad/panda.h" - -struct PandaTest : public Panda { - PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type); - void test_can_send(); - void test_can_recv(uint32_t chunk_size = 0); - void test_chunked_can_recv(); - - std::map test_data; - int can_list_size = 0; - int total_pakets_size = 0; - MessageBuilder msg; - capnp::List::Reader can_data_list; -}; - -PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) { - this->hw_type = hw_type; - int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8); - // prepare test data - for (int i = 0; i < data_limit; ++i) { - std::random_device rd; - std::independent_bits_engine rbe(rd()); - - int data_len = dlc_to_len[i]; - std::string bytes(data_len, '\0'); - std::generate(bytes.begin(), bytes.end(), std::ref(rbe)); - test_data[data_len] = bytes; - } - - // generate can messages for this panda - auto can_list = msg.initEvent().initSendcan(can_list_size); - for (uint8_t i = 0; i < can_list_size; ++i) { - auto can = can_list[i]; - uint32_t id = util::random_int(0, std::size(dlc_to_len) - 1); - const std::string &dat = test_data[dlc_to_len[id]]; - can.setAddress(i); - can.setSrc(util::random_int(0, 2) + bus_offset); - can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); - total_pakets_size += sizeof(can_header) + dat.size(); - } - - can_data_list = can_list.asReader(); - INFO("test " << can_list_size << " packets, total size " << total_pakets_size); -} - -void PandaTest::test_can_send() { - std::vector unpacked_data; - this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { - unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]); - }); - REQUIRE(unpacked_data.size() == total_pakets_size); - - int cnt = 0; - INFO("test can message integrity"); - for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) { - can_header header; - memcpy(&header, &unpacked_data[pos], sizeof(can_header)); - const uint8_t data_len = dlc_to_len[header.data_len_code]; - pckt_len = sizeof(can_header) + data_len; - - REQUIRE(header.addr == cnt); - REQUIRE(test_data.find(data_len) != test_data.end()); - const std::string &dat = test_data[data_len]; - REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0); - ++cnt; - } - REQUIRE(cnt == can_list_size); -} - -void PandaTest::test_can_recv(uint32_t rx_chunk_size) { - std::vector frames; - this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) { - if (rx_chunk_size == 0) { - REQUIRE(this->unpack_can_buffer(data, size, frames)); - } else { - this->receive_buffer_size = 0; - uint32_t pos = 0; - - while (pos < size) { - uint32_t chunk_size = std::min(rx_chunk_size, size - pos); - memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size); - this->receive_buffer_size += chunk_size; - pos += chunk_size; - - REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames)); - } - } - }); - - REQUIRE(frames.size() == can_list_size); - for (int i = 0; i < frames.size(); ++i) { - REQUIRE(frames[i].address == i); - REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end()); - const std::string &dat = test_data[frames[i].dat.size()]; - REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0); - } -} - -TEST_CASE("send/recv CAN 2.0 packets") { - auto bus_offset = GENERATE(0, 4); - auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); - PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS); - - SECTION("can_send") { - test.test_can_send(); - } - SECTION("can_receive") { - test.test_can_recv(); - } - SECTION("chunked_can_receive") { - test.test_can_recv(0x40); - } -} - -TEST_CASE("send/recv CAN FD packets") { - auto bus_offset = GENERATE(0, 4); - auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); - PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA); - - SECTION("can_send") { - test.test_can_send(); - } - SECTION("can_receive") { - test.test_can_recv(); - } - SECTION("chunked_can_receive") { - test.test_can_recv(0x40); - } -} diff --git a/selfdrive/rtshield.py b/selfdrive/rtshield.py new file mode 100755 index 00000000000000..45571fe2db3841 --- /dev/null +++ b/selfdrive/rtshield.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +import os +import time +from typing import NoReturn + +from common.realtime import set_core_affinity, set_realtime_priority + +# RT shield - ensure CPU 3 always remains available for RT processes +# runs as SCHED_FIFO with minimum priority to ensure kthreads don't +# get scheduled onto CPU 3, but it's always preemptible by realtime +# openpilot processes + +def main() -> NoReturn: + set_core_affinity([int(os.getenv("CORE", "3")), ]) + set_realtime_priority(1) + + while True: + time.sleep(0.000001) + +if __name__ == "__main__": + main() diff --git a/selfdrive/selfdrived/alertmanager.py b/selfdrive/selfdrived/alertmanager.py deleted file mode 100644 index 385c276a948c95..00000000000000 --- a/selfdrive/selfdrived/alertmanager.py +++ /dev/null @@ -1,67 +0,0 @@ -import copy -import os -import json -from collections import defaultdict -from dataclasses import dataclass - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.selfdrived.events import Alert, EmptyAlert - - -with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f: - OFFROAD_ALERTS = json.load(f) - - -def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None: - if show_alert: - a = copy.copy(OFFROAD_ALERTS[alert]) - a['extra'] = extra_text or '' - Params().put(alert, a) - else: - Params().remove(alert) - - -@dataclass -class AlertEntry: - alert: Alert | None = None - start_frame: int = -1 - end_frame: int = -1 - added_frame: int = -1 - - def active(self, frame: int) -> bool: - return frame <= self.end_frame - - def just_added(self, frame: int) -> bool: - return self.active(frame) and frame == (self.added_frame + 1) - -class AlertManager: - def __init__(self): - self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry) - self.current_alert = EmptyAlert - - def add_many(self, frame: int, alerts: list[Alert]) -> None: - for alert in alerts: - entry = self.alerts[alert.alert_type] - entry.alert = alert - if not entry.just_added(frame): - entry.start_frame = frame - min_end_frame = entry.start_frame + alert.duration - entry.end_frame = max(frame + 1, min_end_frame) - entry.added_frame = frame - - def process_alerts(self, frame: int, clear_event_types: set): - ae = AlertEntry() - for v in self.alerts.values(): - if not v.alert: - continue - - if v.alert.event_type in clear_event_types: - v.end_frame = -1 - - # sort by priority first and then by start_frame - greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame) - if v.active(frame) and greater: - ae = v - - self.current_alert = ae.alert if ae.alert is not None else EmptyAlert diff --git a/selfdrive/selfdrived/alerts_offroad.json b/selfdrive/selfdrived/alerts_offroad.json deleted file mode 100644 index 0fc11b9636bff1..00000000000000 --- a/selfdrive/selfdrived/alerts_offroad.json +++ /dev/null @@ -1,49 +0,0 @@ -{ - "Offroad_TemperatureTooHigh": { - "text": "Device temperature too high. System cooling down before starting. Current internal component temperature: %1", - "severity": 0 - }, - "Offroad_ConnectivityNeededPrompt": { - "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in %1", - "severity": 0, - "_comment": "Set extra field to number of days" - }, - "Offroad_ConnectivityNeeded": { - "text": "Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates.", - "severity": 1 - }, - "Offroad_UpdateFailed": { - "text": "Unable to download updates\n%1", - "severity": 1, - "_comment": "Set extra field to the failed reason." - }, - "Offroad_IsTakingSnapshot": { - "text": "Taking camera snapshots. System won't start until finished.", - "severity": 0 - }, - "Offroad_NeosUpdate": { - "text": "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.", - "severity": 0 - }, - "Offroad_UnregisteredHardware": { - "text": "Failed to register with 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_CarUnrecognized": { - "text": "openpilot 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 - }, - "Offroad_Recalibration": { - "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.", - "severity": 0 - }, - "Offroad_DriverMonitoringUncertain": { - "text": "Poor visibility detected for driver monitoring. Ensure the device has a clear view of the driver. This can be checked in the device settings. Extreme lighting conditions and/or unconventional mounting positions may also trigger this alert.", - "severity": 0 - }, - "Offroad_ExcessiveActuation": { - "text": "Excessive %1 actuation detected on your last drive. Please contact support at https://comma.ai/support and share your device's Dongle ID for troubleshooting.", - "severity": 1, - "_comment": "Set extra field to lateral or longitudinal." - } -} diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py deleted file mode 100755 index 6fb96b6e5a0dcb..00000000000000 --- a/selfdrive/selfdrived/events.py +++ /dev/null @@ -1,1122 +0,0 @@ -#!/usr/bin/env python3 -import bisect -import math -import os -from enum import IntEnum -from collections.abc import Callable - -from cereal import log, car -import cereal.messaging as messaging -from openpilot.common.constants import CV -from openpilot.common.git import get_short_branch -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER -from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER -from openpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION -from openpilot.system.hardware import HARDWARE - -AlertSize = log.SelfdriveState.AlertSize -AlertStatus = log.SelfdriveState.AlertStatus -VisualAlert = car.CarControl.HUDControl.VisualAlert -AudibleAlert = car.CarControl.HUDControl.AudibleAlert -EventName = log.OnroadEvent.EventName - - -# Alert priorities -class Priority(IntEnum): - LOWEST = 0 - LOWER = 1 - LOW = 2 - MID = 3 - HIGH = 4 - HIGHEST = 5 - - -# Event types -class ET: - ENABLE = 'enable' - PRE_ENABLE = 'preEnable' - OVERRIDE_LATERAL = 'overrideLateral' - OVERRIDE_LONGITUDINAL = 'overrideLongitudinal' - NO_ENTRY = 'noEntry' - WARNING = 'warning' - USER_DISABLE = 'userDisable' - SOFT_DISABLE = 'softDisable' - IMMEDIATE_DISABLE = 'immediateDisable' - PERMANENT = 'permanent' - - -# get event name from enum -EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} - - -class Events: - def __init__(self): - self.events: list[int] = [] - self.static_events: list[int] = [] - self.event_counters = dict.fromkeys(EVENTS.keys(), 0) - - @property - def names(self) -> list[int]: - return self.events - - def __len__(self) -> int: - return len(self.events) - - def add(self, event_name: int, static: bool=False) -> None: - if static: - bisect.insort(self.static_events, event_name) - bisect.insort(self.events, event_name) - - def clear(self) -> None: - self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()} - self.events = self.static_events.copy() - - def contains(self, event_type: str) -> bool: - return any(event_type in EVENTS.get(e, {}) for e in self.events) - - def create_alerts(self, event_types: list[str], callback_args=None): - if callback_args is None: - callback_args = [] - - ret = [] - for e in self.events: - types = EVENTS[e].keys() - for et in event_types: - if et in types: - alert = EVENTS[e][et] - if not isinstance(alert, Alert): - alert = alert(*callback_args) - - if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay: - alert.alert_type = f"{EVENT_NAME[e]}/{et}" - alert.event_type = et - ret.append(alert) - return ret - - def add_from_msg(self, events): - for e in events: - bisect.insort(self.events, e.name.raw) - - def to_msg(self): - ret = [] - for event_name in self.events: - event = log.OnroadEvent.new_message() - event.name = event_name - for event_type in EVENTS.get(event_name, {}): - setattr(event, event_type, True) - ret.append(event) - return ret - - -class Alert: - def __init__(self, - alert_text_1: str, - alert_text_2: str, - alert_status: log.SelfdriveState.AlertStatus, - alert_size: log.SelfdriveState.AlertSize, - priority: Priority, - visual_alert: car.CarControl.HUDControl.VisualAlert, - audible_alert: car.CarControl.HUDControl.AudibleAlert, - duration: float, - creation_delay: float = 0.): - - self.alert_text_1 = alert_text_1 - self.alert_text_2 = alert_text_2 - self.alert_status = alert_status - self.alert_size = alert_size - self.priority = priority - self.visual_alert = visual_alert - self.audible_alert = audible_alert - - self.duration = int(duration / DT_CTRL) - - self.creation_delay = creation_delay - - self.alert_type = "" - self.event_type: str | None = None - - def __str__(self) -> str: - return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" - - def __gt__(self, alert2) -> bool: - if not isinstance(alert2, Alert): - return False - return self.priority > alert2.priority - -EmptyAlert = Alert("" , "", AlertStatus.normal, AlertSize.none, Priority.LOWEST, - VisualAlert.none, AudibleAlert.none, 0) - -class NoEntryAlert(Alert): - def __init__(self, alert_text_2: str, - alert_text_1: str = "openpilot Unavailable", - visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none): - if HARDWARE.get_device_type() == 'mici': - alert_text_1, alert_text_2 = alert_text_2, alert_text_1 - super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, - AlertSize.mid, Priority.LOW, visual_alert, - AudibleAlert.refuse, 3.) - - -class SoftDisableAlert(Alert): - def __init__(self, alert_text_2: str): - super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, - AlertStatus.userPrompt, AlertSize.full, - Priority.MID, VisualAlert.steerRequired, - AudibleAlert.warningSoft, 2.), - - -# less harsh version of SoftDisable, where the condition is user-triggered -class UserSoftDisableAlert(SoftDisableAlert): - def __init__(self, alert_text_2: str): - super().__init__(alert_text_2), - self.alert_text_1 = "openpilot will disengage" - - -class ImmediateDisableAlert(Alert): - def __init__(self, alert_text_2: str): - super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.steerRequired, - AudibleAlert.warningImmediate, 4.), - - -class EngagementAlert(Alert): - def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert): - super().__init__("", "", - AlertStatus.normal, AlertSize.none, - Priority.MID, VisualAlert.none, - audible_alert, .2), - - -class NormalPermanentAlert(Alert): - def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.): - super().__init__(alert_text_1, alert_text_2, - AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small, - priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay), - - -class StartupAlert(Alert): - def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal): - alert_size = AlertSize.mid - if HARDWARE.get_device_type() == 'mici': - if alert_text_2 == "Always keep hands on wheel and eyes on road": - alert_text_2 = "" - alert_size = AlertSize.small - super().__init__(alert_text_1, alert_text_2, - alert_status, alert_size, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.), - - - -# ********** helper functions ********** -def get_display_speed(speed_ms: float, metric: bool) -> str: - speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))) - unit = 'km/h' if metric else 'mph' - return f"{speed} {unit}" - - -# ********** alert callback functions ********** - -AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], Alert] - - -def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if soft_disable_time < int(0.5 / DT_CTRL): - return ImmediateDisableAlert(alert_text_2) - return SoftDisableAlert(alert_text_2) - return func - -def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if soft_disable_time < int(0.5 / DT_CTRL): - return ImmediateDisableAlert(alert_text_2) - return UserSoftDisableAlert(alert_text_2) - return func - -def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - branch = get_short_branch() # Ensure get_short_branch is cached to avoid lags on startup - if "REPLAY" in os.environ: - branch = "replay" - - return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt) - -def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") - - -def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return Alert( - f"Steer Assist Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", - "", - AlertStatus.userPrompt, AlertSize.small, - 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: - first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' - return Alert( - f"{first_word}: {sm['liveCalibration'].calPerc:.0f}%", - f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", - AlertStatus.normal, AlertSize.mid, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) - - -def audio_feedback_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - duration = FEEDBACK_MAX_DURATION - ((sm['audioFeedback'].blockNum + 1) * SAMPLE_BUFFER / SAMPLE_RATE) - return NormalPermanentAlert( - "Recording Audio Feedback", - f"{round(duration)} second{'s' if round(duration) != 1 else ''} remaining. Press again to save early.", - priority=Priority.LOW) - - -# *** debug alerts *** - -def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - full_perc = round(100. - sm['deviceState'].freeSpacePercent) - return NormalPermanentAlert("Out of Storage", f"{full_perc}% full") - - -def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan - err = CS.vEgo - mdl - msg = f"Speed Error: {err:.1f} m/s" - return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid") - - -def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] - msg = ', '.join(not_running) - return NoEntryAlert(msg, alert_text_1="Process Not Running") - - -def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])] - msg = ', '.join(bs[:4]) # can't fit too many on one line - return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes") - - -def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] - return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) - - -def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - rpy = sm['liveCalibration'].rpyCalib - yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) - pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) - angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)" - return NormalPermanentAlert("Calibration Invalid", angles) - - -def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if not sm['liveParameters'].angleOffsetValid: - angle_offset_deg = sm['liveParameters'].angleOffsetDeg - title = "Steering misalignment detected" - text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)" - elif not sm['liveParameters'].steerRatioValid: - steer_ratio = sm['liveParameters'].steerRatio - title = "Steer ratio mismatch" - text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})" - elif not sm['liveParameters'].stiffnessFactorValid: - stiffness_factor = sm['liveParameters'].stiffnessFactor - title = "Abnormal tire stiffness" - text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})" - else: - return NoEntryAlert("paramsd Temporary Error") - - return NoEntryAlert(alert_text_1=title, alert_text_2=text) - -def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - cpu = max(sm['deviceState'].cpuTempC, default=0.) - gpu = max(sm['deviceState'].gpuTempC, default=0.) - temp = max((cpu, gpu, sm['deviceState'].memoryTempC)) - return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C") - - -def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used") - - -def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - x = max(sm['deviceState'].cpuUsagePercent, default=0.) - return NormalPermanentAlert("High CPU Usage", f"{x}% used") - - -def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped") - - -def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - text = "Enable Adaptive Cruise to Engage" - if CP.brand == "honda": - text = "Enable Main Switch to Engage" - return NoEntryAlert(text) - - -def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - gb = sm['carControl'].actuators.accel / 4. - steer = sm['carControl'].actuators.torque - vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" - return NormalPermanentAlert("Joystick Mode", vals) - - -def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - ad = sm['alertDebug'] - audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none - alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal - alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small - return Alert(ad.alertText1, ad.alertText2, - alert_status, alert_size, - Priority.LOW, VisualAlert.none, audible_alert, 0.2) - - -def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - personality = str(personality).title() - return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) - - -def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - text = "Toggle stock LKAS on or off to engage" - if CP.brand == "tesla": - text = "Switch to Traffic-Aware Cruise Control to engage" - elif CP.brand == "mazda": - text = "Enable your car's LKAS to engage" - elif CP.brand == "nissan": - text = "Disable your car's stock LKAS to engage" - return NormalPermanentAlert("Invalid LKAS setting", text) - - - -EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { - # ********** events with no alerts ********** - - EventName.stockFcw: {}, - EventName.actuatorsApiUnavailable: {}, - - # ********** events only containing alerts displayed in all states ********** - - EventName.joystickDebug: { - ET.WARNING: joystick_alert, - ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), - }, - - EventName.longitudinalManeuver: { - ET.WARNING: longitudinal_maneuver_alert, - ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode", - "Ensure road ahead is clear"), - }, - - EventName.selfdriveInitializing: { - ET.NO_ENTRY: NoEntryAlert("System Initializing"), - }, - - EventName.startup: { - ET.PERMANENT: StartupAlert("Be ready to take over at any time") - }, - - EventName.startupMaster: { - ET.PERMANENT: startup_master_alert, - }, - - EventName.startupNoControl: { - ET.PERMANENT: StartupAlert("Dashcam mode"), - ET.NO_ENTRY: NoEntryAlert("Dashcam mode"), - }, - - EventName.startupNoCar: { - ET.PERMANENT: StartupAlert("Dashcam mode for unsupported car"), - }, - - EventName.startupNoSecOcKey: { - ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", - "Security Key Not Available", - priority=Priority.HIGH), - }, - - EventName.dashcamMode: { - ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", - priority=Priority.LOWEST), - }, - - EventName.invalidLkasSetting: { - ET.PERMANENT: invalid_lkas_setting_alert, - ET.NO_ENTRY: NoEntryAlert("Invalid LKAS setting"), - }, - - EventName.cruiseMismatch: { - #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), - }, - - # openpilot doesn't recognize the car. This switches openpilot into a - # read-only mode. This can be solved by adding your fingerprint. - # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information - EventName.carUnrecognized: { - ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", - "Car Unrecognized", - priority=Priority.LOWEST), - }, - - EventName.aeb: { - ET.PERMANENT: Alert( - "BRAKE!", - "Emergency Braking: Risk of Collision", - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), - ET.NO_ENTRY: NoEntryAlert("AEB: Risk of Collision"), - }, - - EventName.stockAeb: { - ET.PERMANENT: Alert( - "BRAKE!", - "Stock AEB: Risk of Collision", - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), - ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"), - }, - - EventName.stockLkas: { - ET.NO_ENTRY: NoEntryAlert("Stock LKAS: Lane Departure Detected"), - }, - - EventName.fcw: { - ET.PERMANENT: Alert( - "BRAKE!", - "Risk of Collision", - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.), - }, - - EventName.ldw: { - ET.PERMANENT: Alert( - "Lane Departure Detected", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.), - }, - - # ********** events only containing alerts that display while engaged ********** - - EventName.steerTempUnavailableSilent: { - ET.WARNING: Alert( - "Steering Assist Temporarily Unavailable", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), - }, - - EventName.preDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.promptDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "Driver Distracted", - AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), - }, - - EventName.driverDistracted: { - ET.PERMANENT: Alert( - "DISENGAGE IMMEDIATELY", - "Driver Distracted", - AlertStatus.critical, AlertSize.full, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), - }, - - EventName.preDriverUnresponsive: { - ET.PERMANENT: Alert( - "Touch Steering Wheel: No Face Detected", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), - }, - - EventName.promptDriverUnresponsive: { - ET.PERMANENT: Alert( - "Touch Steering Wheel", - "Driver Unresponsive", - AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), - }, - - EventName.driverUnresponsive: { - ET.PERMANENT: Alert( - "DISENGAGE IMMEDIATELY", - "Driver Unresponsive", - AlertStatus.critical, AlertSize.full, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), - }, - - EventName.manualRestart: { - ET.WARNING: Alert( - "TAKE CONTROL", - "Resume Driving Manually", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - - EventName.resumeRequired: { - ET.WARNING: Alert( - "Press Resume to Exit Standstill", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - - EventName.belowSteerSpeed: { - ET.WARNING: below_steer_speed_alert, - }, - - EventName.preLaneChangeLeft: { - ET.WARNING: Alert( - "Steer Left to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.preLaneChangeRight: { - ET.WARNING: Alert( - "Steer Right to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.laneChangeBlocked: { - ET.WARNING: Alert( - "Car Detected in Blindspot", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), - }, - - EventName.laneChange: { - ET.WARNING: Alert( - "Changing Lanes", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.steerSaturated: { - ET.WARNING: Alert( - "Take Control", - "Turn Exceeds Steering Limit", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.), - }, - - # Thrown when the fan is driven at >50% but is not rotating - EventName.fanMalfunction: { - ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"), - }, - - # Camera is not outputting frames - EventName.cameraMalfunction: { - ET.PERMANENT: camera_malfunction_alert, - ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"), - ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"), - }, - # Camera framerate too low - EventName.cameraFrameRate: { - ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"), - ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"), - ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"), - }, - - # Unused - - EventName.locationdTemporaryError: { - ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"), - ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"), - }, - - EventName.locationdPermanentError: { - ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"), - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"), - ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"), - }, - - # openpilot tries to learn certain parameters about your car by observing - # how the car behaves to steering inputs from both human and openpilot driving. - # This includes: - # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle - # - tire stiffness: how much grip your tires have - # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight - # This alert is thrown when any of these values exceed a sanity check. This can be caused by - # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub - EventName.paramsdTemporaryError: { - ET.NO_ENTRY: paramsd_invalid_alert, - ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"), - }, - - EventName.paramsdPermanentError: { - ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"), - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"), - ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"), - }, - - # ********** events that affect controls state transitions ********** - - EventName.pcmEnable: { - ET.ENABLE: EngagementAlert(AudibleAlert.engage), - }, - - EventName.buttonEnable: { - ET.ENABLE: EngagementAlert(AudibleAlert.engage), - }, - - EventName.pcmDisable: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - }, - - EventName.buttonCancel: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"), - }, - - EventName.brakeHold: { - ET.WARNING: Alert( - "Press Resume to Exit Brake Hold", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - - EventName.parkBrake: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"), - }, - - EventName.pedalPressed: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Pedal Pressed", - visual_alert=VisualAlert.brakePressed), - }, - - EventName.steerDisengage: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Steering Pressed"), - }, - - EventName.preEnableStandstill: { - ET.PRE_ENABLE: Alert( - "Release Brake to Engage", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), - }, - - EventName.gasPressedOverride: { - ET.OVERRIDE_LONGITUDINAL: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.steerOverride: { - ET.OVERRIDE_LATERAL: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.wrongCarMode: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: wrong_car_mode_alert, - }, - - EventName.resumeBlocked: { - ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"), - }, - - EventName.wrongCruiseMode: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"), - }, - - EventName.steerTempUnavailable: { - ET.SOFT_DISABLE: soft_disable_alert("Steering Assist Temporarily Unavailable"), - ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), - }, - - EventName.steerTimeLimit: { - ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"), - ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"), - }, - - EventName.outOfSpace: { - ET.PERMANENT: out_of_space_alert, - ET.NO_ENTRY: NoEntryAlert("Out of Storage"), - }, - - EventName.belowEngageSpeed: { - ET.NO_ENTRY: below_engage_speed_alert, - }, - - EventName.sensorDataInvalid: { - ET.PERMANENT: Alert( - "Sensor Data Invalid", - "Possible Hardware Issue", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"), - ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), - }, - - EventName.noGps: { - }, - - EventName.tooDistracted: { - ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), - }, - - EventName.excessiveActuation: { - ET.SOFT_DISABLE: soft_disable_alert("Excessive Actuation"), - ET.NO_ENTRY: NoEntryAlert("Excessive Actuation"), - }, - - EventName.overheat: { - ET.PERMANENT: overheat_alert, - ET.SOFT_DISABLE: soft_disable_alert("System Overheated"), - ET.NO_ENTRY: NoEntryAlert("System Overheated"), - }, - - EventName.wrongGear: { - ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"), - ET.NO_ENTRY: NoEntryAlert("Gear not D"), - }, - - # This alert is thrown when the calibration angles are outside of the acceptable range. - # For example if the device is pointed too much to the left or the right. - # Usually this can only be solved by removing the mount from the windshield completely, - # and attaching while making sure the device is pointed straight forward and is level. - # See https://comma.ai/setup for more information - EventName.calibrationInvalid: { - ET.PERMANENT: calibration_invalid_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"), - ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), - }, - - EventName.calibrationIncomplete: { - ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), - ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), - }, - - EventName.calibrationRecalibrating: { - ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), - ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), - }, - - EventName.doorOpen: { - ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), - ET.NO_ENTRY: NoEntryAlert("Door Open"), - }, - - EventName.seatbeltNotLatched: { - ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"), - ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), - }, - - EventName.espDisabled: { - ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"), - ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"), - }, - - EventName.lowBattery: { - ET.SOFT_DISABLE: soft_disable_alert("Low Battery"), - ET.NO_ENTRY: NoEntryAlert("Low Battery"), - }, - - # Different openpilot services communicate between each other at a certain - # interval. If communication does not follow the regular schedule this alert - # is thrown. This can mean a service crashed, did not broadcast a message for - # ten times the regular interval, or the average interval is more than 10% too high. - EventName.commIssue: { - ET.SOFT_DISABLE: soft_disable_alert("Communication Issue Between Processes"), - ET.NO_ENTRY: comm_issue_alert, - }, - EventName.commIssueAvgFreq: { - ET.SOFT_DISABLE: soft_disable_alert("Low Communication Rate Between Processes"), - ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"), - }, - - EventName.selfdrivedLagging: { - ET.SOFT_DISABLE: soft_disable_alert("System Lagging"), - ET.NO_ENTRY: NoEntryAlert("Selfdrive Process Lagging: Reboot Your Device"), - }, - - # Thrown when manager detects a service exited unexpectedly while driving - EventName.processNotRunning: { - ET.NO_ENTRY: process_not_running_alert, - ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"), - }, - - EventName.radarFault: { - ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"), - ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), - }, - - EventName.radarTempUnavailable: { - ET.SOFT_DISABLE: soft_disable_alert("Radar Temporarily Unavailable"), - ET.NO_ENTRY: NoEntryAlert("Radar Temporarily Unavailable"), - }, - - # Every frame from the camera should be processed by the model. If modeld - # is not processing frames fast enough they have to be dropped. This alert is - # thrown when over 20% of frames are dropped. - EventName.modeldLagging: { - ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"), - ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"), - ET.PERMANENT: modeld_lagging_alert, - }, - - # Besides predicting the path, lane lines and lead car data the model also - # predicts the current velocity and rotation speed of the car. If the model is - # very uncertain about the current velocity while the car is moving, this - # usually means the model has trouble understanding the scene. This is used - # as a heuristic to warn the driver. - EventName.posenetInvalid: { - ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"), - ET.NO_ENTRY: posenet_invalid_alert, - }, - - # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we - # alert the driver the device might have fallen from the windshield. - EventName.deviceFalling: { - ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"), - ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), - }, - - EventName.lowMemory: { - ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"), - ET.PERMANENT: low_memory_alert, - ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), - }, - - EventName.accFaulted: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), - ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), - ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), - }, - - EventName.espActive: { - ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Active"), - ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Active"), - }, - - EventName.controlsMismatch: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), - ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), - }, - - # Sometimes the USB stack on the device can get into a bad state - # causing the connection to the panda to be lost - EventName.usbError: { - ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"), - ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device"), - ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), - }, - - # This alert can be thrown for the following reasons: - # - No CAN data received at all - # - CAN data is received, but some message are not received at the right frequency - # If you're not writing a new car port, this is usually cause by faulty wiring - EventName.canError: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Unknown Vehicle Variant"), - ET.PERMANENT: Alert( - "Unknown Vehicle Variant", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("Unknown Vehicle Variant"), - }, - - EventName.canBusMissing: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"), - ET.PERMANENT: Alert( - "CAN Bus Disconnected: Likely Faulty Cable", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"), - }, - - EventName.steerUnavailable: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"), - ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"), - ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"), - }, - - EventName.reverseGear: { - ET.PERMANENT: Alert( - "Reverse\nGear", - "", - AlertStatus.normal, AlertSize.full, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), - ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"), - ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), - }, - - # On cars that use stock ACC the car can decide to cancel ACC for various reasons. - # When this happens we can no long control the car so the user needs to be warned immediately. - EventName.cruiseDisabled: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"), - }, - - # When the relay in the harness box opens the CAN bus between the LKAS camera - # and the rest of the car is separated. When messages from the LKAS camera - # are received on the car side this usually means the relay hasn't opened correctly - # and this alert is thrown. - EventName.relayMalfunction: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"), - ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"), - ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"), - }, - - EventName.speedTooLow: { - ET.IMMEDIATE_DISABLE: Alert( - "openpilot Canceled", - "Speed too low", - AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), - }, - - # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable. - EventName.speedTooHigh: { - ET.WARNING: Alert( - "Speed Too High", - "Model uncertain at this speed", - AlertStatus.userPrompt, AlertSize.mid, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.), - ET.NO_ENTRY: NoEntryAlert("Slow down to engage"), - }, - - EventName.vehicleSensorsInvalid: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), - ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), - ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), - }, - - EventName.personalityChanged: { - ET.WARNING: personality_changed_alert, - }, - - EventName.userBookmark: { - ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5), - }, - - EventName.audioFeedback: { - ET.PERMANENT: audio_feedback_alert, - }, -} - - -if HARDWARE.get_device_type() == 'mici': - EVENTS.update({ - EventName.preDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 2), - }, - EventName.promptDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "Driver Distracted", - AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, 1), - }, - EventName.resumeRequired: { - ET.WARNING: Alert( - "Press Resume", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - EventName.preLaneChangeLeft: { - ET.WARNING: Alert( - "Steer Left", - "Confirm Lane Change", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - EventName.preLaneChangeRight: { - ET.WARNING: Alert( - "Steer Right", - "Confirm Lane Change", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - EventName.laneChangeBlocked: { - ET.WARNING: Alert( - "Car in Blindspot", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), - }, - EventName.steerSaturated: { - ET.WARNING: Alert( - "take control", - "turn exceeds limit", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.), - }, - EventName.calibrationIncomplete: { - ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), - ET.NO_ENTRY: NoEntryAlert("Calibrating"), - }, - EventName.reverseGear: { - ET.PERMANENT: Alert( - "Reverse", - "", - AlertStatus.normal, AlertSize.full, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), - ET.USER_DISABLE: ImmediateDisableAlert("Reverse"), - ET.NO_ENTRY: NoEntryAlert("Reverse"), - }, - }) - - -if __name__ == '__main__': - # print all alerts by type and priority - from cereal.services import SERVICE_LIST - from collections import defaultdict - - event_names = {v: k for k, v in EventName.schema.enumerants.items()} - alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list)) - - CP = car.CarParams.new_message() - CS = car.CarState.new_message() - sm = messaging.SubMaster(list(SERVICE_LIST.keys())) - - for i, alerts in EVENTS.items(): - for et, alert in alerts.items(): - if callable(alert): - alert = alert(CP, CS, sm, False, 1, log.LongitudinalPersonality.standard) - alerts_by_type[et][alert.priority].append(event_names[i]) - - all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {} - for et, priority_alerts in alerts_by_type.items(): - all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True) - - for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]): - print(f"**** {status} ****") - for p, alert_list in evs: - print(f" {repr(p)}:") - print(" ", ', '.join(alert_list), "\n") diff --git a/selfdrive/selfdrived/helpers.py b/selfdrive/selfdrived/helpers.py deleted file mode 100644 index f7468cbe4361ff..00000000000000 --- a/selfdrive/selfdrived/helpers.py +++ /dev/null @@ -1,54 +0,0 @@ -import math -from enum import StrEnum, auto - -from cereal import car, messaging -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.locationd.helpers import Pose -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY -from opendbc.car.lateral import ISO_LATERAL_ACCEL -from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX - -MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL) -MIN_LATERAL_ENGAGE_BUFFER = int(1 / DT_CTRL) - - -class ExcessiveActuationType(StrEnum): - LONGITUDINAL = auto() - LATERAL = auto() - - -class ExcessiveActuationCheck: - def __init__(self): - self._excessive_counter = 0 - self._engaged_counter = 0 - - def update(self, sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose) -> ExcessiveActuationType | None: - # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc. - # longitudinal - accel_calibrated = calibrated_pose.acceleration.x - excessive_long_actuation = sm['carControl'].longActive and (accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2) - - # lateral - yaw_rate = calibrated_pose.angular_velocity.yaw - roll = sm['liveParameters'].roll - roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) - - # Prevent false positives after overriding - excessive_lat_actuation = False - self._engaged_counter = self._engaged_counter + 1 if sm['carControl'].latActive and not CS.steeringPressed else 0 - if self._engaged_counter > MIN_LATERAL_ENGAGE_BUFFER: - if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2: - excessive_lat_actuation = True - - # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements - livepose_valid = abs(CS.aEgo - accel_calibrated) < 2 - self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0 - - excessive_type = None - if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT: - if excessive_long_actuation: - excessive_type = ExcessiveActuationType.LONGITUDINAL - else: - excessive_type = ExcessiveActuationType.LATERAL - - return excessive_type diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py deleted file mode 100755 index 997c7e37701153..00000000000000 --- a/selfdrive/selfdrived/selfdrived.py +++ /dev/null @@ -1,533 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import threading - -import cereal.messaging as messaging - -from cereal import car, log -from msgq.visionipc import VisionIpcClient, VisionStreamType - - -from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL -from openpilot.common.swaglog import cloudlog -from openpilot.common.gps import get_gps_location_service - -from openpilot.selfdrive.car.car_specific import CarSpecificEvents -from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose -from openpilot.selfdrive.selfdrived.events import Events, ET -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.version import get_build_metadata -from openpilot.system.hardware import HARDWARE - -REPLAY = "REPLAY" in os.environ -SIMULATION = "SIMULATION" in os.environ -TESTING_CLOSET = "TESTING_CLOSET" in os.environ - -LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()} - -ThermalStatus = log.DeviceState.ThermalStatus -State = log.SelfdriveState.OpenpilotState -PandaType = log.PandaState.PandaType -LaneChangeState = log.LaneChangeState -LaneChangeDirection = log.LaneChangeDirection -EventName = log.OnroadEvent.EventName -ButtonType = car.CarState.ButtonEvent.Type -SafetyModel = car.CarParams.SafetyModel - -IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) - - -class SelfdriveD: - def __init__(self, CP=None): - self.params = Params() - - # Ensure the current branch is cached, otherwise the first cycle lags - build_metadata = get_build_metadata() - - if CP is None: - cloudlog.info("selfdrived is waiting for CarParams") - self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) - cloudlog.info("selfdrived got CarParams") - else: - self.CP = CP - - self.car_events = CarSpecificEvents(self.CP) - - self.pose_calibrator = PoseCalibrator() - self.calibrated_pose: Pose | None = None - self.excessive_actuation_check = ExcessiveActuationCheck() - self.excessive_actuation = self.params.get("Offroad_ExcessiveActuation") is not None - - # Setup sockets - self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) - - self.gps_location_service = get_gps_location_service(self.params) - self.gps_packets = [self.gps_location_service] - self.sensor_packets = ["accelerometer", "gyroscope"] - self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] - - # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches - self.car_state_sock = messaging.sub_sock('carState', timeout=20) - - ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] - if SIMULATION: - ignore += ['driverCameraState', 'managerState'] - if REPLAY: - # no vipc in replay will make them ignored anyways - ignore += ['roadCameraState', 'wideRoadCameraState'] - self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', - 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \ - self.camera_packets + self.sensor_packets + self.gps_packets, - ignore_alive=ignore, ignore_avg_freq=ignore, - ignore_valid=ignore, frequency=int(1/DT_CTRL)) - - # read params - self.is_metric = self.params.get_bool("IsMetric") - self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") - - car_recognized = self.CP.brand != 'mock' - - # cleanup old params - if not self.CP.alphaLongitudinalAvailable: - self.params.remove("AlphaLongitudinalEnabled") - if not self.CP.openpilotLongitudinalControl: - self.params.remove("ExperimentalMode") - - self.CS_prev = car.CarState.new_message() - self.AM = AlertManager() - self.events = Events() - - self.initialized = False - self.enabled = False - self.active = False - self.mismatch_counter = 0 - self.cruise_mismatch_counter = 0 - self.last_steering_pressed_frame = 0 - self.distance_traveled = 0 - self.last_functional_fan_frame = 0 - self.events_prev = [] - self.logged_comm_issue = None - self.not_running_prev = None - self.experimental_mode = False - self.personality = self.params.get("LongitudinalPersonality", return_default=True) - self.recalibrating_seen = False - self.state_machine = StateMachine() - self.rk = Ratekeeper(100, print_delay_threshold=None) - - # Determine startup event - self.startup_event = EventName.startup if build_metadata.openpilot.comma_remote and build_metadata.tested_channel else EventName.startupMaster - if HARDWARE.get_device_type() == 'mici': - self.startup_event = None - if not car_recognized: - self.startup_event = EventName.startupNoCar - elif car_recognized and self.CP.passive: - self.startup_event = EventName.startupNoControl - elif self.CP.secOcRequired and not self.CP.secOcKeyAvailable: - self.startup_event = EventName.startupNoSecOcKey - - if not car_recognized: - self.events.add(EventName.carUnrecognized, static=True) - set_offroad_alert("Offroad_CarUnrecognized", True) - elif self.CP.passive: - self.events.add(EventName.dashcamMode, static=True) - - def update_events(self, CS): - """Compute onroadEvents from carState""" - - self.events.clear() - - if self.sm['controlsState'].lateralControlState.which() == 'debugState': - self.events.add(EventName.joystickDebug) - self.startup_event = None - - if self.sm.recv_frame['alertDebug'] > 0: - self.events.add(EventName.longitudinalManeuver) - self.startup_event = None - - # Add startup event - if self.startup_event is not None: - self.events.add(self.startup_event) - self.startup_event = None - - # Don't add any more events if not initialized - if not self.initialized: - self.events.add(EventName.selfdriveInitializing) - return - - # Check for user bookmark press (bookmark button or end of LKAS button feedback) - if self.sm.updated['userBookmark']: - self.events.add(EventName.userBookmark) - - if self.sm.updated['audioFeedback']: - self.events.add(EventName.audioFeedback) - - # Don't add any more events while in dashcam mode - if self.CP.passive: - return - - # Block resume if cruise never previously enabled - resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: - self.events.add(EventName.resumeBlocked) - - if not self.CP.notCar: - self.events.add_from_msg(self.sm['driverMonitoringState'].events) - - # Add car events, ignore if CAN isn't valid - if CS.canValid: - car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() - self.events.add_from_msg(car_events) - - if self.CP.notCar: - # wait for everything to init first - if self.sm.frame > int(5. / DT_CTRL) and self.initialized: - # body always wants to enable - self.events.add(EventName.pcmEnable) - - # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 - if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ - (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): - self.events.add(EventName.pedalPressed) - - # Create events for temperature, disk space, and memory - if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: - self.events.add(EventName.overheat) - if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: - self.events.add(EventName.outOfSpace) - if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: - self.events.add(EventName.lowMemory) - - # Alert if fan isn't spinning for 5 seconds - if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: - if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: - # allow enough time for the fan controller in the panda to recover from stalls - if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: - self.events.add(EventName.fanMalfunction) - else: - self.last_functional_fan_frame = self.sm.frame - - # Handle calibration status - cal_status = self.sm['liveCalibration'].calStatus - if cal_status != log.LiveCalibrationData.Status.calibrated: - if cal_status == log.LiveCalibrationData.Status.uncalibrated: - self.events.add(EventName.calibrationIncomplete) - elif cal_status == log.LiveCalibrationData.Status.recalibrating: - if not self.recalibrating_seen: - set_offroad_alert("Offroad_Recalibration", True) - self.recalibrating_seen = True - self.events.add(EventName.calibrationRecalibrating) - else: - self.events.add(EventName.calibrationInvalid) - - # Lane departure warning - if self.is_ldw_enabled and self.sm.valid['driverAssistance']: - if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture: - self.events.add(EventName.ldw) - - # ****************************************************************************************** - # NOTE: To fork maintainers. - # Disabling or nerfing safety features will get you and your users banned from our servers. - # We recommend that you do not change these numbers from the defaults. - if self.sm.updated['liveCalibration']: - self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) - if self.sm.updated['livePose']: - device_pose = Pose.from_live_pose(self.sm['livePose']) - self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) - - if self.calibrated_pose is not None: - excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose) - if not self.excessive_actuation and excessive_actuation is not None: - set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text=str(excessive_actuation)) - self.excessive_actuation = True - - if self.excessive_actuation: - self.events.add(EventName.excessiveActuation) - # ****************************************************************************************** - - # Handle lane change - if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: - direction = self.sm['modelV2'].meta.laneChangeDirection - if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ - (CS.rightBlindspot and direction == LaneChangeDirection.right): - self.events.add(EventName.laneChangeBlocked) - else: - if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) - else: - self.events.add(EventName.preLaneChangeRight) - elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, - LaneChangeState.laneChangeFinishing): - self.events.add(EventName.laneChange) - - for i, pandaState in enumerate(self.sm['pandaStates']): - # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput - if i < len(self.CP.safetyConfigs): - safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ - pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ - pandaState.alternativeExperience != self.CP.alternativeExperience - else: - safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES - - # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda - if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: - self.events.add(EventName.controlsMismatch) - - if log.PandaState.FaultType.relayMalfunction in pandaState.faults: - self.events.add(EventName.relayMalfunction) - - # Handle HW and system malfunctions - # Order is very intentional here. Be careful when modifying this. - # All events here should at least have NO_ENTRY and SOFT_DISABLE. - num_events = len(self.events) - - not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} - if self.sm.recv_frame['managerState'] and len(not_running): - if not_running != self.not_running_prev: - cloudlog.event("process_not_running", not_running=not_running, error=True) - self.not_running_prev = not_running - if self.sm.recv_frame['managerState'] and not_running: - self.events.add(EventName.processNotRunning) - else: - if not SIMULATION and not self.rk.lagging: - if not self.sm.all_alive(self.camera_packets): - self.events.add(EventName.cameraMalfunction) - elif not self.sm.all_freq_ok(self.camera_packets): - self.events.add(EventName.cameraFrameRate) - if not REPLAY and self.rk.lagging: - self.events.add(EventName.selfdrivedLagging) - if self.sm['radarState'].radarErrors.canError: - self.events.add(EventName.canError) - elif self.sm['radarState'].radarErrors.radarUnavailableTemporary: - self.events.add(EventName.radarTempUnavailable) - elif any(self.sm['radarState'].radarErrors.to_dict().values()): - self.events.add(EventName.radarFault) - if not self.sm.valid['pandaStates']: - self.events.add(EventName.usbError) - if CS.canTimeout: - self.events.add(EventName.canBusMissing) - elif not CS.canValid: - self.events.add(EventName.canError) - - # generic catch-all. ideally, a more specific event should be added above instead - has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) - no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - if not self.sm.all_checks() and no_system_errors: - if not self.sm.all_alive(): - self.events.add(EventName.commIssue) - elif not self.sm.all_freq_ok(): - self.events.add(EventName.commIssueAvgFreq) - else: - self.events.add(EventName.commIssue) - - logs = { - 'invalid': [s for s, valid in self.sm.valid.items() if not valid], - 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], - 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - } - if logs != self.logged_comm_issue: - cloudlog.event("commIssue", error=True, **logs) - self.logged_comm_issue = logs - else: - self.logged_comm_issue = None - - if not self.CP.notCar: - if not self.sm['livePose'].posenetOK: - self.events.add(EventName.posenetInvalid) - if not self.sm['livePose'].inputsOK: - self.events.add(EventName.locationdTemporaryError) - if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY): - self.events.add(EventName.paramsdTemporaryError) - - # conservative HW alert. if the data or frequency are off, locationd will throw an error - if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets): - self.events.add(EventName.sensorDataInvalid) - - if not REPLAY: - # Check for mismatch between openpilot and car's PCM - cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) - self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 - if self.cruise_mismatch_counter > int(6. / DT_CTRL): - self.events.add(EventName.cruiseMismatch) - - # Send a "steering required alert" if saturation count has reached the limit - if CS.steeringPressed: - self.last_steering_pressed_frame = self.sm.frame - recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 - controlstate = self.sm['controlsState'] - lac = getattr(controlstate.lateralControlState, controlstate.lateralControlState.which()) - if lac.active and not recent_steer_pressed and not self.CP.notCar: - clipped_speed = max(CS.vEgo, 0.3) - actual_lateral_accel = controlstate.curvature * (clipped_speed**2) - desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2) - undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2 - turning = abs(desired_lateral_accel) > 1.0 - # TODO: lac.saturated includes speed and other checks, should be pulled out - if undershooting and turning and lac.saturated: - self.events.add(EventName.steerSaturated) - - # Check for FCW - stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 - model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking - planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled - if (planner_fcw or model_fcw) and not self.CP.notCar: - self.events.add(EventName.fcw) - - # GPS checks - gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0 - if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500): - self.events.add(EventName.noGps) - if gps_ok: - self.distance_traveled = 0 - self.distance_traveled += abs(CS.vEgo) * DT_CTRL - - # TODO: fix simulator - if not SIMULATION or REPLAY: - if self.sm['modelV2'].frameDropPerc > 20: - self.events.add(EventName.modeldLagging) - - # Decrement personality on distance button press - if self.CP.openpilotLongitudinalControl: - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): - self.personality = (self.personality - 1) % 3 - self.params.put_nonblocking('LongitudinalPersonality', self.personality) - self.events.add(EventName.personalityChanged) - - def data_sample(self): - _car_state = messaging.recv_one(self.car_state_sock) - CS = _car_state.carState if _car_state else self.CS_prev - - self.sm.update(0) - - if not self.initialized: - all_valid = CS.canValid and self.sm.all_checks() - timed_out = self.sm.frame * DT_CTRL > 6. - if all_valid or timed_out or (SIMULATION and not REPLAY): - available_streams = VisionIpcClient.available_streams("camerad", block=False) - if VisionStreamType.VISION_STREAM_ROAD not in available_streams: - self.sm.ignore_alive.append('roadCameraState') - self.sm.ignore_valid.append('roadCameraState') - if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: - self.sm.ignore_alive.append('wideRoadCameraState') - self.sm.ignore_valid.append('wideRoadCameraState') - - if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']): - self.state_machine.state = State.enabled - - self.initialized = True - cloudlog.event( - "selfdrived.initialized", - dt=self.sm.frame*DT_CTRL, - timeout=timed_out, - canValid=CS.canValid, - invalid=[s for s, valid in self.sm.valid.items() if not valid], - not_alive=[s for s, alive in self.sm.alive.items() if not alive], - not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - error=True, - ) - - # When the panda and selfdrived do not agree on controls_allowed - # we want to disengage openpilot. However the status from the panda goes through - # another socket other than the CAN messages and one can arrive earlier than the other. - # Therefore we allow a mismatch for two samples, then we trigger the disengagement. - if not self.enabled: - self.mismatch_counter = 0 - - # All pandas not in silent mode must have controlsAllowed when openpilot is enabled - if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] - if ps.safetyModel not in IGNORED_SAFETY_MODES): - self.mismatch_counter += 1 - - return CS - - def update_alerts(self, CS): - clear_event_types = set() - if ET.WARNING not in self.state_machine.current_alert_types: - clear_event_types.add(ET.WARNING) - if self.enabled: - clear_event_types.add(ET.NO_ENTRY) - - pers = LONGITUDINAL_PERSONALITY_MAP[self.personality] - alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, - self.state_machine.soft_disable_timer, pers]) - self.AM.add_many(self.sm.frame, alerts) - self.AM.process_alerts(self.sm.frame, clear_event_types) - - def publish_selfdriveState(self, CS): - # selfdriveState - ss_msg = messaging.new_message('selfdriveState') - ss_msg.valid = True - ss = ss_msg.selfdriveState - ss.enabled = self.enabled - ss.active = self.active - ss.state = self.state_machine.state - ss.engageable = not self.events.contains(ET.NO_ENTRY) - ss.experimentalMode = self.experimental_mode - ss.personality = self.personality - - ss.alertText1 = self.AM.current_alert.alert_text_1 - ss.alertText2 = self.AM.current_alert.alert_text_2 - ss.alertSize = self.AM.current_alert.alert_size - ss.alertStatus = self.AM.current_alert.alert_status - ss.alertType = self.AM.current_alert.alert_type - ss.alertSound = self.AM.current_alert.audible_alert - ss.alertHudVisual = self.AM.current_alert.visual_alert - - self.pm.send('selfdriveState', ss_msg) - - # onroadEvents - logged every second or on change - if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): - ce_send = messaging.new_message('onroadEvents', len(self.events)) - ce_send.valid = True - ce_send.onroadEvents = self.events.to_msg() - self.pm.send('onroadEvents', ce_send) - self.events_prev = self.events.names.copy() - - def step(self): - CS = self.data_sample() - self.update_events(CS) - if not self.CP.passive and self.initialized: - self.enabled, self.active = self.state_machine.update(self.events) - self.update_alerts(CS) - - self.publish_selfdriveState(CS) - - self.CS_prev = CS - - def params_thread(self, evt): - while not evt.is_set(): - self.is_metric = self.params.get_bool("IsMetric") - self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl - self.personality = self.params.get("LongitudinalPersonality", return_default=True) - time.sleep(0.1) - - def run(self): - e = threading.Event() - t = threading.Thread(target=self.params_thread, args=(e, )) - try: - t.start() - while True: - self.step() - self.rk.monitor_time() - finally: - e.set() - t.join() - - -def main(): - config_realtime_process(4, Priority.CTRL_HIGH) - s = SelfdriveD() - s.run() - -if __name__ == "__main__": - main() diff --git a/selfdrive/selfdrived/state.py b/selfdrive/selfdrived/state.py deleted file mode 100644 index 073ddb56eb9dc6..00000000000000 --- a/selfdrive/selfdrived/state.py +++ /dev/null @@ -1,98 +0,0 @@ -from cereal import log -from openpilot.selfdrive.selfdrived.events import Events, ET -from openpilot.common.realtime import DT_CTRL - -State = log.SelfdriveState.OpenpilotState - -SOFT_DISABLE_TIME = 3 # seconds -ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) -ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) - -class StateMachine: - def __init__(self): - self.current_alert_types = [ET.PERMANENT] - self.state = State.disabled - self.soft_disable_timer = 0 - - def update(self, events: Events): - # decrement the soft disable timer at every step, as it's reset on - # entrance in SOFT_DISABLING state - self.soft_disable_timer = max(0, self.soft_disable_timer - 1) - - self.current_alert_types = [ET.PERMANENT] - - # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING - if self.state != State.disabled: - # user and immediate disable always have priority in a non-disabled state - if events.contains(ET.USER_DISABLE): - self.state = State.disabled - self.current_alert_types.append(ET.USER_DISABLE) - - elif events.contains(ET.IMMEDIATE_DISABLE): - self.state = State.disabled - self.current_alert_types.append(ET.IMMEDIATE_DISABLE) - - else: - # ENABLED - if self.state == State.enabled: - if events.contains(ET.SOFT_DISABLE): - self.state = State.softDisabling - self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) - self.current_alert_types.append(ET.SOFT_DISABLE) - - elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): - self.state = State.overriding - self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] - - # SOFT DISABLING - elif self.state == State.softDisabling: - if not events.contains(ET.SOFT_DISABLE): - # no more soft disabling condition, so go back to ENABLED - self.state = State.enabled - - elif self.soft_disable_timer > 0: - self.current_alert_types.append(ET.SOFT_DISABLE) - - elif self.soft_disable_timer <= 0: - self.state = State.disabled - - # PRE ENABLING - elif self.state == State.preEnabled: - if not events.contains(ET.PRE_ENABLE): - self.state = State.enabled - else: - self.current_alert_types.append(ET.PRE_ENABLE) - - # OVERRIDING - elif self.state == State.overriding: - if events.contains(ET.SOFT_DISABLE): - self.state = State.softDisabling - self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) - self.current_alert_types.append(ET.SOFT_DISABLE) - elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)): - self.state = State.enabled - else: - self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] - - # DISABLED - elif self.state == State.disabled: - if events.contains(ET.ENABLE): - if events.contains(ET.NO_ENTRY): - self.current_alert_types.append(ET.NO_ENTRY) - - else: - if events.contains(ET.PRE_ENABLE): - self.state = State.preEnabled - elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): - self.state = State.overriding - else: - self.state = State.enabled - self.current_alert_types.append(ET.ENABLE) - - # Check if openpilot is engaged and actuators are enabled - enabled = self.state in ENABLED_STATES - active = self.state in ACTIVE_STATES - if active: - self.current_alert_types.append(ET.WARNING) - return enabled, active - diff --git a/selfdrive/selfdrived/tests/test_alertmanager.py b/selfdrive/selfdrived/tests/test_alertmanager.py deleted file mode 100644 index 030b7d4515c2f2..00000000000000 --- a/selfdrive/selfdrived/tests/test_alertmanager.py +++ /dev/null @@ -1,58 +0,0 @@ -import random - -from openpilot.selfdrive.selfdrived.events import Alert, EmptyAlert, EVENTS -from openpilot.selfdrive.selfdrived.alertmanager import AlertManager - - -class TestAlertManager: - - def test_duration(self): - """ - Enforce that an alert lasts for max(alert duration, duration the alert is added) - """ - for duration in range(1, 100): - alert = None - while not isinstance(alert, Alert): - event = random.choice([e for e in EVENTS.values() if len(e)]) - alert = random.choice(list(event.values())) - - alert.duration = duration - - # check two cases: - # - alert is added to AM for <= the alert's duration - # - alert is added to AM for > alert's duration - for greater in (True, False): - if greater: - add_duration = duration + random.randint(1, 10) - else: - add_duration = random.randint(1, duration) - show_duration = max(duration, add_duration) - - AM = AlertManager() - for frame in range(duration+10): - if frame < add_duration: - AM.add_many(frame, [alert, ]) - AM.process_alerts(frame, set()) - - shown = AM.current_alert != EmptyAlert - should_show = frame <= show_duration - assert shown == should_show, f"{frame=} {add_duration=} {duration=}" - - # check one case: - # - if alert is re-added to AM before it ends the duration is extended - if duration > 1: - AM = AlertManager() - show_duration = duration * 2 - for frame in range(duration * 2 + 10): - if frame == 0: - AM.add_many(frame, [alert, ]) - - if frame == duration: - # add alert one frame before it ends - assert AM.current_alert == alert - AM.add_many(frame, [alert, ]) - AM.process_alerts(frame, set()) - - shown = AM.current_alert != EmptyAlert - should_show = frame <= show_duration - assert shown == should_show, f"{frame=} {duration=}" diff --git a/selfdrive/selfdrived/tests/test_alerts.py b/selfdrive/selfdrived/tests/test_alerts.py deleted file mode 100644 index c9718069992b10..00000000000000 --- a/selfdrive/selfdrived/tests/test_alerts.py +++ /dev/null @@ -1,130 +0,0 @@ -import copy -import json -import os -import random -from PIL import Image, ImageDraw, ImageFont - -from cereal import log, car -from cereal.messaging import SubMaster -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS - -AlertSize = log.SelfdriveState.AlertSize - -OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json") - -# TODO: add callback alerts -ALERTS = [] -for event_types in EVENTS.values(): - for alert in event_types.values(): - ALERTS.append(alert) - - -class TestAlerts: - - @classmethod - def setup_class(cls): - with open(OFFROAD_ALERTS_PATH) as f: - cls.offroad_alerts = json.loads(f.read()) - - # Create fake objects for callback - cls.CS = car.CarState.new_message() - cls.CP = car.CarParams.new_message() - cfg = [c for c in CONFIGS if c.proc_name == 'selfdrived'][0] - cls.sm = SubMaster(cfg.pubs) - - def test_events_defined(self): - # Ensure all events in capnp schema are defined in events.py - events = log.OnroadEvent.EventName.schema.enumerants - - for name, e in events.items(): - if not name.endswith("DEPRECATED"): - fail_msg = f"{name} @{e} not in EVENTS" - assert e in EVENTS.keys(), fail_msg - - # ensure alert text doesn't exceed allowed width - def test_alert_text_length(self): - font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts") - regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") - bold_font_path = os.path.join(font_path, "Inter-Bold.ttf") - semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") - - max_text_width = 2160 - 300 # full screen width is usable, minus sidebar - draw = ImageDraw.Draw(Image.new('RGB', (0, 0))) - - fonts = { - AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)], - AlertSize.mid: [ImageFont.truetype(bold_font_path, 88), - ImageFont.truetype(regular_font_path, 66)], - } - - for alert in ALERTS: - if not isinstance(alert, Alert): - alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard) - - # for full size alerts, both text fields wrap the text, - # so it's unlikely that they would go past the max width - if alert.alert_size in (AlertSize.none, AlertSize.full): - continue - - for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): - if i >= len(fonts[alert.alert_size]): - break - - font = fonts[alert.alert_size][i] - left, _, right, _ = draw.textbbox((0, 0), txt, font) - width = right - left - msg = f"type: {alert.alert_type} msg: {txt}" - assert width <= max_text_width, msg - - def test_alert_sanity_check(self): - for event_types in EVENTS.values(): - for event_type, a in event_types.items(): - # TODO: add callback alerts - if not isinstance(a, Alert): - continue - - if a.alert_size == AlertSize.none: - assert len(a.alert_text_1) == 0 - assert len(a.alert_text_2) == 0 - elif a.alert_size == AlertSize.small: - assert len(a.alert_text_1) > 0 - assert len(a.alert_text_2) == 0 - elif a.alert_size == AlertSize.mid: - assert len(a.alert_text_1) > 0 - assert len(a.alert_text_2) > 0 - else: - assert len(a.alert_text_1) > 0 - - assert a.duration >= 0. - - if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE): - assert a.creation_delay == 0. - - def test_offroad_alerts(self): - params = Params() - for a in self.offroad_alerts: - # set the alert - alert = copy.copy(self.offroad_alerts[a]) - set_offroad_alert(a, True) - alert['extra'] = '' - assert alert == params.get(a) - - # then delete it - set_offroad_alert(a, False) - assert params.get(a) is None - - def test_offroad_alerts_extra_text(self): - params = Params() - for i in range(50): - # set the alert - a = random.choice(list(self.offroad_alerts)) - alert = self.offroad_alerts[a] - set_offroad_alert(a, True, extra_text="a"*i) - - written_alert = params.get(a) - assert "a"*i == written_alert['extra'] - assert alert["text"] == written_alert['text'] diff --git a/selfdrive/selfdrived/tests/test_state_machine.py b/selfdrive/selfdrived/tests/test_state_machine.py deleted file mode 100644 index b720f48f1ec02b..00000000000000 --- a/selfdrive/selfdrived/tests/test_state_machine.py +++ /dev/null @@ -1,92 +0,0 @@ -from cereal import log -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.selfdrived.state import StateMachine, SOFT_DISABLE_TIME -from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert - -State = log.SelfdriveState.OpenpilotState - -# The event types that maintain the current state -MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), - State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} -ALL_STATES = tuple(State.schema.enumerants.values()) -# The event types checked in DISABLED section of state machine -ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) - - -def make_event(event_types): - event = {} - for ev in event_types: - event[ev] = NormalPermanentAlert("alert") - EVENTS[0] = event - return 0 - - -class TestStateMachine: - def setup_method(self): - self.events = Events() - self.state_machine = StateMachine() - self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) - - def test_immediate_disable(self): - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) - self.state_machine.state = state - self.state_machine.update(self.events) - assert State.disabled == self.state_machine.state - self.events.clear() - - def test_user_disable(self): - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.events.add(make_event([et, ET.USER_DISABLE])) - self.state_machine.state = state - self.state_machine.update(self.events) - assert State.disabled == self.state_machine.state - self.events.clear() - - def test_soft_disable(self): - for state in ALL_STATES: - if state == State.preEnabled: # preEnabled considers NO_ENTRY instead - continue - for et in MAINTAIN_STATES[state]: - self.events.add(make_event([et, ET.SOFT_DISABLE])) - self.state_machine.state = state - self.state_machine.update(self.events) - assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling - self.events.clear() - - def test_soft_disable_timer(self): - self.state_machine.state = State.enabled - self.events.add(make_event([ET.SOFT_DISABLE])) - self.state_machine.update(self.events) - for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): - assert self.state_machine.state == State.softDisabling - self.state_machine.update(self.events) - - assert self.state_machine.state == State.disabled - - def test_no_entry(self): - # Make sure noEntry keeps us disabled - for et in ENABLE_EVENT_TYPES: - self.events.add(make_event([ET.NO_ENTRY, et])) - self.state_machine.update(self.events) - assert self.state_machine.state == State.disabled - self.events.clear() - - def test_no_entry_pre_enable(self): - # preEnabled with noEntry event - self.state_machine.state = State.preEnabled - self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) - self.state_machine.update(self.events) - assert self.state_machine.state == State.preEnabled - - def test_maintain_states(self): - # Given current state's event type, we should maintain state - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.state_machine.state = state - self.events.add(make_event([et])) - self.state_machine.update(self.events) - assert self.state_machine.state == state - self.events.clear() diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript new file mode 100644 index 00000000000000..db32887e7f4073 --- /dev/null +++ b/selfdrive/sensord/SConscript @@ -0,0 +1,19 @@ +Import('env', 'arch', 'common', 'cereal', 'messaging') + +sensors = [ + 'sensors/file_sensor.cc', + 'sensors/i2c_sensor.cc', + 'sensors/light_sensor.cc', + 'sensors/bmx055_accel.cc', + 'sensors/bmx055_gyro.cc', + 'sensors/bmx055_magn.cc', + 'sensors/bmx055_temp.cc', + 'sensors/lsm6ds3_accel.cc', + 'sensors/lsm6ds3_gyro.cc', + 'sensors/lsm6ds3_temp.cc', + 'sensors/mmc5603nj_magn.cc', +] +libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj'] +if arch == "larch64": + libs.append('i2c') +env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/selfdrive/sensord/__init__.py b/selfdrive/sensord/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/sensord/libdiag.h b/selfdrive/sensord/libdiag.h new file mode 100644 index 00000000000000..03a59464edc5c1 --- /dev/null +++ b/selfdrive/sensord/libdiag.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define DIAG_MAX_RX_PKT_SIZ 4096 + +bool Diag_LSM_Init(uint8_t* pIEnv); +bool Diag_LSM_DeInit(void); + +// DCI + +#define DIAG_CON_APSS 0x001 +#define DIAG_CON_MPSS 0x002 +#define DIAG_CON_LPASS 0x004 +#define DIAG_CON_WCNSS 0x008 + +enum { + DIAG_DCI_NO_ERROR = 1001, +} diag_dci_error_type; + +int diag_register_dci_client(int*, uint16_t*, int, void*); +int diag_log_stream_config(int client_id, int set_mask, uint16_t log_codes_array[], int num_codes); +int diag_register_dci_stream(void (*func_ptr_logs)(unsigned char *ptr, int len), void (*func_ptr_events)(unsigned char *ptr, int len)); +int diag_release_dci_client(int*); + +int diag_send_dci_async_req(int client_id, unsigned char buf[], int bytes, unsigned char *rsp_ptr, int rsp_len, + void (*func_ptr)(unsigned char *ptr, int len, void *data_ptr), void *data_ptr); + + +#ifdef __cplusplus +} +#endif diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py new file mode 100755 index 00000000000000..e38e2d4c33143d --- /dev/null +++ b/selfdrive/sensord/pigeond.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python3 +import sys +import time +import signal +import serial +import struct +import requests +import urllib.parse +from datetime import datetime +from typing import List, Optional + +from cereal import messaging +from common.params import Params +from system.swaglog import cloudlog +from selfdrive.hardware import TICI +from common.gpio import gpio_init, gpio_set +from selfdrive.hardware.tici.pins import GPIO + +UBLOX_TTY = "/dev/ttyHS0" + +UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00" +UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00" +UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00" +UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00" +UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03" +UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00" + +def set_power(enabled: bool) -> None: + gpio_init(GPIO.UBLOX_SAFEBOOT_N, True) + gpio_init(GPIO.UBLOX_PWR_EN, True) + gpio_init(GPIO.UBLOX_RST_N, True) + + gpio_set(GPIO.UBLOX_SAFEBOOT_N, True) + gpio_set(GPIO.UBLOX_PWR_EN, enabled) + gpio_set(GPIO.UBLOX_RST_N, enabled) + + +def add_ubx_checksum(msg: bytes) -> bytes: + A = B = 0 + for b in msg[2:]: + A = (A + b) % 256 + B = (B + A) % 256 + return msg + bytes([A, B]) + +def get_assistnow_messages(token: bytes) -> List[bytes]: + # make request + # TODO: implement adding the last known location + r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({ + 'token': token, + 'gnss': 'gps,glo', + 'datatype': 'eph,alm,aux', + }, safe=':,'), timeout=5) + assert r.status_code == 200, "Got invalid status code" + dat = r.content + + # split up messages + msgs = [] + while len(dat) > 0: + assert dat[:2] == b"\xB5\x62" + msg_len = 6 + (dat[5] << 8 | dat[4]) + 2 + msgs.append(dat[:msg_len]) + dat = dat[msg_len:] + return msgs + + +class TTYPigeon(): + def __init__(self): + self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0) + + def send(self, dat: bytes) -> None: + self.tty.write(dat) + + def receive(self) -> bytes: + dat = b'' + while len(dat) < 0x1000: + d = self.tty.read(0x40) + dat += d + if len(d) == 0: + break + return dat + + def set_baud(self, baud: int) -> None: + self.tty.baudrate = baud + + def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool: + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + if ack in dat: + cloudlog.debug("Received ACK from ublox") + return True + elif nack in dat: + cloudlog.error("Received NACK from ublox") + return False + elif time.monotonic() - st > timeout: + cloudlog.error("No response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None: + self.send(dat) + self.wait_for_ack(ack, nack) + + def wait_for_backup_restore_status(self, timeout: float = 1.) -> int: + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + position = dat.find(UBLOX_BACKUP_RESTORE_MSG) + if position >= 0 and len(dat) >= position + 11: + return dat[position + 10] + elif time.monotonic() - st > timeout: + cloudlog.error("No backup restore response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + +def initialize_pigeon(pigeon: TTYPigeon) -> None: + # try initializing a few times + for _ in range(10): + try: + pigeon.set_baud(9600) + + # up baud rate + pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A") + time.sleep(0.1) + pigeon.set_baud(460800) + + # other configuration messages + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x00\x00\x06\x18") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x01\x08\x22") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x03\x0A\x24") + pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10") + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63") + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24") + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81") + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74") + cloudlog.debug("pigeon configured") + + # try restoring almanac backup + pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") + restore_status = pigeon.wait_for_backup_restore_status() + if restore_status == 2: + cloudlog.warning("almanac backup restored") + elif restore_status == 3: + cloudlog.warning("no almanac backup found") + else: + cloudlog.error(f"failed to restore almanac backup, status: {restore_status}") + + # sending time to ublox + t_now = datetime.utcnow() + if t_now >= datetime(2021, 6, 1): + cloudlog.warning("Sending current time to ublox") + + # UBX-MGA-INI-TIME_UTC + msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack(" 0: + if dat[0] == 0x00: + cloudlog.warning("received invalid data from ublox, re-initing!") + initialize_pigeon(pigeon) + continue + + # send out to socket + msg = messaging.new_message('ubloxRaw', len(dat)) + msg.ubloxRaw = dat[:] + pm.send('ubloxRaw', msg) + +if __name__ == "__main__": + main() diff --git a/selfdrive/sensord/rawgps/compare.py b/selfdrive/sensord/rawgps/compare.py new file mode 100755 index 00000000000000..0ec15b81fb5467 --- /dev/null +++ b/selfdrive/sensord/rawgps/compare.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import cereal.messaging as messaging +from laika import constants + +if __name__ == "__main__": + sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss']) + + meas = None + while 1: + sm.update() + if sm['ubloxGnss'].which() == "measurementReport": + meas = sm['ubloxGnss'].measurementReport.measurements + if not sm.updated['qcomGnss'] or meas is None: + continue + report = sm['qcomGnss'].measurementReport + if report.source not in [0, 1]: + continue + GLONASS = report.source == 1 + recv_time = report.milliseconds / 1000 + + car = [] + print("qcom has ", list(sorted([x.svId for x in report.sv]))) + print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)]))) + for i in report.sv: + # match to ublox + tm = None + for m in meas: + if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS: + tm = m + if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS: + tm = m + if tm is None: + continue + + if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown: + sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000 + ublox_psuedorange = tm.pseudorange + qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT + if GLONASS: + glonass_freq = tm.glonassFrequencyIndex - 7 + ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler) + else: + ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler + qcom_speed = i.unfilteredSpeed + car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno)) + + if len(car) == 0: + print("nothing to compare") + continue + + pr_err, speed_err = 0., 0. + for c in car: + ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5] + pr_err += ublox_psuedorange - qcom_psuedorange + speed_err += ublox_speed - qcom_speed + pr_err /= len(car) + speed_err /= len(car) + print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err)) + for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)): # type: ignore + svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c + print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" % + (svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, + ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno)) + + + diff --git a/system/qcomgpsd/modemdiag.py b/selfdrive/sensord/rawgps/modemdiag.py similarity index 85% rename from system/qcomgpsd/modemdiag.py rename to selfdrive/sensord/rawgps/modemdiag.py index 5d72aeba9e9211..cc2bc5b2613e8a 100644 --- a/system/qcomgpsd/modemdiag.py +++ b/selfdrive/sensord/rawgps/modemdiag.py @@ -1,3 +1,5 @@ +import os +import time import select from serial import Serial from crcmod import mkCrcFun @@ -9,7 +11,18 @@ def __init__(self): self.pend = b'' def open_serial(self): - serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True) + def op(): + return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0) + try: + serial = op() + except Exception: + # TODO: this is a hack to get around modemmanager's exclusive open + print("unlocking serial...") + os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'') + os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'') + time.sleep(0.5) + os.system("sudo chmod 666 /dev/ttyUSB0") + serial = op() serial.flush() serial.reset_input_buffer() serial.reset_output_buffer() diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py new file mode 100755 index 00000000000000..7c4582902bcb91 --- /dev/null +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python3 +import os +import sys +import signal +import itertools +import math +import time +from typing import NoReturn +from struct import unpack_from, calcsize, pack +import cereal.messaging as messaging +from cereal import log +from system.swaglog import cloudlog +from laika.gps_time import GPSTime + +from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv +from selfdrive.sensord.rawgps.structs import dict_unpacker +from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv +from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv +from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv +from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT +from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT + +DEBUG = int(os.getenv("DEBUG", "0"))==1 + +miscStatusFields = { + "multipathEstimateIsValid": 0, + "directionIsValid": 1, +} + +measurementStatusFields = { + "subMillisecondIsValid": 0, + "subBitTimeIsKnown": 1, + "satelliteTimeIsKnown": 2, + "bitEdgeConfirmedFromSignal": 3, + "measuredVelocity": 4, + "fineOrCoarseVelocity": 5, + "lockPointValid": 6, + "lockPointPositive": 7, + + "lastUpdateFromDifference": 9, + "lastUpdateFromVelocityDifference": 10, + "strongIndicationOfCrossCorelation": 11, + "tentativeMeasurement": 12, + "measurementNotUsable": 13, + "sirCheckIsNeeded": 14, + "probationMode": 15, + + "multipathIndicator": 24, + "imdJammingIndicator": 25, + "lteB13TxJammingIndicator": 26, + "freshMeasurementIndicator": 27, +} + +measurementStatusGPSFields = { + "gpsRoundRobinRxDiversity": 18, + "gpsRxDiversity": 19, + "gpsLowBandwidthRxDiversityCombined": 20, + "gpsHighBandwidthNu4": 21, + "gpsHighBandwidthNu8": 22, + "gpsHighBandwidthUniform": 23, +} + +measurementStatusGlonassFields = { + "glonassMeanderBitEdgeValid": 16, + "glonassTimeMarkValid": 17 +} + +def main() -> NoReturn: + unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) + unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) + + unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) + unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) + + unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) + unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + + log_types = [ + LOG_GNSS_GPS_MEASUREMENT_REPORT, + LOG_GNSS_GLONASS_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + ] + pub_types = ['qcomGnss'] + unpack_position, _ = dict_unpacker(position_report) + log_types.append(LOG_GNSS_POSITION_REPORT) + pub_types.append("gpsLocation") + + # connect to modem + diag = ModemDiag() + + # NV enable OEMDRE + # TODO: it has to reboot for this to take effect + DIAG_NV_READ_F = 38 + DIAG_NV_WRITE_F = 39 + NV_GNSS_OEM_FEATURE_MASK = 7165 + + opcode, payload = send_recv(diag, DIAG_NV_WRITE_F, pack(' 0: + cloudlog.debug("have %d pending messages" % pending_msgs) + assert log_outer_length == len(inner_log_packet) + (log_inner_length, log_type, log_time), log_payload = unpack_from(' 0: + assert len(sats)//dat['svCount'] == size_meas_sv + for i in range(dat['svCount']): + sv = report.sv[i] + sv.init('measurementStatus') + sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)]) + for k,v in sat.items(): + if k == "parityErrorCount": + sv.gpsParityErrorCount = v + elif k == "frequencyIndex": + sv.glonassFrequencyIndex = v + elif k == "hemmingErrorCount": + sv.glonassHemmingErrorCount = v + elif k == "measurementStatus": + for kk,vv in itertools.chain(*measurement_status_fields): + setattr(sv.measurementStatus, kk, bool(v & (1< + +#include "common/swaglog.h" +#include "common/timing.h" + +BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Accel::init() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_ACCEL_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + + // 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; + } + +fail: + return ret; +} + +void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event) { + 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; + + 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); + +} diff --git a/selfdrive/sensord/sensors/bmx055_accel.h b/selfdrive/sensord/sensors/bmx055_accel.h new file mode 100644 index 00000000000000..86ec419cde2d4f --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_accel.h @@ -0,0 +1,37 @@ +#pragma once + +#include "selfdrive/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_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_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(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc new file mode 100644 index 00000000000000..74b22d8fef6ab5 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -0,0 +1,80 @@ +#include "bmx055_gyro.h" + +#include +#include + +#include "common/swaglog.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Gyro::init() { + int ret = 0; + uint8_t buffer[1]; + + ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_GYRO_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID); + ret = -1; + goto fail; + } + + // 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; + } + +fail: + return ret; +} + +void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event) { + 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); + + 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); + +} diff --git a/selfdrive/sensord/sensors/bmx055_gyro.h b/selfdrive/sensord/sensors/bmx055_gyro.h new file mode 100644 index 00000000000000..ed0c16ff05f134 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.h @@ -0,0 +1,37 @@ +#pragma once + +#include "selfdrive/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_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_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(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc new file mode 100644 index 00000000000000..a2c793eff61b35 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -0,0 +1,250 @@ +#include "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() { + int ret; + uint8_t buffer[1]; + 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 + ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + if(ret < 0) { + LOGE("Enabling power failed: %d", ret); + goto fail; + } + util::sleep_for(5); // wait until the chip is powered on + + // read chip ID + ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_MAGN_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID); + return -1; + } + + // 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; + } + + + return 0; + + fail: + 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; +} + + +void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) { + 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)); + + if (parse_xyz(buffer, &_x, &_y, &z)) { + 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); +} diff --git a/selfdrive/sensord/sensors/bmx055_magn.h b/selfdrive/sensord/sensors/bmx055_magn.h new file mode 100644 index 00000000000000..d60fd5515c15da --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.h @@ -0,0 +1,63 @@ +#pragma once +#include + +#include "selfdrive/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(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc new file mode 100644 index 00000000000000..85bdea9e61252a --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -0,0 +1,44 @@ +#include "bmx055_temp.h" + +#include + +#include "selfdrive/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() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_ACCEL_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + +fail: + return ret; +} + +void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event) { + 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; + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); +} diff --git a/selfdrive/sensord/sensors/bmx055_temp.h b/selfdrive/sensord/sensors/bmx055_temp.h new file mode 100644 index 00000000000000..5ffaa8fb6b3d79 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_temp.h @@ -0,0 +1,12 @@ +#pragma once + +#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "selfdrive/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(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/constants.h b/selfdrive/sensord/sensors/constants.h new file mode 100644 index 00000000000000..c216f838a5ab0e --- /dev/null +++ b/selfdrive/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/selfdrive/sensord/sensors/file_sensor.cc b/selfdrive/sensord/sensors/file_sensor.cc new file mode 100644 index 00000000000000..812a41fa8a256c --- /dev/null +++ b/selfdrive/sensord/sensors/file_sensor.cc @@ -0,0 +1,14 @@ +#include "file_sensor.h" + +#include + +FileSensor::FileSensor(std::string filename) : file(filename) { +} + +int FileSensor::init() { + return file.is_open() ? 0 : 1; +} + +FileSensor::~FileSensor() { + file.close(); +} diff --git a/selfdrive/sensord/sensors/file_sensor.h b/selfdrive/sensord/sensors/file_sensor.h new file mode 100644 index 00000000000000..c5b4643e16b506 --- /dev/null +++ b/selfdrive/sensord/sensors/file_sensor.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" +#include "selfdrive/sensord/sensors/sensor.h" + +class FileSensor : public Sensor { +protected: + std::ifstream file; + +public: + FileSensor(std::string filename); + ~FileSensor(); + int init(); + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc new file mode 100644 index 00000000000000..40dfa4a736516e --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -0,0 +1,28 @@ +#include "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) : bus(bus) { +} + +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); +} diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h new file mode 100644 index 00000000000000..7832475a98e927 --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include "cereal/gen/cpp/log.capnp.h" +#include "common/i2c.h" +#include "selfdrive/sensord/sensors/constants.h" +#include "selfdrive/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; + virtual uint8_t get_device_address() = 0; + +public: + I2CSensor(I2CBus *bus); + int read_register(uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint register_address, uint8_t data); + virtual int init() = 0; + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors/light_sensor.cc b/selfdrive/sensord/sensors/light_sensor.cc new file mode 100644 index 00000000000000..4497343684a925 --- /dev/null +++ b/selfdrive/sensord/sensors/light_sensor.cc @@ -0,0 +1,22 @@ +#include "light_sensor.h" + +#include + +#include "common/timing.h" +#include "selfdrive/sensord/sensors/constants.h" + +void LightSensor::get_event(cereal::SensorEventData::Builder &event) { + uint64_t start_time = nanos_since_boot(); + file.clear(); + file.seekg(0); + + int value; + file >> value; + + event.setSource(cereal::SensorEventData::SensorSource::RPR0521); + event.setVersion(1); + event.setSensor(SENSOR_LIGHT); + event.setType(SENSOR_TYPE_LIGHT); + event.setTimestamp(start_time); + event.setLight(value); +} diff --git a/selfdrive/sensord/sensors/light_sensor.h b/selfdrive/sensord/sensors/light_sensor.h new file mode 100644 index 00000000000000..faf901d41c633b --- /dev/null +++ b/selfdrive/sensord/sensors/light_sensor.h @@ -0,0 +1,8 @@ +#pragma once +#include "file_sensor.h" + +class LightSensor : public FileSensor { +public: + LightSensor(std::string filename) : FileSensor(filename){}; + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc new file mode 100644 index 00000000000000..d923986dbe2f51 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -0,0 +1,64 @@ +#include "lsm6ds3_accel.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" + +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Accel::init() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID && buffer[0] != LSM6DS3TRC_ACCEL_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + + if (buffer[0] == LSM6DS3TRC_ACCEL_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + // 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; + } + + +fail: + return ret; +} + +void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event) { + + uint64_t start_time = nanos_since_boot(); + 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; + + event.setSource(source); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {y, -x, z}; + auto svec = event.initAcceleration(); + svec.setV(xyz); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.h b/selfdrive/sensord/sensors/lsm6ds3_accel.h new file mode 100644 index 00000000000000..4a6b68744519c7 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.h @@ -0,0 +1,26 @@ +#pragma once + +#include "selfdrive/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_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#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_ODR_104HZ (0b0100 << 4) + + +class LSM6DS3_Accel : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; +public: + LSM6DS3_Accel(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc new file mode 100644 index 00000000000000..c7711d34e33262 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -0,0 +1,68 @@ +#include "lsm6ds3_gyro.h" + +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Gyro::init() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_GYRO_CHIP_ID && buffer[0] != LSM6DS3TRC_GYRO_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID); + ret = -1; + goto fail; + } + + if (buffer[0] == LSM6DS3TRC_GYRO_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + // 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; + } + + +fail: + return ret; +} + +void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event) { + + uint64_t start_time = nanos_since_boot(); + 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); + + event.setSource(source); + event.setVersion(2); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {y, -x, z}; + auto svec = event.initGyroUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.h b/selfdrive/sensord/sensors/lsm6ds3_gyro.h new file mode 100644 index 00000000000000..d7e8f0025a7c3a --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.h @@ -0,0 +1,26 @@ +#pragma once + +#include "selfdrive/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_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 + +// Constants +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) + + +class LSM6DS3_Gyro : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; +public: + LSM6DS3_Gyro(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.cc b/selfdrive/sensord/sensors/lsm6ds3_temp.cc new file mode 100644 index 00000000000000..1dd179d69e2f76 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.cc @@ -0,0 +1,50 @@ +#include "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 = 0; + uint8_t buffer[1]; + + ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != LSM6DS3_TEMP_CHIP_ID && buffer[0] != LSM6DS3TRC_TEMP_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID); + ret = -1; + goto fail; + } + + if (buffer[0] == LSM6DS3TRC_TEMP_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + +fail: + return ret; +} + +void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event) { + + 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; + + event.setSource(source); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + +} diff --git a/selfdrive/sensord/sensors/lsm6ds3_temp.h b/selfdrive/sensord/sensors/lsm6ds3_temp.h new file mode 100644 index 00000000000000..8188f467005fd7 --- /dev/null +++ b/selfdrive/sensord/sensors/lsm6ds3_temp.h @@ -0,0 +1,25 @@ +#pragma once + +#include "selfdrive/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(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.cc b/selfdrive/sensord/sensors/mmc5603nj_magn.cc new file mode 100644 index 00000000000000..7c654ce7a39890 --- /dev/null +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.cc @@ -0,0 +1,77 @@ +#include "mmc5603nj_magn.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" + +MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int MMC5603NJ_Magn::init() { + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1); + if(ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != MMC5603NJ_CHIP_ID) { + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID); + ret = -1; + goto fail; + } + + // Set 100 Hz + ret = set_register(MMC5603NJ_I2C_REG_ODR, 100); + 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; + } + + // Set compute measurement rate + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); + if (ret < 0) { + goto fail; + } + + // Enable continuous mode, set every 100 measurements + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11); + if (ret < 0) { + goto fail; + } + +fail: + return ret; +} + +void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) { + + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[9]; + int 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; + float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale; + float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale; + + 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 xyz[] = {x, y, z}; + auto svec = event.initMagneticUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/mmc5603nj_magn.h b/selfdrive/sensord/sensors/mmc5603nj_magn.h new file mode 100644 index 00000000000000..58840bbf277dd0 --- /dev/null +++ b/selfdrive/sensord/sensors/mmc5603nj_magn.h @@ -0,0 +1,29 @@ +#pragma once + +#include "selfdrive/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) + +class MMC5603NJ_Magn : public I2CSensor { + uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} +public: + MMC5603NJ_Magn(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/sensor.h b/selfdrive/sensord/sensors/sensor.h new file mode 100644 index 00000000000000..3fb58ad2ae60b5 --- /dev/null +++ b/selfdrive/sensord/sensors/sensor.h @@ -0,0 +1,10 @@ +#pragma once + +#include "cereal/gen/cpp/log.capnp.h" + +class Sensor { +public: + virtual ~Sensor() {}; + virtual int init() = 0; + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc new file mode 100644 index 00000000000000..65fe43f65f8dbe --- /dev/null +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -0,0 +1,116 @@ +#include + +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/i2c.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "selfdrive/sensord/sensors/bmx055_accel.h" +#include "selfdrive/sensord/sensors/bmx055_gyro.h" +#include "selfdrive/sensord/sensors/bmx055_magn.h" +#include "selfdrive/sensord/sensors/bmx055_temp.h" +#include "selfdrive/sensord/sensors/constants.h" +#include "selfdrive/sensord/sensors/light_sensor.h" +#include "selfdrive/sensord/sensors/lsm6ds3_accel.h" +#include "selfdrive/sensord/sensors/lsm6ds3_gyro.h" +#include "selfdrive/sensord/sensors/lsm6ds3_temp.h" +#include "selfdrive/sensord/sensors/mmc5603nj_magn.h" +#include "selfdrive/sensord/sensors/sensor.h" + +#define I2C_BUS_IMU 1 + +ExitHandler do_exit; + +int sensor_loop() { + I2CBus *i2c_bus_imu; + + try { + i2c_bus_imu = new I2CBus(I2C_BUS_IMU); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } + + BMX055_Accel bmx055_accel(i2c_bus_imu); + BMX055_Gyro bmx055_gyro(i2c_bus_imu); + BMX055_Magn bmx055_magn(i2c_bus_imu); + BMX055_Temp bmx055_temp(i2c_bus_imu); + + LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu); + LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu); + LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu); + + MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu); + + LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); + + // Sensor init + std::vector> sensors_init; // Sensor, required + sensors_init.push_back({&bmx055_accel, false}); + sensors_init.push_back({&bmx055_gyro, false}); + sensors_init.push_back({&bmx055_magn, false}); + sensors_init.push_back({&bmx055_temp, false}); + + sensors_init.push_back({&lsm6ds3_accel, true}); + sensors_init.push_back({&lsm6ds3_gyro, true}); + sensors_init.push_back({&lsm6ds3_temp, true}); + + sensors_init.push_back({&mmc5603nj_magn, false}); + + sensors_init.push_back({&light, true}); + + bool has_magnetometer = false; + + // Initialize sensors + std::vector sensors; + for (auto &sensor : sensors_init) { + int err = sensor.first->init(); + if (err < 0) { + // Fail on required sensors + if (sensor.second) { + LOGE("Error initializing sensors"); + return -1; + } + } else { + if (sensor.first == &bmx055_magn || sensor.first == &mmc5603nj_magn) { + has_magnetometer = true; + } + sensors.push_back(sensor.first); + } + } + + if (!has_magnetometer) { + LOGE("No magnetometer present"); + return -1; + } + + PubMaster pm({"sensorEvents"}); + + while (!do_exit) { + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + + const int num_events = sensors.size(); + MessageBuilder msg; + auto sensor_events = msg.initEvent().initSensorEvents(num_events); + + for (int i = 0; i < num_events; i++) { + auto event = sensor_events[i]; + sensors[i]->get_event(event); + } + + pm.send("sensorEvents", msg); + + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin)); + } + return 0; +} + +int main(int argc, char *argv[]) { + setpriority(PRIO_PROCESS, 0, -18); + return sensor_loop(); +} diff --git a/selfdrive/sensord/tests/__init__.py b/selfdrive/sensord/tests/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/sensord/tests/test_pigeond.py b/selfdrive/sensord/tests/test_pigeond.py new file mode 100755 index 00000000000000..d15b731d0c3f20 --- /dev/null +++ b/selfdrive/sensord/tests/test_pigeond.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import time +import unittest + +import cereal.messaging as messaging +from cereal.services import service_list +from common.gpio import gpio_read +from selfdrive.test.helpers import with_processes +from selfdrive.manager.process_config import managed_processes +from system.hardware import TICI +from system.hardware.tici.pins import GPIO + + +# TODO: test TTFF when we have good A-GNSS +class TestPigeond(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + def tearDown(self): + managed_processes['pigeond'].stop() + + @with_processes(['pigeond']) + def test_frequency(self): + sm = messaging.SubMaster(['ubloxRaw']) + + # setup time + time.sleep(2) + sm.update() + + for _ in range(int(10 * service_list['ubloxRaw'].frequency)): + sm.update() + assert sm.all_checks() + + def test_startup_time(self): + for _ in range(5): + sm = messaging.SubMaster(['ubloxRaw']) + managed_processes['pigeond'].start() + + start_time = time.monotonic() + for __ in range(10): + sm.update(1 * 1000) + if sm.updated['ubloxRaw']: + break + assert sm.rcv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time" + + et = time.monotonic() - start_time + assert et < 5, f"pigeond took {et:.1f}s to start" + managed_processes['pigeond'].stop() + + def test_turns_off_ublox(self): + for s in (0.1, 0.5, 1, 5): + managed_processes['pigeond'].start() + time.sleep(s) + managed_processes['pigeond'].stop() + + assert gpio_read(GPIO.UBLOX_RST_N) == 0 + assert gpio_read(GPIO.UBLOX_PWR_EN) == 0 + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/sensord/tests/test_sensord.py b/selfdrive/sensord/tests/test_sensord.py new file mode 100755 index 00000000000000..9fd918c9716139 --- /dev/null +++ b/selfdrive/sensord/tests/test_sensord.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +import time +import unittest + +import cereal.messaging as messaging +from system.hardware import TICI +from selfdrive.test.helpers import with_processes + +TEST_TIMESPAN = 10 + +SENSOR_CONFIGURATIONS = ( + { + ('bmx055', 'acceleration'), + ('bmx055', 'gyroUncalibrated'), + ('bmx055', 'magneticUncalibrated'), + ('bmx055', 'temperature'), + ('lsm6ds3', 'acceleration'), + ('lsm6ds3', 'gyroUncalibrated'), + ('lsm6ds3', 'temperature'), + ('rpr0521', 'light'), + }, + { + ('lsm6ds3', 'acceleration'), + ('lsm6ds3', 'gyroUncalibrated'), + ('lsm6ds3', 'temperature'), + ('mmc5603nj', 'magneticUncalibrated'), + ('rpr0521', 'light'), + }, + { + ('bmx055', 'acceleration'), + ('bmx055', 'gyroUncalibrated'), + ('bmx055', 'magneticUncalibrated'), + ('bmx055', 'temperature'), + ('lsm6ds3trc', 'acceleration'), + ('lsm6ds3trc', 'gyroUncalibrated'), + ('lsm6ds3trc', 'temperature'), + ('rpr0521', 'light'), + }, + { + ('lsm6ds3trc', 'acceleration'), + ('lsm6ds3trc', 'gyroUncalibrated'), + ('lsm6ds3trc', 'temperature'), + ('mmc5603nj', 'magneticUncalibrated'), + ('rpr0521', 'light'), + }, +) + + +class TestSensord(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + @with_processes(['sensord']) + def test_sensors_present(self): + sensor_events = messaging.sub_sock("sensorEvents", timeout=0.1) + + start_time_sec = time.time() + events = [] + while time.time() - start_time_sec < TEST_TIMESPAN: + events += messaging.drain_sock(sensor_events) + time.sleep(0.01) + + seen = set() + for event in events: + for measurement in event.sensorEvents: + # Filter out unset events + if measurement.version == 0: + continue + seen.add((str(measurement.source), measurement.which())) + + self.assertIn(seen, SENSOR_CONFIGURATIONS) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/sensord/tests/ttff_test.py b/selfdrive/sensord/tests/ttff_test.py new file mode 100755 index 00000000000000..e2cbc6d14463c8 --- /dev/null +++ b/selfdrive/sensord/tests/ttff_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import time +import atexit + +from cereal import messaging +from selfdrive.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/selfdrive/sentry.py b/selfdrive/sentry.py new file mode 100644 index 00000000000000..aa409ea394a5c0 --- /dev/null +++ b/selfdrive/sentry.py @@ -0,0 +1,75 @@ +"""Install exception handler for process crash.""" +import sentry_sdk +from enum import Enum +from sentry_sdk.integrations.threading import ThreadingIntegration + +from common.params import Params +from selfdrive.athena.registration import is_registered_device +from system.hardware import HARDWARE, PC +from system.swaglog import cloudlog +from system.version import get_branch, get_commit, get_origin, get_version, \ + is_comma_remote, is_dirty, is_tested_branch + + +class SentryProject(Enum): + # python project + SELFDRIVE = "https://6f3c7076c1e14b2aa10f5dde6dda0cc4@o33823.ingest.sentry.io/77924" + # native project + SELFDRIVE_NATIVE = "https://3e4b586ed21a4479ad5d85083b639bc6@o33823.ingest.sentry.io/157615" + + +def report_tombstone(fn: str, message: str, contents: str) -> None: + cloudlog.error({'tombstone': message}) + + with sentry_sdk.configure_scope() as scope: + scope.set_extra("tombstone_fn", fn) + scope.set_extra("tombstone", contents) + sentry_sdk.capture_message(message=message) + sentry_sdk.flush() + + +def capture_exception(*args, **kwargs) -> None: + cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + + try: + sentry_sdk.capture_exception(*args, **kwargs) + sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 + except Exception: + cloudlog.exception("sentry exception") + + +def set_tag(key: str, value: str) -> None: + sentry_sdk.set_tag(key, value) + + +def init(project: SentryProject) -> None: + # forks like to mess with this, so double check + comma_remote = is_comma_remote() and "commaai" in get_origin(default="") + if not comma_remote or not is_registered_device() or PC: + return + + env = "release" if is_tested_branch() else "master" + dongle_id = Params().get("DongleId", encoding='utf-8') + + integrations = [] + if project == SentryProject.SELFDRIVE: + integrations.append(ThreadingIntegration(propagate_hub=True)) + else: + sentry_sdk.utils.MAX_STRING_LENGTH = 8192 + + sentry_sdk.init(project.value, + default_integrations=False, + release=get_version(), + integrations=integrations, + traces_sample_rate=1.0, + environment=env) + + sentry_sdk.set_user({"id": dongle_id}) + sentry_sdk.set_tag("dirty", is_dirty()) + sentry_sdk.set_tag("origin", get_origin()) + sentry_sdk.set_tag("branch", get_branch()) + sentry_sdk.set_tag("commit", get_commit()) + sentry_sdk.set_tag("device", HARDWARE.get_device_type()) + + if project == SentryProject.SELFDRIVE: + sentry_sdk.Hub.current.start_session() diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py new file mode 100755 index 00000000000000..7dc002727e2ccd --- /dev/null +++ b/selfdrive/statsd.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +import os +import zmq +import time +from pathlib import Path +from collections import defaultdict +from datetime import datetime, timezone +from typing import NoReturn, Union, List, Dict + +from common.params import Params +from cereal.messaging import SubMaster +from system.swaglog import cloudlog +from system.hardware import HARDWARE +from common.file_helpers import atomic_write_in_dir +from system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty +from selfdrive.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S + + +class METRIC_TYPE: + GAUGE = 'g' + SAMPLE = 'sa' + +class StatLog: + def __init__(self): + self.pid = None + + def connect(self) -> None: + self.zctx = zmq.Context() + self.sock = self.zctx.socket(zmq.PUSH) + self.sock.setsockopt(zmq.LINGER, 10) + self.sock.connect(STATS_SOCKET) + self.pid = os.getpid() + + def _send(self, metric: str) -> None: + if os.getpid() != self.pid: + self.connect() + + try: + self.sock.send_string(metric, zmq.NOBLOCK) + except zmq.error.Again: + # drop :/ + pass + + def gauge(self, name: str, value: float) -> None: + self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}") + + # Samples will be recorded in a buffer and at aggregation time, + # statistical properties will be logged (mean, count, percentiles, ...) + def sample(self, name: str, value: float): + self._send(f"{name}:{value}|{METRIC_TYPE.SAMPLE}") + + +def main() -> NoReturn: + dongle_id = Params().get("DongleId", encoding='utf-8') + def get_influxdb_line(measurement: str, value: Union[float, Dict[str, float]], timestamp: datetime, tags: dict) -> str: + res = f"{measurement}" + for k, v in tags.items(): + res += f",{k}={str(v)}" + res += " " + + if isinstance(value, float): + value = {'value': value} + + for k, v in value.items(): + res += f"{k}={v}," + + res += f"dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n" + return res + + # open statistics socket + ctx = zmq.Context().instance() + sock = ctx.socket(zmq.PULL) + sock.bind(STATS_SOCKET) + + # initialize stats directory + Path(STATS_DIR).mkdir(parents=True, exist_ok=True) + + # initialize tags + tags = { + 'started': False, + 'version': get_short_version(), + 'branch': get_short_branch(), + 'dirty': is_dirty(), + 'origin': get_normalized_origin(), + 'deviceType': HARDWARE.get_device_type(), + } + + # subscribe to deviceState for started state + sm = SubMaster(['deviceState']) + + idx = 0 + last_flush_time = time.monotonic() + gauges = {} + samples: Dict[str, List[float]] = defaultdict(list) + while True: + started_prev = sm['deviceState'].started + sm.update() + + # Update metrics + while True: + try: + metric = sock.recv_string(zmq.NOBLOCK) + try: + metric_type = metric.split('|')[1] + metric_name = metric.split(':')[0] + metric_value = float(metric.split('|')[0].split(':')[1]) + + if metric_type == METRIC_TYPE.GAUGE: + gauges[metric_name] = metric_value + elif metric_type == METRIC_TYPE.SAMPLE: + samples[metric_name].append(metric_value) + else: + cloudlog.event("unknown metric type", metric_type=metric_type) + except Exception: + cloudlog.event("malformed metric", metric=metric) + except zmq.error.Again: + break + + # flush when started state changes or after FLUSH_TIME_S + if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev): + result = "" + current_time = datetime.utcnow().replace(tzinfo=timezone.utc) + tags['started'] = sm['deviceState'].started + + for key, value in gauges.items(): + result += get_influxdb_line(f"gauge.{key}", value, current_time, tags) + + for key, values in samples.items(): + values.sort() + sample_count = len(values) + sample_sum = sum(values) + + stats = { + 'count': sample_count, + 'min': values[0], + 'max': values[-1], + 'mean': sample_sum / sample_count, + } + for percentile in [0.05, 0.5, 0.95]: + value = values[int(round(percentile * (sample_count - 1)))] + stats[f"p{int(percentile * 100)}"] = value + + result += get_influxdb_line(f"sample.{key}", stats, current_time, tags) + + # clear intermediate data + gauges.clear() + samples.clear() + last_flush_time = time.monotonic() + + # check that we aren't filling up the drive + if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT: + if len(result) > 0: + stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}") + with atomic_write_in_dir(stats_path) as f: + f.write(result) + idx += 1 + else: + cloudlog.error("stats dir full") + + +if __name__ == "__main__": + main() +else: + statlog = StatLog() diff --git a/selfdrive/test/ci_shell.sh b/selfdrive/test/ci_shell.sh new file mode 100755 index 00000000000000..a5ff714b2d3cdd --- /dev/null +++ b/selfdrive/test/ci_shell.sh @@ -0,0 +1,19 @@ +#!/bin/bash -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +OP_ROOT="$DIR/../../" + +if [ -z "$BUILD" ]; then + docker pull ghcr.io/commaai/openpilot-base:latest +else + docker build --cache-from ghcr.io/commaai/openpilot-base:latest -t ghcr.io/commaai/openpilot-base:latest -f $OP_ROOT/Dockerfile.openpilot_base . +fi + +docker run \ + -it \ + --rm \ + --volume $OP_ROOT:$OP_ROOT \ + --workdir $PWD \ + --env PYTHONPATH=$OP_ROOT \ + ghcr.io/commaai/openpilot-base:latest \ + /bin/bash diff --git a/selfdrive/test/cpp_harness.py b/selfdrive/test/cpp_harness.py deleted file mode 100755 index f9f425102b8515..00000000000000 --- a/selfdrive/test/cpp_harness.py +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python3 -import subprocess -import sys - -from openpilot.common.prefix import OpenpilotPrefix - -with OpenpilotPrefix(): - ret = subprocess.call(sys.argv[1:]) - -sys.exit(ret) diff --git a/selfdrive/test/docker_build.sh b/selfdrive/test/docker_build.sh deleted file mode 100755 index 4d58a1507c2326..00000000000000 --- a/selfdrive/test/docker_build.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/usr/bin/env bash -set -e - -# To build sim and docs, you can run the following to mount the scons cache to the same place as in CI: -# mkdir -p .ci_cache/scons_cache -# sudo mount --bind /tmp/scons_cache/ .ci_cache/scons_cache - -SCRIPT_DIR=$(dirname "$0") -OPENPILOT_DIR=$SCRIPT_DIR/../../ -if [ -n "$TARGET_ARCHITECTURE" ]; then - PLATFORM="linux/$TARGET_ARCHITECTURE" - TAG_SUFFIX="-$TARGET_ARCHITECTURE" -else - PLATFORM="linux/$(uname -m)" - TAG_SUFFIX="" -fi - -source $SCRIPT_DIR/docker_common.sh $1 "$TAG_SUFFIX" - -DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR - -if [ -n "$PUSH_IMAGE" ]; then - docker push $REMOTE_TAG - docker tag $REMOTE_TAG $REMOTE_SHA_TAG - docker push $REMOTE_SHA_TAG -fi diff --git a/selfdrive/test/docker_common.sh b/selfdrive/test/docker_common.sh deleted file mode 100644 index 2887fff74bc32e..00000000000000 --- a/selfdrive/test/docker_common.sh +++ /dev/null @@ -1,18 +0,0 @@ -if [ "$1" = "base" ]; then - export DOCKER_IMAGE=openpilot-base - export DOCKER_FILE=Dockerfile.openpilot_base -elif [ "$1" = "prebuilt" ]; then - export DOCKER_IMAGE=openpilot-prebuilt - export DOCKER_FILE=Dockerfile.openpilot -else - echo "Invalid docker build image: '$1'" - exit 1 -fi - -export DOCKER_REGISTRY=ghcr.io/commaai -export COMMIT_SHA=$(git rev-parse HEAD) - -TAG_SUFFIX=$2 -LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX -REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG -REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA diff --git a/selfdrive/test/fuzzy_generation.py b/selfdrive/test/fuzzy_generation.py deleted file mode 100644 index 131dab47b23a85..00000000000000 --- a/selfdrive/test/fuzzy_generation.py +++ /dev/null @@ -1,81 +0,0 @@ -import capnp -import hypothesis.strategies as st -from typing import Any -from collections.abc import Callable -from functools import cache - -from cereal import log - -DrawType = Callable[[st.SearchStrategy], Any] - - -class FuzzyGenerator: - def __init__(self, draw: DrawType, real_floats: bool): - self.draw = draw - self.native_type_map = FuzzyGenerator._get_native_type_map(real_floats) - - def generate_native_type(self, field: str) -> st.SearchStrategy[bool | int | float | str | bytes]: - value_func = self.native_type_map.get(field) - if value_func is not None: - return value_func - else: - raise NotImplementedError(f'Invalid type: {field}') - - def generate_field(self, field: capnp.lib.capnp._StructSchemaField) -> st.SearchStrategy: - def rec(field_type: capnp.lib.capnp._DynamicStructReader) -> st.SearchStrategy: - type_which = field_type.which() - if type_which == 'struct': - return self.generate_struct(field.schema.elementType if base_type == 'list' else field.schema) - elif type_which == 'list': - return st.lists(rec(field_type.list.elementType)) - elif type_which == 'enum': - schema = field.schema.elementType if base_type == 'list' else field.schema - return st.sampled_from(list(schema.enumerants.keys())) - else: - return self.generate_native_type(type_which) - - try: - if hasattr(field.proto, 'slot'): - slot_type = field.proto.slot.type - base_type = slot_type.which() - return rec(slot_type) - else: - return self.generate_struct(field.schema) - except capnp.lib.capnp.KjException: - return self.generate_struct(field.schema) - - def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: str | None = None) -> st.SearchStrategy[dict[str, Any]]: - single_fill: tuple[str, ...] = (event,) if event else (self.draw(st.sampled_from(schema.union_fields)),) if schema.union_fields else () - fields_to_generate = schema.non_union_fields + single_fill - return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in fields_to_generate if not field.endswith('DEPRECATED')}) - - @staticmethod - @cache - def _get_native_type_map(real_floats: bool) -> dict[str, st.SearchStrategy]: - return { - 'bool': st.booleans(), - 'int8': st.integers(min_value=-2**7, max_value=2**7-1), - 'int16': st.integers(min_value=-2**15, max_value=2**15-1), - 'int32': st.integers(min_value=-2**31, max_value=2**31-1), - 'int64': st.integers(min_value=-2**63, max_value=2**63-1), - 'uint8': st.integers(min_value=0, max_value=2**8-1), - 'uint16': st.integers(min_value=0, max_value=2**16-1), - 'uint32': st.integers(min_value=0, max_value=2**32-1), - 'uint64': st.integers(min_value=0, max_value=2**64-1), - 'float32': st.floats(width=32, allow_nan=not real_floats, allow_infinity=not real_floats), - 'float64': st.floats(width=64, allow_nan=not real_floats, allow_infinity=not real_floats), - 'text': st.text(max_size=1000), - 'data': st.binary(max_size=1000), - 'anyPointer': st.text(), # Note: No need to define a separate function for anyPointer - } - - @classmethod - def get_random_msg(cls, draw: DrawType, struct: capnp.lib.capnp._StructModule, real_floats: bool = False) -> dict[str, Any]: - fg = cls(draw, real_floats=real_floats) - data: dict[str, Any] = draw(fg.generate_struct(struct.schema)) - return data - - @classmethod - def get_random_event_msg(cls, draw: DrawType, events: list[str], real_floats: bool = False) -> list[dict[str, Any]]: - fg = cls(draw, real_floats=real_floats) - return [draw(fg.generate_struct(log.Event.schema, e)) for e in sorted(events)] diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 81635aa31f669b..8cc996c28dbf35 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -1,128 +1,58 @@ -import contextlib -import http.server import os -import threading import time -import pytest - from functools import wraps -import cereal.messaging as messaging -from openpilot.common.params import Params -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.version import training_version, terms_version +from system.hardware import PC +from selfdrive.manager.process_config import managed_processes +from system.version import training_version, terms_version def set_params_enabled(): - os.environ['FINGERPRINT'] = "TOYOTA_COROLLA_TSS2" - os.environ['LOGPRINT'] = "debug" - + from common.params import Params params = Params() params.put("HasAcceptedTerms", terms_version) params.put("CompletedTrainingVersion", training_version) params.put_bool("OpenpilotEnabledToggle", True) + params.put_bool("Passive", False) - # valid calib - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = 20 - msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - params.put("CalibrationParams", msg.to_bytes()) + +def phone_only(f): + @wraps(f) + def wrap(self, *args, **kwargs): + if PC: + self.skipTest("This test is not meant to run on PC") + f(self, *args, **kwargs) + return wrap def release_only(f): @wraps(f) def wrap(self, *args, **kwargs): if "RELEASE" not in os.environ: - pytest.skip("This test is only for release branches") + self.skipTest("This test is only for release branches") f(self, *args, **kwargs) return wrap - -@contextlib.contextmanager -def processes_context(processes, init_time=0, ignore_stopped=None): +def with_processes(processes, init_time=0, ignore_stopped=None): ignore_stopped = [] if ignore_stopped is None else ignore_stopped - # start and assert started - for n, p in enumerate(processes): - managed_processes[p].start() - if n < len(processes) - 1: - time.sleep(init_time) - - assert all(managed_processes[name].proc.exitcode is None for name in processes) - - try: - yield [managed_processes[name] for name in processes] - # assert processes are still started - assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) - finally: - for p in processes: - managed_processes[p].stop() - - -def with_processes(processes, init_time=0, ignore_stopped=None): def wrapper(func): @wraps(func) def wrap(*args, **kwargs): - with processes_context(processes, init_time, ignore_stopped): - return func(*args, **kwargs) + # start and assert started + for n, p in enumerate(processes): + managed_processes[p].start() + if n < len(processes) - 1: + time.sleep(init_time) + assert all(managed_processes[name].proc.exitcode is None for name in processes) + + # call the function + try: + func(*args, **kwargs) + # assert processes are still started + assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) + finally: + for p in processes: + managed_processes[p].stop() return wrap return wrapper - - -def noop(*args, **kwargs): - pass - - -def read_segment_list(segment_list_path): - with open(segment_list_path) as f: - seg_list = f.read().splitlines() - - return [(platform[2:], segment) for platform, segment in zip(seg_list[::2], seg_list[1::2], strict=True)] - - -@contextlib.contextmanager -def http_server_context(handler, setup=None): - host = '127.0.0.1' - server = http.server.HTTPServer((host, 0), handler) - port = server.server_port - t = threading.Thread(target=server.serve_forever) - t.start() - - if setup is not None: - setup(host, port) - - try: - yield (host, port) - finally: - server.shutdown() - server.server_close() - t.join() - - -def with_http_server(func, handler=http.server.BaseHTTPRequestHandler, setup=None): - @wraps(func) - def inner(*args, **kwargs): - with http_server_context(handler, setup) as (host, port): - return func(*args, f"http://{host}:{port}", **kwargs) - return inner - - -def DirectoryHttpServer(directory) -> type[http.server.SimpleHTTPRequestHandler]: - # creates an http server that serves files from directory - class Handler(http.server.SimpleHTTPRequestHandler): - API_NO_RESPONSE = False - API_BAD_RESPONSE = False - - def do_GET(self): - if self.API_NO_RESPONSE: - return - - if self.API_BAD_RESPONSE: - self.send_response(500, "") - return - super().do_GET() - - def __init__(self, *args, **kwargs): - super().__init__(*args, directory=str(directory), **kwargs) - - return Handler diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index ba0379f2d725bf..dad5d898447820 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -1,8 +1,8 @@ import numpy as np -from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant +from selfdrive.test.longitudinal_maneuvers.plant import Plant -class Maneuver: +class Maneuver(): def __init__(self, title, duration, **kwargs): # Was tempted to make a builder class self.distance_lead = kwargs.get("initial_distance_lead", 200.0) @@ -12,18 +12,11 @@ def __init__(self, title, duration, **kwargs): self.breakpoints = kwargs.get("breakpoints", [0.0, duration]) self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))]) self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))]) - self.prob_throttle_values = kwargs.get("prob_throttle_values", [1.0 for i in range(len(self.breakpoints))]) self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))]) - self.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))]) self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) self.ensure_start = kwargs.get("ensure_start", False) - self.ensure_slowdown = kwargs.get("ensure_slowdown", False) - self.enabled = kwargs.get("enabled", True) - self.e2e = kwargs.get("e2e", False) - self.personality = kwargs.get("personality", 0) - self.force_decel = kwargs.get("force_decel", False) self.duration = duration self.title = title @@ -33,52 +26,36 @@ def evaluate(self): lead_relevancy=self.lead_relevancy, speed=self.speed, distance_lead=self.distance_lead, - enabled=self.enabled, only_lead2=self.only_lead2, only_radar=self.only_radar, - e2e=self.e2e, - personality=self.personality, - force_decel=self.force_decel, ) valid = True logs = [] - while plant.current_time < self.duration: - speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) - prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) - cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) - pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values) - prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values) - log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle) + while plant.current_time() < self.duration: + speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values) + log = plant.step(speed_lead, prob, cruise) d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. log['d_rel'] = d_rel log['v_rel'] = v_rel - logs.append(np.array([plant.current_time, + logs.append(np.array([plant.current_time(), log['distance'], log['distance_lead'], log['speed'], speed_lead, - log['acceleration'], - log['d_rel']])) + log['acceleration']])) - if d_rel < .4 and (self.only_radar or prob_lead > 0.5): + if d_rel < .4 and (self.only_radar or prob > 0.5): print("Crashed!!!!") valid = False - if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3: + if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: print('LongitudinalPlanner not starting!') valid = False - if self.ensure_slowdown and log['speed'] > 5.5: - print('LongitudinalPlanner not slowing down!') - valid = False - - if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04: - print('Not stopping with force decel') - valid = False - - print("maneuver end", valid) return valid, np.array(logs) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index b8c6adb4368028..21af1cd3b18e21 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -4,24 +4,22 @@ from cereal import log import cereal.messaging as messaging -from openpilot.common.realtime import Ratekeeper, DT_MDL -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner -from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from common.realtime import Ratekeeper, DT_MDL +from selfdrive.controls.lib.longcontrol import LongCtrlState +from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner -class Plant: +class Plant(): messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): + only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: Plant.radar = messaging.pub_sock('radarState') Plant.controls_state = messaging.pub_sock('controlsState') - Plant.selfdrive_state = messaging.pub_sock('selfdriveState') Plant.car_state = messaging.pub_sock('carState') Plant.plan = messaging.sub_sock('longitudinalPlan') Plant.messaging_initialized = True @@ -30,42 +28,34 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, self.distance = 0. self.speed = speed - self.should_stop = False self.acceleration = 0.0 + self.speeds = [] # lead car - self.lead_relevancy = lead_relevancy self.distance_lead = distance_lead - self.enabled = enabled - self.only_lead2 = only_lead2 - self.only_radar = only_radar - self.e2e = e2e - self.personality = personality - self.force_decel = force_decel + self.lead_relevancy = lead_relevancy + self.only_lead2=only_lead2 + self.only_radar=only_radar self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate - time.sleep(0.1) + time.sleep(1) self.sm = messaging.SubMaster(['longitudinalPlan']) - from opendbc.car.honda.values import CAR - from opendbc.car.honda.interface import CarInterface + from selfdrive.car.honda.values import CAR + from selfdrive.car.honda.interface import CarInterface - self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=self.speed) + self.planner = LongitudinalPlanner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed) - @property def current_time(self): return float(self.rk.frame) / self.rate - def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0): + def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') - ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') - lp = messaging.new_message('liveParameters') - car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead @@ -75,28 +65,27 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle v_rel = v_lead - self.speed if self.only_radar: status = True - elif prob_lead > .5: + elif prob > .5: status = True else: status = False else: d_rel = 200. v_rel = 0. - prob_lead = 0.0 + prob = 0.0 status = False lead = log.RadarState.LeadData.new_message() lead.dRel = float(d_rel) - lead.yRel = 0.0 + lead.yRel = float(0.0) lead.vRel = float(v_rel) lead.aRel = float(a_lead - self.acceleration) lead.vLead = float(v_lead) lead.vLeadK = float(v_lead) lead.aLeadK = float(a_lead) - # TODO use real radard logic for this - lead.aLeadTau = float(_LEAD_ACCEL_TAU) + lead.aLeadTau = float(1.5) lead.status = status - lead.modelProb = float(prob_lead) + lead.modelProb = float(prob) if not self.only_lead2: radar.radarState.leadOne = lead radar.radarState.leadTwo = lead @@ -104,40 +93,32 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle # Simulate model predicting slightly faster speed # this is to ensure lead policy is effective when model # does not predict slowdown in e2e mode - position = log.XYZTData.new_message() - position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] + position = log.ModelDataV2.XYZTData.new_message() + position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)] model.modelV2.position = position - model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1) - velocity = log.XYZTData.new_message() - velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] - velocity.x[0] = float(self.speed) # always start at current speed + velocity = log.ModelDataV2.XYZTData.new_message() + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)] model.modelV2.velocity = velocity - acceleration = log.XYZTData.new_message() - acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + acceleration = log.ModelDataV2.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)] model.modelV2.acceleration = acceleration - model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] - control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off - ss.selfdriveState.experimentalMode = self.e2e - ss.selfdriveState.personality = self.personality - control.controlsState.forceDecel = self.force_decel + + + control.controlsState.longControlState = LongCtrlState.pid + control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) - car_state.carState.standstill = bool(self.speed < 0.01) - car_state.carState.vCruise = float(v_cruise * 3.6) - car_control.carControl.orientationNED = [0., float(pitch), 0.] + car_state.carState.standstill = self.speed < 0.01 # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, 'carState': car_state.carState, - 'carControl': car_control.carControl, 'controlsState': control.controlsState, - 'selfdriveState': ss.selfdriveState, - 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} self.planner.update(sm) - self.acceleration = self.planner.output_a_target - self.speed = self.speed + self.acceleration * self.ts - self.should_stop = self.planner.output_should_stop + self.speed = self.planner.v_desired_filter.x + self.acceleration = self.planner.a_desired + self.speeds = self.planner.v_desired_trajectory.tolist() fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts @@ -157,9 +138,9 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle v_rel = 0. # print at 5hz - # if (self.rk.frame % (self.rate // 5)) == 0: - # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" - # % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) + if (self.rk.frame % (self.rate // 5)) == 0: + print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** @@ -169,7 +150,7 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle "distance": self.distance, "speed": self.speed, "acceleration": self.acceleration, - "should_stop": self.should_stop, + "speeds": self.speeds, "distance_lead": self.distance_lead, "fcw": fcw, } diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py old mode 100644 new mode 100755 index ab1800b4fbb0b7..c7c2915878fafb --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,191 +1,162 @@ -import itertools -from parameterized import parameterized_class +#!/usr/bin/env python3 +import os +import unittest -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE -from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver +from common.params import Params +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # TODO: make new FCW tests -def create_maneuvers(kwargs): - maneuvers = [ - Maneuver( - 'approach stopped car at 25m/s, initial distance: 120m', - duration=20., - initial_speed=25., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[30., 0.], - breakpoints=[0., 1.], - **kwargs, - ), - Maneuver( - 'approach stopped car at 20m/s, initial distance 90m', - duration=20., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=90., - speed_lead_values=[20., 0.], - breakpoints=[0., 1.], - **kwargs, - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 35.0], - **kwargs, - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 25.0], - **kwargs, - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', - duration=50., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 21.66], - **kwargs, - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', - duration=40., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values=[20., 20., 0.], - prob_lead_values=[0., 1., 1.], - cruise_values=[20., 20., 20.], - breakpoints=[2., 2.01, 8.8], - **kwargs, - ), - Maneuver( - "approach stopped car at 20m/s, with prob_lead_values", - duration=30., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[0.0, 0., 0.], - prob_lead_values=[0.0, 0., 1.], - cruise_values=[20., 20., 20.], - breakpoints=[0.0, 2., 2.01], - **kwargs, - ), - Maneuver( - "approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1", - duration=30., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[0.0, 0., 0.], - prob_throttle_values=[1., 0., 0.], - cruise_values=[20., 20., 20.], - pitch_values=[0., -0.1, -0.1], - breakpoints=[0.0, 2., 2.01], - **kwargs, - ), - Maneuver( - "approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1", - duration=30., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=120., - speed_lead_values=[0.0, 0., 0.], - prob_throttle_values=[1., 0., 0.], - cruise_values=[20., 20., 20.], - pitch_values=[0., 0.1, 0.1], - breakpoints=[0.0, 2., 2.01], - **kwargs, - ), - Maneuver( - "approach slower cut-in car at 20m/s", - duration=20., - initial_speed=20., - lead_relevancy=True, - initial_distance_lead=50., - speed_lead_values=[15., 15.], - breakpoints=[1., 11.], - only_lead2=True, - **kwargs, - ), - Maneuver( - "stay stopped behind radar override lead", - duration=20., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[0., 0.], - prob_lead_values=[0., 0.], - breakpoints=[1., 11.], - only_radar=True, - **kwargs, - ), - Maneuver( - "NaN recovery", - duration=30., - initial_speed=15., - lead_relevancy=True, - initial_distance_lead=60., - speed_lead_values=[0., 0., 0.0], - breakpoints=[1., 1.01, 11.], - cruise_values=[float("nan"), 15., 15.], - **kwargs, - ), - Maneuver( - 'cruising at 25 m/s while disabled', - duration=20., - initial_speed=25., - lead_relevancy=False, - enabled=False, - **kwargs, - ), - Maneuver( - "slow to 5m/s with allow_throttle = False and pitch = +0.1", - duration=30., - initial_speed=20., - lead_relevancy=False, - prob_throttle_values=[1., 0., 0.], - cruise_values=[20., 20., 20.], - pitch_values=[0., 0.1, 0.1], - breakpoints=[0.0, 2., 2.01], - ensure_slowdown=True, - **kwargs, - )] - if not kwargs['force_decel']: - # controls relies on planner commanding to move for stock-ACC resume spamming - maneuvers.append(Maneuver( - "resume from a stop", - duration=20., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=STOP_DISTANCE, - speed_lead_values=[0., 0., 2.], - breakpoints=[1., 10., 15.], - ensure_start=True, - **kwargs, - )) - return maneuvers +maneuvers = [ + Maneuver( + 'approach stopped car at 20m/s, initial distance: 120m', + duration=20., + initial_speed=25., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[30., 0.], + breakpoints=[0., 1.], + ), + Maneuver( + 'approach stopped car at 20m/s, initial distance 90m', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=90., + speed_lead_values=[20., 0.], + breakpoints=[0., 1.], + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 35.0], + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 25.0], + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 21.66], + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', + duration=40., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + prob_lead_values=[0., 1., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[2., 2.01, 8.8], + ), + Maneuver( + "approach stopped car at 20m/s, with prob_lead_values", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_lead_values=[0.0, 0., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[0.0, 2., 2.01], + ), + Maneuver( + "approach slower cut-in car at 20m/s", + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + breakpoints=[1., 11.], + only_lead2=True, + ), + Maneuver( + "stay stopped behind radar override lead", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[0., 0.], + prob_lead_values=[0., 0.], + breakpoints=[1., 11.], + only_radar=True, + ), + Maneuver( + "NaN recovery", + duration=30., + initial_speed=15., + lead_relevancy=True, + initial_distance_lead=60., + speed_lead_values=[0., 0., 0.0], + breakpoints=[1., 1.01, 11.], + cruise_values=[float("nan"), 15., 15.], + ), + # controls relies on planner commanding to move for stock-ACC resume spamming + Maneuver( + "resume from a stop", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=STOP_DISTANCE, + speed_lead_values=[0., 0., 2.], + breakpoints=[1., 10., 15.], + ensure_start=True, + ), +] -@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2)) -class TestLongitudinalControl: - e2e: bool - force_decel: bool +class LongitudinalControl(unittest.TestCase): + @classmethod + def setUpClass(cls): + os.environ['SIMULATION'] = "1" + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['NO_CAN_TIMEOUT'] = "1" - def test_maneuver(self, subtests): - for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}): - with subtests.test(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel): - print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') - valid, _ = maneuver.evaluate() - assert valid + params = Params() + params.clear_all() + params.put_bool("Passive", bool(os.getenv("PASSIVE"))) + params.put_bool("OpenpilotEnabledToggle", True) + + # hack + def test_longitudinal_setup(self): + pass + + +def run_maneuver_worker(k): + def run(self): + params = Params() + + man = maneuvers[k] + params.put_bool("EndToEndLong", True) + print(man.title, ' in e2e mode') + valid, _ = man.evaluate() + self.assertTrue(valid, msg=man.title) + params.put_bool("EndToEndLong", False) + print(man.title, ' in acc mode') + valid, _ = man.evaluate() + self.assertTrue(valid, msg=man.title) + return run + +for k in range(len(maneuvers)): + setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}", + run_maneuver_worker(k)) + + +if __name__ == "__main__": + unittest.main(failfast=True) diff --git a/selfdrive/test/openpilotci.py b/selfdrive/test/openpilotci.py new file mode 100755 index 00000000000000..1d1b6ec423da5c --- /dev/null +++ b/selfdrive/test/openpilotci.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +import os +import sys +import subprocess + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" +TOKEN_PATH = "/data/azure_token" + + +def get_url(route_name, segment_num, log_type="rlog"): + ext = "hevc" if log_type.endswith('camera') else "bz2" + return BASE_URL + f"{route_name.replace('|', '/')}/{segment_num}/{log_type}.{ext}" + + +def upload_file(path, name): + from azure.storage.blob import BlockBlobService # pylint: disable=import-error + + sas_token = os.environ.get("AZURE_TOKEN", None) + if os.path.isfile(TOKEN_PATH): + sas_token = open(TOKEN_PATH).read().strip() + + if sas_token is None: + sas_token = subprocess.check_output("az storage container generate-sas --account-name commadataci --name openpilotci --https-only --permissions lrw \ + --expiry $(date -u '+%Y-%m-%dT%H:%M:%SZ' -d '+1 hour') --auth-mode login --as-user --output tsv", shell=True).decode().strip("\n") + service = BlockBlobService(account_name="commadataci", sas_token=sas_token) + service.create_blob_from_path("openpilotci", name, path) + return "https://commadataci.blob.core.windows.net/openpilotci/" + name + + +if __name__ == "__main__": + for f in sys.argv[1:]: + name = os.path.basename(f) + url = upload_file(f, name) + print(url) diff --git a/selfdrive/test/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore index a35cd58d415c63..63c37e64e16552 100644 --- a/selfdrive/test/process_replay/.gitignore +++ b/selfdrive/test/process_replay/.gitignore @@ -1 +1,2 @@ fakedata/ +debayer_diff.txt diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index 8e279c71cd8b8d..531ddb3a023c46 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -5,7 +5,7 @@ Process replay is a regression test designed to identify any changes in the outp If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated. Use `test_processes.py` to run the test locally. -Log files are cached by default. Use `DISABLE_FILEREADER_CACHE='1' test_processes.py` to disable caching. +Use `FILEREADER_CACHE='1' test_processes.py` to cache log files. Currently the following processes are tested: @@ -15,9 +15,9 @@ Currently the following processes are tested: * calibrationd * dmonitoringd * locationd +* laikad * paramsd * ubloxd -* torqued ### Usage ``` @@ -30,97 +30,18 @@ optional arguments: --whitelist-cars WHITELIST_CARS Whitelist given cars from the test (e.g. HONDA) --blacklist-procs BLACKLIST_PROCS Blacklist given processes from the test (e.g. controlsd) --blacklist-cars BLACKLIST_CARS Blacklist given cars from the test (e.g. HONDA) - --ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. driverMonitoringState.events) - --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents) + --ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. carState.events) + --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. carEvents) --update-refs Updates reference logs using current commit --upload-only Skips testing processes and uploads logs from previous test run ``` ## Forks -openpilot forks can use this test with their own reference logs, by default `test_proccesses.py` saves logs locally. +openpilot forks can use this test with their own reference logs, by default `test_proccess.py` saves logs locally. To generate new logs: `./test_processes.py` Then, check in the new logs using git-lfs. Make sure to also update the `ref_commit` file to the current commit. - -## API - -Process replay test suite exposes programmatic APIs for simultaneously running processes or groups of processes on provided logs. - -```py -def replay_process_with_name(name: Union[str, Iterable[str]], lr: LogIterable, *args, **kwargs) -> List[capnp._DynamicStructReader]: - -def replay_process( - cfg: Union[ProcessConfig, Iterable[ProcessConfig]], lr: LogIterable, frs: Optional[Dict[str, Any]] = None, - fingerprint: Optional[str] = None, return_all_logs: bool = False, custom_params: Optional[Dict[str, Any]] = None, disable_progress: bool = False -) -> List[capnp._DynamicStructReader]: -``` - -Example usage: -```py -from openpilot.selfdrive.test.process_replay import replay_process_with_name -from openpilot.tools.lib.logreader import LogReader - -lr = LogReader(...) - -# provide a name of the process to replay -output_logs = replay_process_with_name('locationd', lr) - -# or list of names -output_logs = replay_process_with_name(['ubloxd', 'locationd'], lr) -``` - -Supported processes: -* controlsd -* radard -* plannerd -* calibrationd -* dmonitoringd -* locationd -* paramsd -* ubloxd -* torqued -* modeld -* dmonitoringmodeld - -Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. - -```py -from openpilot.selfdrive.test.process_replay import get_custom_params_from_lr - -previous_segment_lr = LogReader(...) -current_segment_lr = LogReader(...) - -custom_params = get_custom_params_from_lr(previous_segment_lr, 'last') - -output_logs = replay_process_with_name('calibrationd', lr, custom_params=custom_params) -``` - -Replaying processes that use VisionIPC (e.g. modeld, dmonitoringmodeld) require additional `frs` dictionary with camera states as keys and `FrameReader` objects as values. - -```py -from openpilot.tools.lib.framereader import FrameReader - -frs = { - 'roadCameraState': FrameReader(...), - 'wideRoadCameraState': FrameReader(...), - 'driverCameraState': FrameReader(...), -} - -output_logs = replay_process_with_name(['modeld', 'dmonitoringmodeld'], lr, frs=frs) -``` - -To capture stdout/stderr of the replayed process, `captured_output_store` can be provided. - -```py -output_store = dict() -# pass dictionary by reference, it will be filled with standard outputs - even if process replay fails -output_logs = replay_process_with_name(['radard', 'plannerd'], lr, captured_output_store=output_store) - -# entries with captured output in format { 'out': '...', 'err': '...' } will be added to provided dictionary for each replayed process -print(output_store['radard']['out']) # radard stdout -print(output_store['radard']['err']) # radard stderr -``` diff --git a/selfdrive/test/process_replay/__init__.py b/selfdrive/test/process_replay/__init__.py index b9942771864a51..e69de29bb2d1d6 100644 --- a/selfdrive/test/process_replay/__init__.py +++ b/selfdrive/test/process_replay/__init__.py @@ -1,2 +0,0 @@ -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, get_process_config, get_custom_params_from_lr, \ - replay_process, replay_process_with_name # noqa: F401 diff --git a/selfdrive/test/process_replay/capture.py b/selfdrive/test/process_replay/capture.py deleted file mode 100644 index 90c279ef35b1e7..00000000000000 --- a/selfdrive/test/process_replay/capture.py +++ /dev/null @@ -1,59 +0,0 @@ -import os -import sys - -from typing import no_type_check - -class FdRedirect: - def __init__(self, file_prefix: str, fd: int): - fname = os.path.join("/tmp", f"{file_prefix}.{fd}") - if os.path.exists(fname): - os.unlink(fname) - self.dest_fd = os.open(fname, os.O_WRONLY | os.O_CREAT) - self.dest_fname = fname - self.source_fd = fd - os.set_inheritable(self.dest_fd, True) - - def __del__(self): - os.close(self.dest_fd) - - def purge(self) -> None: - os.unlink(self.dest_fname) - - def read(self) -> bytes: - with open(self.dest_fname, "rb") as f: - return f.read() or b"" - - def link(self) -> None: - os.dup2(self.dest_fd, self.source_fd) - - -class ProcessOutputCapture: - def __init__(self, proc_name: str, prefix: str): - prefix = f"{proc_name}_{prefix}" - self.stdout_redirect = FdRedirect(prefix, 1) - self.stderr_redirect = FdRedirect(prefix, 2) - - def __del__(self): - self.stdout_redirect.purge() - self.stderr_redirect.purge() - - @no_type_check # ipython classes have incompatible signatures - def link_with_current_proc(self) -> None: - try: - # prevent ipykernel from redirecting stdout/stderr of python subprocesses - from ipykernel.iostream import OutStream - if isinstance(sys.stdout, OutStream): - sys.stdout = sys.__stdout__ - if isinstance(sys.stderr, OutStream): - sys.stderr = sys.__stderr__ - except ImportError: - pass - - # link stdout/stderr to the fifo - self.stdout_redirect.link() - self.stderr_redirect.link() - - def read_outerr(self) -> tuple[str, str]: - out_str = self.stdout_redirect.read().decode() - err_str = self.stderr_redirect.read().decode() - return out_str, err_str diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 13d51a636f1b9e..bf6daf5fed5aa9 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -1,16 +1,26 @@ #!/usr/bin/env python3 +import bz2 import sys import math -import capnp import numbers import dictdiffer from collections import Counter -from openpilot.tools.lib.logreader import LogReader +from tools.lib.logreader import LogReader EPSILON = sys.float_info.epsilon +def save_log(dest, log_msgs, compress=True): + dat = b"".join(msg.as_builder().to_bytes() for msg in log_msgs) + + if compress: + dat = bz2.compress(dat) + + with open(dest, "wb") as f: + f.write(dat) + + def remove_ignored_fields(msg, ignore): msg = msg.as_builder() for key in ignore: @@ -20,36 +30,43 @@ def remove_ignored_fields(msg, ignore): continue for k in keys[:-1]: - # indexing into list - if k.isdigit(): - attr = attr[int(k)] - else: - attr = getattr(attr, k) - - v = getattr(attr, keys[-1]) - if isinstance(v, bool): - val = False - elif isinstance(v, numbers.Number): - val = 0 - elif isinstance(v, (list, capnp.lib.capnp._DynamicListBuilder)): - val = [] + try: + attr = getattr(msg, k) + except AttributeError: + break else: - raise NotImplementedError(f"Unknown type: {type(v)}") - setattr(attr, keys[-1], val) - return msg + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + else: + raise NotImplementedError + setattr(attr, keys[-1], val) + return msg.as_reader() -def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None,): +def get_field_tolerance(diff_field, field_tolerances): + diff_field_str = diff_field[0] + for s in diff_field[1:]: + # loop until number in field + if not isinstance(s, str): + break + diff_field_str += '.'+s + if diff_field_str in field_tolerances: + return field_tolerances[diff_field_str] + + +def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None, field_tolerances=None): if ignore_fields is None: ignore_fields = [] if ignore_msgs is None: ignore_msgs = [] - tolerance = EPSILON if tolerance is None else tolerance + if field_tolerances is None: + field_tolerances = {} + default_tolerance = EPSILON if tolerance is None else tolerance - log1, log2 = ( - [m for m in log if m.which() not in ignore_msgs] - for log in (log1, log2) - ) + log1, log2 = (list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)) if len(log1) != len(log2): cnt1 = Counter(m.which() for m in log1) @@ -57,16 +74,17 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}\n\t\t{cnt1}\n\t\t{cnt2}") diff = [] - for msg1, msg2 in zip(log1, log2, strict=True): + for msg1, msg2 in zip(log1, log2): if msg1.which() != msg2.which(): + print(msg1, msg2) raise Exception("msgs not aligned between logs") - msg1 = remove_ignored_fields(msg1, ignore_fields) - msg2 = remove_ignored_fields(msg2, ignore_fields) + msg1_bytes = remove_ignored_fields(msg1, ignore_fields).as_builder().to_bytes() + msg2_bytes = remove_ignored_fields(msg2, ignore_fields).as_builder().to_bytes() - if msg1.to_bytes() != msg2.to_bytes(): - msg1_dict = msg1.as_reader().to_dict(verbose=True) - msg2_dict = msg2.as_reader().to_dict(verbose=True) + if msg1_bytes != msg2_bytes: + msg1_dict = msg1.to_dict(verbose=True) + msg2_dict = msg2.to_dict(verbose=True) dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) @@ -75,10 +93,13 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non def outside_tolerance(diff): try: if diff[0] == "change": + field_tolerance = default_tolerance + if (tol := get_field_tolerance(diff[1], field_tolerances)) is not None: + field_tolerance = tol a, b = diff[2] finite = math.isfinite(a) and math.isfinite(b) if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): - return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + return abs(a - b) > max(field_tolerance, field_tolerance * max(abs(a), abs(b))) except TypeError: pass return True @@ -89,62 +110,7 @@ def outside_tolerance(diff): return diff -def format_process_diff(diff): - diff_short, diff_long = "", "" - - if isinstance(diff, str): - diff_short += f" {diff}\n" - diff_long += f"\t{diff}\n" - else: - cnt: dict[str, int] = {} - for d in diff: - diff_long += f"\t{str(d)}\n" - - k = str(d[1]) - cnt[k] = 1 if k not in cnt else cnt[k] + 1 - - for k, v in sorted(cnt.items()): - diff_short += f" {k}: {v}\n" - - return diff_short, diff_long - - -def format_diff(results, log_paths, ref_commit): - diff_short, diff_long = "", "" - diff_long += f"***** tested against commit {ref_commit} *****\n" - - failed = False - for segment, result in list(results.items()): - diff_short += f"***** results for segment {segment} *****\n" - diff_long += f"***** differences for segment {segment} *****\n" - - for proc, diff in list(result.items()): - diff_long += f"*** process: {proc} ***\n" - diff_long += f"\tref: {log_paths[segment][proc]['ref']}\n" - diff_long += f"\tnew: {log_paths[segment][proc]['new']}\n\n" - - diff_short += f" {proc}\n" - - if isinstance(diff, str) or len(diff): - diff_short += f" ref: {log_paths[segment][proc]['ref']}\n" - diff_short += f" new: {log_paths[segment][proc]['new']}\n\n" - failed = True - - proc_diff_short, proc_diff_long = format_process_diff(diff) - - diff_long += proc_diff_long - diff_short += proc_diff_short - - return diff_short, diff_long, failed - - if __name__ == "__main__": log1 = list(LogReader(sys.argv[1])) log2 = list(LogReader(sys.argv[2])) - ignore_fields = sys.argv[3:] or ["logMonoTime"] - results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}} - log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}} - diff_short, diff_long, failed = format_diff(results, log_paths, None) - - print(diff_long) - print(diff_short) + print(compare_logs(log1, log2, sys.argv[3:])) diff --git a/selfdrive/test/process_replay/debayer_replay_ref_commit b/selfdrive/test/process_replay/debayer_replay_ref_commit new file mode 100644 index 00000000000000..551fc680bab9dd --- /dev/null +++ b/selfdrive/test/process_replay/debayer_replay_ref_commit @@ -0,0 +1 @@ +8f9ba7540b4549b4a57312129b8ff678d045f70f \ No newline at end of file diff --git a/selfdrive/test/process_replay/helpers.py b/selfdrive/test/process_replay/helpers.py new file mode 100644 index 00000000000000..8571f36c36f7cf --- /dev/null +++ b/selfdrive/test/process_replay/helpers.py @@ -0,0 +1,26 @@ +import os +import shutil +import uuid + +from common.params import Params + +class OpenpilotPrefix(object): + def __init__(self, prefix: str = None) -> None: + self.prefix = prefix if prefix else str(uuid.uuid4()) + self.msgq_path = os.path.join('/dev/shm', self.prefix) + + def __enter__(self): + os.environ['OPENPILOT_PREFIX'] = self.prefix + try: + os.mkdir(self.msgq_path) + except FileExistsError: + pass + + def __exit__(self, exc_type, exc_obj, exc_tb): + symlink_path = Params().get_param_path() + if os.path.exists(symlink_path): + shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True) + os.remove(symlink_path) + shutil.rmtree(self.msgq_path, ignore_errors=True) + del os.environ['OPENPILOT_PREFIX'] + return False diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py deleted file mode 100644 index 232722d1b1edc1..00000000000000 --- a/selfdrive/test/process_replay/migration.py +++ /dev/null @@ -1,473 +0,0 @@ -from collections import defaultdict -from collections.abc import Callable -from typing import cast -import capnp -import functools -import traceback - -from cereal import messaging, car, log -from opendbc.car.fingerprints import MIGRATION -from opendbc.car.toyota.values import EPS_SCALE, ToyotaSafetyFlags -from opendbc.car.ford.values import CAR as FORD, FordFlags, FordSafetyFlags -from opendbc.car.hyundai.values import HyundaiSafetyFlags -from opendbc.car.gm.values import GMSafetyFlags -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta -from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX -from openpilot.system.manager.process_config import managed_processes -from openpilot.tools.lib.logreader import LogIterable - -MessageWithIndex = tuple[int, capnp.lib.capnp._DynamicStructReader] -MigrationOps = tuple[list[tuple[int, capnp.lib.capnp._DynamicStructReader]], list[capnp.lib.capnp._DynamicStructReader], list[int]] -MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps] - - -# rules for migration functions -# 1. must use the decorator @migration(inputs=[...], product="...") and MigrationFunc signature -# 2. it only gets the messages that are in the inputs list -# 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): - migrations = [ - migrate_sensorEvents, - migrate_carParams, - migrate_gpsLocation, - migrate_deviceState, - migrate_carOutput, - migrate_controlsState, - migrate_carState, - migrate_liveLocationKalman, - migrate_liveTracks, - migrate_driverAssistance, - migrate_drivingModelData, - migrate_onroadEvents, - migrate_driverMonitoringState, - migrate_longitudinalPlan, - ] - if manager_states: - migrations.append(migrate_managerState) - if panda_states: - migrations.extend([migrate_pandaStates, migrate_peripheralState]) - if camera_states: - migrations.append(migrate_cameraStates) - - return migrate(lr, migrations) - - -def migrate(lr: LogIterable, migration_funcs: list[MigrationFunc]): - lr = list(lr) - grouped = defaultdict(list) - for i, msg in enumerate(lr): - grouped[msg.which()].append(i) - - replace_ops, add_ops, del_ops = [], [], [] - for migration in migration_funcs: - assert hasattr(migration, "inputs") and hasattr(migration, "product"), "Migration functions must use @migration decorator" - if migration.product in grouped: # skip if product already exists - continue - - sorted_indices = sorted(ii for i in cast(list[str], migration.inputs) for ii in grouped.get(i, [])) - msg_gen = [(i, lr[i]) for i in sorted_indices] - r_ops, a_ops, d_ops = migration(msg_gen) - replace_ops.extend(r_ops) - add_ops.extend(a_ops) - del_ops.extend(d_ops) - - for index, msg in replace_ops: - lr[index] = msg - for index in sorted(del_ops, reverse=True): - del lr[index] - for msg in add_ops: - lr.append(msg) - lr = sorted(lr, key=lambda x: x.logMonoTime) - - return lr - - -def migration(inputs: list[str], product: str|None=None): - def decorator(func): - @functools.wraps(func) - def wrapper(*args, **kwargs): - return func(*args, **kwargs) - wrapper.inputs = inputs - wrapper.product = product - return wrapper - return decorator - - -@migration(inputs=["longitudinalPlan", "carParams"]) -def migrate_longitudinalPlan(msgs): - ops = [] - - needs_migration = all(msg.longitudinalPlan.aTarget == 0.0 for _, msg in msgs if msg.which() == 'longitudinalPlan') - CP = next((m.carParams for _, m in msgs if m.which() == 'carParams'), None) - if not needs_migration or CP is None: - return [], [], [] - - for index, msg in msgs: - if msg.which() != 'longitudinalPlan': - continue - new_msg = msg.as_builder() - a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) - new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop) - ops.append((index, new_msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["longitudinalPlan"], product="driverAssistance") -def migrate_driverAssistance(msgs): - add_ops = [] - for _, msg in msgs: - new_msg = messaging.new_message('driverAssistance', valid=True, logMonoTime=msg.logMonoTime) - add_ops.append(new_msg.as_reader()) - return [], add_ops, [] - - -@migration(inputs=["modelV2"], product="drivingModelData") -def migrate_drivingModelData(msgs): - add_ops = [] - for _, msg in msgs: - dmd = messaging.new_message('drivingModelData', valid=msg.valid, logMonoTime=msg.logMonoTime) - for field in ["frameId", "frameIdExtra", "frameDropPerc", "modelExecutionTime", "action"]: - setattr(dmd.drivingModelData, field, getattr(msg.modelV2, field)) - for meta_field in ["laneChangeState", "laneChangeState"]: - setattr(dmd.drivingModelData.meta, meta_field, getattr(msg.modelV2.meta, meta_field)) - if len(msg.modelV2.laneLines) and len(msg.modelV2.laneLineProbs): - fill_lane_line_meta(dmd.drivingModelData.laneLineMeta, msg.modelV2.laneLines, msg.modelV2.laneLineProbs) - if all(len(a) for a in [msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z]): - fill_xyz_poly(dmd.drivingModelData.path, ModelConstants.POLY_PATH_DEGREE, msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z) - add_ops.append( dmd.as_reader()) - return [], add_ops, [] - - -@migration(inputs=["liveTracksDEPRECATED"], product="liveTracks") -def migrate_liveTracks(msgs): - ops = [] - for index, msg in msgs: - new_msg = messaging.new_message('liveTracks') - new_msg.valid = msg.valid - new_msg.logMonoTime = msg.logMonoTime - - pts = [] - for track in msg.liveTracksDEPRECATED: - pt = car.RadarData.RadarPoint() - pt.trackId = track.trackId - - pt.dRel = track.dRel - pt.yRel = track.yRel - pt.vRel = track.vRel - pt.aRel = track.aRel - pt.measured = True - pts.append(pt) - - new_msg.liveTracks.points = pts - ops.append((index, new_msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["liveLocationKalmanDEPRECATED"], product="livePose") -def migrate_liveLocationKalman(msgs): - nans = [float('nan')] * 3 - ops = [] - for index, msg in msgs: - m = messaging.new_message('livePose') - m.valid = msg.valid - m.logMonoTime = msg.logMonoTime - for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]: - lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field) - lp_field.x, lp_field.y, lp_field.z = llk_field.value or nans - lp_field.xStd, lp_field.yStd, lp_field.zStd = llk_field.std or nans - lp_field.valid = llk_field.valid - for flag in ["inputsOK", "posenetOK", "sensorsOK"]: - setattr(m.livePose, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag)) - ops.append((index, m.as_reader())) - return ops, [], [] - - -@migration(inputs=["controlsState"], product="selfdriveState") -def migrate_controlsState(msgs): - add_ops = [] - for _, msg in msgs: - m = messaging.new_message('selfdriveState') - m.valid = msg.valid - m.logMonoTime = msg.logMonoTime - ss = m.selfdriveState - for field in ("enabled", "active", "state", "engageable", "alertText1", "alertText2", - "alertStatus", "alertSize", "alertType", "experimentalMode", - "personality"): - setattr(ss, field, getattr(msg.controlsState, field+"DEPRECATED")) - add_ops.append(m.as_reader()) - return [], add_ops, [] - - -@migration(inputs=["carState", "controlsState"]) -def migrate_carState(msgs): - ops = [] - last_cs = None - for index, msg in msgs: - if msg.which() == 'controlsState': - last_cs = msg - elif msg.which() == 'carState' and last_cs is not None: - if last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruise > 0.1: - msg = msg.as_builder() - msg.carState.vCruise = last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruiseCluster = last_cs.controlsState.vCruiseClusterDEPRECATED - ops.append((index, msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["managerState"]) -def migrate_managerState(msgs): - ops = [] - for index, msg in msgs: - new_msg = msg.as_builder() - new_msg.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] - ops.append((index, new_msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["gpsLocation", "gpsLocationExternal"]) -def migrate_gpsLocation(msgs): - ops = [] - for index, msg in msgs: - new_msg = msg.as_builder() - g = getattr(new_msg, new_msg.which()) - # hasFix is a newer field - if not g.hasFix and g.flags == 1: - g.hasFix = True - ops.append((index, new_msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["deviceState", "initData"]) -def migrate_deviceState(msgs): - init_data = next((m.initData for _, m in msgs if m.which() == 'initData'), None) - device_state = next((m.deviceState for _, m in msgs if m.which() == 'deviceState'), None) - if init_data is None or device_state is None: - return [], [], [] - - ops = [] - for i, msg in msgs: - if msg.which() == 'deviceState': - n = msg.as_builder() - n.deviceState.deviceType = init_data.deviceType - ops.append((i, n.as_reader())) - return ops, [], [] - - -@migration(inputs=["carControl"], product="carOutput") -def migrate_carOutput(msgs): - add_ops = [] - for _, msg in msgs: - co = messaging.new_message('carOutput') - co.valid = msg.valid - co.logMonoTime = msg.logMonoTime - co.carOutput.actuatorsOutput = msg.carControl.actuatorsOutputDEPRECATED - add_ops.append(co.as_reader()) - return [], add_ops, [] - - -@migration(inputs=["pandaStates", "pandaStateDEPRECATED", "carParams"]) -def migrate_pandaStates(msgs): - # TODO: safety param migration should be handled automatically - safety_param_migration = { - "TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | ToyotaSafetyFlags.STOCK_LONGITUDINAL, - "TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | ToyotaSafetyFlags.ALT_BRAKE, - "KIA_EV6": HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CANFD_LKA_STEERING, - "CHEVROLET_VOLT": GMSafetyFlags.EV, - "CHEVROLET_BOLT_EUV": GMSafetyFlags.EV | GMSafetyFlags.HW_CAM, - } - # TODO: get new Ford route - safety_param_migration |= dict.fromkeys((set(FORD) - FORD.with_flags(FordFlags.CANFD)), FordSafetyFlags.LONG_CONTROL) - - # Migrate safety param base on carParams - CP = next((m.carParams for _, m in msgs if m.which() == 'carParams'), None) - assert CP is not None, "carParams message not found" - fingerprint = MIGRATION.get(CP.carFingerprint, CP.carFingerprint) - if fingerprint in safety_param_migration: - safety_param = safety_param_migration[fingerprint].value - elif len(CP.safetyConfigs): - safety_param = CP.safetyConfigs[0].safetyParam - if CP.safetyConfigs[0].safetyParamDEPRECATED != 0: - safety_param = CP.safetyConfigs[0].safetyParamDEPRECATED - else: - safety_param = CP.safetyParamDEPRECATED - - ops = [] - for index, msg in msgs: - if msg.which() == 'pandaStateDEPRECATED': - new_msg = messaging.new_message('pandaStates', 1) - new_msg.valid = msg.valid - new_msg.logMonoTime = msg.logMonoTime - new_msg.pandaStates[0] = msg.pandaStateDEPRECATED - new_msg.pandaStates[0].safetyParam = safety_param - ops.append((index, new_msg.as_reader())) - elif msg.which() == 'pandaStates': - new_msg = msg.as_builder() - new_msg.pandaStates[-1].safetyParam = safety_param - # Clear DISABLE_DISENGAGE_ON_GAS bit to fix controls mismatch - new_msg.pandaStates[-1].alternativeExperience &= ~1 - ops.append((index, new_msg.as_reader())) - return ops, [], [] - - -@migration(inputs=["pandaStates", "pandaStateDEPRECATED"], product="peripheralState") -def migrate_peripheralState(msgs): - add_ops = [] - - which = "pandaStates" if any(msg.which() == "pandaStates" for _, msg in msgs) else "pandaStateDEPRECATED" - for _, msg in msgs: - if msg.which() != which: - continue - new_msg = messaging.new_message("peripheralState") - new_msg.valid = msg.valid - new_msg.logMonoTime = msg.logMonoTime - add_ops.append(new_msg.as_reader()) - return [], add_ops, [] - - -@migration(inputs=["roadEncodeIdx", "wideRoadEncodeIdx", "driverEncodeIdx", "roadCameraState", "wideRoadCameraState", "driverCameraState"]) -def migrate_cameraStates(msgs): - add_ops, del_ops = [], [] - frame_to_encode_id = defaultdict(dict) - # just for encodeId fallback mechanism - min_frame_id = defaultdict(lambda: float('inf')) - - for _, msg in msgs: - if msg.which() not in ["roadEncodeIdx", "wideRoadEncodeIdx", "driverEncodeIdx"]: - continue - - encode_index = getattr(msg, msg.which()) - meta = meta_from_encode_index(msg.which()) - - assert encode_index.segmentId < 1200, f"Encoder index segmentId greater that 1200: {msg.which()} {encode_index.segmentId}" - frame_to_encode_id[meta.camera_state][encode_index.frameId] = encode_index.segmentId - - for index, msg in msgs: - if msg.which() not in ["roadCameraState", "wideRoadCameraState", "driverCameraState"]: - continue - - camera_state = getattr(msg, msg.which()) - min_frame_id[msg.which()] = min(min_frame_id[msg.which()], camera_state.frameId) - - encode_id = frame_to_encode_id[msg.which()].get(camera_state.frameId) - if encode_id is None: - print(f"Missing encoded frame for camera feed {msg.which()} with frameId: {camera_state.frameId}") - if len(frame_to_encode_id[msg.which()]) != 0: - del_ops.append(index) - continue - - # fallback mechanism for logs without encodeIdx (e.g. logs from before 2022 with dcamera recording disabled) - # try to fake encode_id by subtracting lowest frameId - encode_id = camera_state.frameId - min_frame_id[msg.which()] - print(f"Faking encodeId to {encode_id} for camera feed {msg.which()} with frameId: {camera_state.frameId}") - - new_msg = messaging.new_message(msg.which()) - new_camera_state = getattr(new_msg, new_msg.which()) - new_camera_state.sensor = camera_state.sensor - new_camera_state.frameId = encode_id - new_camera_state.encodeId = encode_id - # timestampSof was added later so it might be missing on some old segments - if camera_state.timestampSof == 0 and camera_state.timestampEof > 25000000: - new_camera_state.timestampSof = camera_state.timestampEof - 18000000 - else: - new_camera_state.timestampSof = camera_state.timestampSof - new_camera_state.timestampEof = camera_state.timestampEof - new_msg.logMonoTime = msg.logMonoTime - new_msg.valid = msg.valid - - del_ops.append(index) - add_ops.append(new_msg.as_reader()) - return [], add_ops, del_ops - - -@migration(inputs=["carParams"]) -def migrate_carParams(msgs): - ops = [] - for index, msg in msgs: - CP = msg.as_builder() - CP.carParams.carFingerprint = MIGRATION.get(CP.carParams.carFingerprint, CP.carParams.carFingerprint) - for car_fw in CP.carParams.carFw: - car_fw.brand = CP.carParams.brand - ops.append((index, CP.as_reader())) - return ops, [], [] - - -@migration(inputs=["sensorEventsDEPRECATED"], product="sensorEvents") -def migrate_sensorEvents(msgs): - add_ops, del_ops = [], [] - for index, msg in msgs: - # migrate to split sensor events - for evt in msg.sensorEventsDEPRECATED: - # build new message for each sensor type - sensor_service = '' - if evt.which() == 'acceleration': - sensor_service = 'accelerometer' - elif evt.which() == 'gyro' or evt.which() == 'gyroUncalibrated': - sensor_service = 'gyroscope' - elif evt.which() == 'light' or evt.which() == 'proximity': - sensor_service = 'lightSensor' - elif evt.which() == 'magnetic' or evt.which() == 'magneticUncalibrated': - sensor_service = 'magnetometer' - elif evt.which() == 'temperature': - sensor_service = 'temperatureSensor' - - m = messaging.new_message(sensor_service) - m.valid = True - m.logMonoTime = msg.logMonoTime - - m_dat = getattr(m, sensor_service) - m_dat.version = evt.version - m_dat.sensor = evt.sensor - m_dat.type = evt.type - m_dat.source = evt.source - m_dat.timestamp = evt.timestamp - setattr(m_dat, evt.which(), getattr(evt, evt.which())) - - add_ops.append(m.as_reader()) - del_ops.append(index) - return [], add_ops, del_ops - - -@migration(inputs=["onroadEventsDEPRECATED"], product="onroadEvents") -def migrate_onroadEvents(msgs): - ops = [] - for index, msg in msgs: - onroadEvents = [] - for event in msg.onroadEventsDEPRECATED: - try: - if not str(event.name).endswith('DEPRECATED'): - # dict converts name enum into string representation - onroadEvents.append(log.OnroadEvent(**event.to_dict())) - except RuntimeError: # Member was null - traceback.print_exc() - - new_msg = messaging.new_message('onroadEvents', len(msg.onroadEventsDEPRECATED)) - new_msg.valid = msg.valid - new_msg.logMonoTime = msg.logMonoTime - new_msg.onroadEvents = onroadEvents - ops.append((index, new_msg.as_reader())) - - return ops, [], [] - - -@migration(inputs=["driverMonitoringState"]) -def migrate_driverMonitoringState(msgs): - ops = [] - for index, msg in msgs: - msg = msg.as_builder() - events = [] - for event in msg.driverMonitoringState.eventsDEPRECATED: - try: - if not str(event.name).endswith('DEPRECATED'): - # dict converts name enum into string representation - events.append(log.OnroadEvent(**event.to_dict())) - except RuntimeError: # Member was null - traceback.print_exc() - - msg.driverMonitoringState.events = events - ops.append((index, msg.as_reader())) - - return ops, [], [] diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 9ba599bac9cc4b..ccd89bea9a0f06 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -1,306 +1,211 @@ #!/usr/bin/env python3 import os -import pickle import sys +import time from collections import defaultdict from typing import Any -import tempfile from itertools import zip_longest -import matplotlib.pyplot as plt -import numpy as np -from tabulate import tabulate - -from openpilot.common.git import get_commit -from openpilot.system.hardware import PC -from openpilot.tools.lib.openpilotci import get_url -from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff -from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process -from openpilot.tools.lib.framereader import FrameReader -from openpilot.tools.lib.logreader import LogReader, save_log -from openpilot.tools.lib.github_utils import GithubUtils - -TEST_ROUTE = "8494c69d3c710e81|000001d4--2648a9a404" -SEGMENT = 4 -START_FRAME = 0 -END_FRAME = 60 - -SEND_EXTRA_INPUTS = bool(int(os.getenv("SEND_EXTRA_INPUTS", "0"))) - -DATA_TOKEN = os.getenv("CI_ARTIFACTS_TOKEN","") -API_TOKEN = os.getenv("GITHUB_COMMENTS_TOKEN","") -MODEL_REPLAY_BUCKET="model_replay_master" -GITHUB = GithubUtils(API_TOKEN, DATA_TOKEN) - -EXEC_TIMINGS = [ - # model, instant max, average max - ("modelV2", 0.035, 0.025), - ("driverStateV2", 0.02, 0.015), -] - -def get_log_fn(test_route, ref="master"): - return f"{test_route}_model_tici_{ref}.zst" - -def plot(proposed, master, title, tmp): - proposed = list(proposed) - master = list(master) - fig, ax = plt.subplots() - ax.plot(master, label='MASTER') - ax.plot(proposed, label='PROPOSED') - plt.legend(loc='best') - plt.title(title) - plt.savefig(f'{tmp}/{title}.png') - return (title + '.png', proposed == master) - -def get_event(logs, event): - return (getattr(m, m.which()) for m in filter(lambda m: m.which() == event, logs)) - -def zl(array, fill): - return zip_longest(array, [], fillvalue=fill) - -def get_idx_if_non_empty(l, idx=None): - return l if idx is None else (l[idx] if len(l) > 0 else None) - -def generate_report(proposed, master, tmp, commit): - ModelV2_Plots = zl([ - (lambda x: get_idx_if_non_empty(x.velocity.x, 0), "velocity.x"), - (lambda x: get_idx_if_non_empty(x.action.desiredCurvature), "desiredCurvature"), - (lambda x: get_idx_if_non_empty(x.action.desiredAcceleration), "desiredAcceleration"), - (lambda x: get_idx_if_non_empty(x.leadsV3[0].x, 0), "leadsV3.x"), - (lambda x: get_idx_if_non_empty(x.laneLines[1].y, 0), "laneLines.y"), - (lambda x: get_idx_if_non_empty(x.meta.desireState, 3), "desireState.laneChangeLeft"), - (lambda x: get_idx_if_non_empty(x.meta.desireState, 4), "desireState.laneChangeRight"), - (lambda x: get_idx_if_non_empty(x.meta.disengagePredictions.gasPressProbs, 1), "gasPressProbs") - ], "modelV2") - DriverStateV2_Plots = zl([ - (lambda x: get_idx_if_non_empty(x.wheelOnRightProb), "wheelOnRightProb"), - (lambda x: get_idx_if_non_empty(x.leftDriverData.faceProb), "leftDriverData.faceProb"), - (lambda x: get_idx_if_non_empty(x.leftDriverData.faceOrientation, 0), "leftDriverData.faceOrientation0"), - (lambda x: get_idx_if_non_empty(x.leftDriverData.leftBlinkProb), "leftDriverData.leftBlinkProb"), - (lambda x: get_idx_if_non_empty(x.leftDriverData.phoneProb), "leftDriverData.phoneProb"), - (lambda x: get_idx_if_non_empty(x.rightDriverData.faceProb), "rightDriverData.faceProb"), - ], "driverStateV2") - - return [plot(map(v[0], get_event(proposed, event)), \ - map(v[0], get_event(master, event)), f"{v[1]}_{commit[:7]}", tmp) \ - for v,event in ([*ModelV2_Plots] + [*DriverStateV2_Plots])] - -def create_table(title, files, link, open_table=False): - if not files: - return "" - table = [f'
    {title}'] - for i,f in enumerate(files): - if not (i % 2): - table.append("") - table.append(f'') - if (i % 2): - table.append("") - table.append("
    ") - table = "".join(table) - return table - -def comment_replay_report(proposed, master, full_logs): - with tempfile.TemporaryDirectory() as tmp: - PR_BRANCH = os.getenv("GIT_BRANCH","") - DATA_BUCKET = f"model_replay_{PR_BRANCH}" +import cereal.messaging as messaging +from cereal.visionipc import VisionIpcServer, VisionStreamType +from common.spinner import Spinner +from common.timeout import Timeout +from common.transformations.camera import tici_f_frame_size, tici_d_frame_size +from system.hardware import PC +from selfdrive.manager.process_config import managed_processes +from selfdrive.test.openpilotci import BASE_URL, get_url +from selfdrive.test.process_replay.compare_logs import compare_logs, save_log +from selfdrive.test.process_replay.test_processes import format_diff +from system.version import get_commit +from tools.lib.framereader import FrameReader +from tools.lib.logreader import LogReader - try: - GITHUB.get_pr_number(PR_BRANCH) - except Exception: - print("No PR associated with this branch. Skipping report.") - return - - commit = get_commit() - files = generate_report(proposed, master, tmp, commit) - - GITHUB.upload_files(DATA_BUCKET, [(x[0], tmp + '/' + x[0]) for x in files]) - - log_name = get_log_fn(TEST_ROUTE, commit) - save_log(log_name, full_logs) - GITHUB.upload_file(DATA_BUCKET, os.path.basename(log_name), log_name) +TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" +SEGMENT = 0 +MAX_FRAMES = 10 if PC else 1300 - diff_files = [x for x in files if not x[1]] - link = GITHUB.get_bucket_link(DATA_BUCKET) - diff_plots = create_table("Model Replay Differences", diff_files, link, open_table=True) - all_plots = create_table("All Model Replay Plots", files, link) - comment = f"ref for commit {commit}: {link}/{log_name}" + diff_plots + all_plots - GITHUB.comment_on_pr(comment, PR_BRANCH, "commaci-public", True) +SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) -def trim_logs(logs, start_frame, end_frame, frs_types, include_all_types): - all_msgs = [] - cam_state_counts = defaultdict(int) - for msg in sorted(logs, key=lambda m: m.logMonoTime): - if msg.which() in frs_types: - cam_state_counts[msg.which()] += 1 - if any(cam_state_counts[state] >= start_frame for state in frs_types): - all_msgs.append(msg) - if all(cam_state_counts[state] == end_frame for state in frs_types): - break +VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, + "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD} +def get_log_fn(ref_commit, test_route): + return f"{test_route}_model_tici_{ref_commit}.bz2" - if len(include_all_types) != 0: - other_msgs = [m for m in logs if m.which() in include_all_types] - all_msgs.extend(other_msgs) - return all_msgs +def replace_calib(msg, calib): + msg = msg.as_builder() + if calib is not None: + msg.liveCalibration.rpyCalib = calib.tolist() + return msg def model_replay(lr, frs): - # modeld is using frame pairs - modeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"roadCameraState", "wideRoadCameraState"}, - {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams", "carState", "carControl", "can"}) - dmodeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"driverCameraState"}, {"driverEncodeIdx", "carParams", "can"}) - - if not SEND_EXTRA_INPUTS: - modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] - dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration'] - - # initial setup - for s in ('liveCalibration', 'deviceState'): - msg = next(msg for msg in lr if msg.which() == s).as_builder() - msg.logMonoTime = lr[0].logMonoTime - modeld_logs.insert(1, msg.as_reader()) - dmodeld_logs.insert(1, msg.as_reader()) - - modeld = get_process_config("modeld") - dmonitoringmodeld = get_process_config("dmonitoringmodeld") - - modeld_msgs = replay_process(modeld, modeld_logs, frs) - dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs) - - msgs = modeld_msgs + dmonitoringmodeld_msgs - - header = ['model', 'max instant', 'max instant allowed', 'average', 'max average allowed', 'test result'] - rows = [] - timings_ok = True - for (s, instant_max, avg_max) in EXEC_TIMINGS: - ts = [getattr(m, s).modelExecutionTime for m in msgs if m.which() == s] - # TODO some init can happen in first iteration - ts = ts[1:] - - errors = [] - if np.max(ts) > instant_max: - errors.append("❌ FAILED MAX TIMING CHECK ❌") - if np.mean(ts) > avg_max: - errors.append("❌ FAILED AVG TIMING CHECK ❌") - - timings_ok = not errors and timings_ok - rows.append([s, np.max(ts), instant_max, np.mean(ts), avg_max, "\n".join(errors) or "✅"]) - - print("------------------------------------------------") - print("----------------- Model Timing -----------------") - print("------------------------------------------------") - print(tabulate(rows, header, tablefmt="simple_grid", stralign="center", numalign="center", floatfmt=".4f")) - assert timings_ok or PC - - return msgs - - -def get_frames(): - regen_cache = "--regen-cache" in sys.argv - frames_cache = '/tmp/model_replay_cache' if PC else '/data/model_replay_cache' - os.makedirs(frames_cache, exist_ok=True) - - cache_name = f'{frames_cache}/{TEST_ROUTE}_{SEGMENT}_{START_FRAME}_{END_FRAME}.pkl' - if os.path.isfile(cache_name) and not regen_cache: - try: - print(f"Loading frames from cache {cache_name}") - return pickle.load(open(cache_name, "rb")) - except Exception as e: - print(f"Failed to load frames from cache {cache_name}: {e}") + if not PC: + spinner = Spinner() + spinner.update("starting model replay") + else: + spinner = None + + vipc_server = VisionIpcServer("camerad") + vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size)) + vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size)) + vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) + vipc_server.start_listener() + + sm = messaging.SubMaster(['modelV2', 'driverStateV2']) + pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan']) + + try: + managed_processes['modeld'].start() + managed_processes['dmonitoringmodeld'].start() + time.sleep(5) + sm.update(1000) + + log_msgs = [] + last_desire = None + recv_cnt = defaultdict(int) + frame_idxs = defaultdict(int) + + # init modeld with valid calibration + cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"] + for _ in range(5): + pm.send(cal_msgs[0].which(), cal_msgs[0].as_builder()) + time.sleep(0.1) + + msgs = defaultdict(list) + for msg in lr: + msgs[msg.which()].append(msg) + + for cam_msgs in zip_longest(msgs['roadCameraState'], msgs['wideRoadCameraState'], msgs['driverCameraState']): + # need a pair of road/wide msgs + if None in (cam_msgs[0], cam_msgs[1]): + break + + for msg in cam_msgs: + if msg is None: + continue + + if SEND_EXTRA_INPUTS: + if msg.which() == "liveCalibration": + last_calib = list(msg.liveCalibration.rpyCalib) + pm.send(msg.which(), replace_calib(msg, last_calib)) + elif msg.which() == "lateralPlan": + last_desire = msg.lateralPlan.desire + dat = messaging.new_message('lateralPlan') + dat.lateralPlan.desire = last_desire + pm.send('lateralPlan', dat) + + if msg.which() in VIPC_STREAM: + msg = msg.as_builder() + camera_state = getattr(msg, msg.which()) + img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="nv12")[0] + frame_idxs[msg.which()] += 1 + + # send camera state and frame + camera_state.frameId = frame_idxs[msg.which()] + pm.send(msg.which(), msg) + vipc_server.send(VIPC_STREAM[msg.which()], img.flatten().tobytes(), camera_state.frameId, + camera_state.timestampSof, camera_state.timestampEof) + + recv = None + if msg.which() in ('roadCameraState', 'wideRoadCameraState'): + if min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: + recv = "modelV2" + elif msg.which() == 'driverCameraState': + recv = "driverStateV2" + + # wait for a response + with Timeout(15, f"timed out waiting for {recv}"): + if recv: + recv_cnt[recv] += 1 + log_msgs.append(messaging.recv_one(sm.sock[recv])) + + if spinner: + spinner.update("replaying models: road %d/%d, driver %d/%d" % (frame_idxs['roadCameraState'], + frs['roadCameraState'].frame_count, frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)) + + + if any(frame_idxs[c] >= frs[c].frame_count for c in frame_idxs.keys()) or frame_idxs['roadCameraState'] == MAX_FRAMES: + break + else: + print(f'Received {frame_idxs["roadCameraState"]} frames') + + finally: + if spinner: + spinner.close() + managed_processes['modeld'].stop() + managed_processes['dmonitoringmodeld'].stop() + + + return log_msgs - frs = { - 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "fcamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME), - 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "dcamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME), - 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "ecamera.hevc"), pix_fmt='nv12', cache_size=END_FRAME - START_FRAME), - } - for fr in frs.values(): - for fidx in range(START_FRAME, END_FRAME): - fr.get(fidx) - fr.it = None - print(f"Dumping frame cache {cache_name}") - pickle.dump(frs, open(cache_name, "wb")) - return frs if __name__ == "__main__": - update = "--update" in sys.argv or (os.getenv("GIT_BRANCH", "") == 'master') + + update = "--update" in sys.argv replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit") # load logs - lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT, "rlog.zst"))) - frs = get_frames() + lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) + frs = { + 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera")), + 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera")), + 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera")) + } - log_msgs = [] - # run replays - log_msgs += model_replay(lr, frs) + # run replay + log_msgs = model_replay(lr, frs) # get diff failed = False if not update: - log_fn = get_log_fn(TEST_ROUTE) + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + log_fn = get_log_fn(ref_commit, TEST_ROUTE) try: - all_logs = list(LogReader(GITHUB.get_file_url(MODEL_REPLAY_BUCKET, log_fn))) - cmp_log = [] - model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "drivingModelData", "cameraOdometry")) - cmp_log += all_logs[model_start_index+START_FRAME*3:model_start_index + END_FRAME*3] - dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2") - cmp_log += all_logs[dmon_start_index+START_FRAME:dmon_start_index + END_FRAME] + cmp_log = list(LogReader(BASE_URL + log_fn))[:2*MAX_FRAMES] ignore = [ 'logMonoTime', - 'drivingModelData.frameDropPerc', - 'drivingModelData.modelExecutionTime', 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime', 'driverStateV2.modelExecutionTime', - 'driverStateV2.gpuExecutionTime' + 'driverStateV2.dspExecutionTime' ] - if PC: - # TODO We ignore whole bunch so we can compare important stuff - # like posenet with reasonable tolerance - ignore += ['modelV2.acceleration.x', - 'modelV2.position.x', - 'modelV2.position.xStd', - 'modelV2.position.y', - 'modelV2.position.yStd', - 'modelV2.position.z', - 'modelV2.position.zStd', - 'drivingModelData.path.xCoefficients',] - for i in range(3): - for field in ('x', 'y', 'v', 'a'): - ignore.append(f'modelV2.leadsV3.{i}.{field}') - ignore.append(f'modelV2.leadsV3.{i}.{field}Std') - for i in range(4): - for field in ('x', 'y', 'z', 't'): - ignore.append(f'modelV2.laneLines.{i}.{field}') - for i in range(2): - for field in ('x', 'y', 'z', 't'): - ignore.append(f'modelV2.roadEdges.{i}.{field}') - tolerance = .3 if PC else None + # TODO this tolerance is absurdly large + tolerance = 5e-1 if PC else None results: Any = {TEST_ROUTE: {}} - log_paths: Any = {TEST_ROUTE: {"models": {'ref': log_fn, 'new': log_fn}}} + log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}} results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) - diff_short, diff_long, failed = format_diff(results, log_paths, 'master') + diff1, diff2, failed = format_diff(results, log_paths, ref_commit) - if "CI" in os.environ: - comment_replay_report(log_msgs, cmp_log, log_msgs) - failed = False - print(diff_long) + print(diff2) print('-------------\n'*5) - print(diff_short) + print(diff1) with open("model_diff.txt", "w") as f: - f.write(diff_long) + f.write(diff2) except Exception as e: print(str(e)) failed = True # upload new refs - if update and not PC: + if (update or failed) and not PC: + from selfdrive.test.openpilotci import upload_file + print("Uploading new refs") - log_fn = get_log_fn(TEST_ROUTE) + + new_commit = get_commit() + log_fn = get_log_fn(new_commit, TEST_ROUTE) save_log(log_fn, log_msgs) try: - GITHUB.upload_file(MODEL_REPLAY_BUCKET, os.path.basename(log_fn), log_fn) + upload_file(log_fn, os.path.basename(log_fn)) except Exception as e: print("failed to upload", e) + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\n\nNew ref commit: ", new_commit) + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit new file mode 100644 index 00000000000000..958d3da14df35b --- /dev/null +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -0,0 +1 @@ +c40319a454840d8a2196ec1227d27b536ee14375 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8af72e5f4e7c94..b4e3f62656a607 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -1,803 +1,583 @@ #!/usr/bin/env python3 +import importlib import os +import sys +import threading import time -import copy -import heapq import signal -from collections import Counter -from dataclasses import dataclass, field -from itertools import islice -from typing import Any -from collections.abc import Callable, Iterable -from tqdm import tqdm +from collections import namedtuple + import capnp -from openpilot.system.hardware.hw import Paths import cereal.messaging as messaging -from cereal import car -from cereal.services import SERVICE_LIST -from msgq.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name -from opendbc.car.can_definitions import CanData -from opendbc.car.car_helpers import get_car, interfaces -from openpilot.common.params import Params -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.common.timeout import Timeout -from openpilot.common.realtime import DT_CTRL -from openpilot.system.manager.process_config import managed_processes -from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams -from openpilot.selfdrive.test.process_replay.migration import migrate_all -from openpilot.selfdrive.test.process_replay.capture import ProcessOutputCapture -from openpilot.tools.lib.logreader import LogIterable -from openpilot.tools.lib.framereader import FrameReader +from cereal import car, log +from cereal.services import service_list +from common.params import Params +from common.timeout import Timeout +from common.realtime import DT_CTRL +from panda.python import ALTERNATIVE_EXPERIENCE +from selfdrive.car.car_helpers import get_car, interfaces +from selfdrive.test.process_replay.helpers import OpenpilotPrefix +from selfdrive.manager.process import PythonProcess +from selfdrive.manager.process_config import managed_processes # Numpy gives different results based on CPU features after version 19 -NUMPY_TOLERANCE = 1e-2 +NUMPY_TOLERANCE = 1e-7 +CI = "CI" in os.environ +TIMEOUT = 15 PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__)) FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/") - -class LauncherWithCapture: - def __init__(self, capture: ProcessOutputCapture, launcher: Callable): - self.capture = capture - self.launcher = launcher - - def __call__(self, *args, **kwargs): - self.capture.link_with_current_proc() - self.launcher(*args, **kwargs) +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config', 'environ', 'subtest_name', "field_tolerances"], defaults=({}, {}, "", {})) -class ReplayContext: - def __init__(self, cfg): - self.proc_name = cfg.proc_name - self.pubs = cfg.pubs - self.main_pub = cfg.main_pub - self.main_pub_drained = cfg.main_pub_drained - assert len(self.pubs) != 0 or self.main_pub is not None - - def __enter__(self): - self.open_context() - - return self +def wait_for_event(evt): + if not evt.wait(TIMEOUT): + if threading.currentThread().getName() == "MainThread": + # tested process likely died. don't let test just hang + raise Exception(f"Timeout reached. Tested process {os.environ['PROC_NAME']} likely crashed.") + else: + # done testing this process, let it die + sys.exit(0) + + +class FakeSocket: + def __init__(self, wait=True): + self.data = [] + self.wait = wait + self.recv_called = threading.Event() + self.recv_ready = threading.Event() + + def receive(self, non_blocking=False): + if non_blocking: + return None + + if self.wait: + self.recv_called.set() + wait_for_event(self.recv_ready) + self.recv_ready.clear() + return self.data.pop() + + def send(self, data): + if self.wait: + wait_for_event(self.recv_called) + self.recv_called.clear() + + self.data.append(data) + + if self.wait: + self.recv_ready.set() + + def wait_for_recv(self): + wait_for_event(self.recv_called) + + +class DumbSocket: + def __init__(self, s=None): + if s is not None: + try: + dat = messaging.new_message(s) + except capnp.lib.capnp.KjException: # pylint: disable=c-extension-no-member + # lists + dat = messaging.new_message(s, 0) + + self.data = dat.to_bytes() + + def receive(self, non_blocking=False): + return self.data + + def send(self, dat): + pass + + +class FakeSubMaster(messaging.SubMaster): + def __init__(self, services, ignore_alive=None, ignore_avg_freq=None): + super().__init__(services, ignore_alive=ignore_alive, ignore_avg_freq=ignore_avg_freq, addr=None) + self.sock = {s: DumbSocket(s) for s in services} + self.update_called = threading.Event() + self.update_ready = threading.Event() + self.wait_on_getitem = False + + def __getitem__(self, s): + # hack to know when fingerprinting is done + if self.wait_on_getitem: + self.update_called.set() + wait_for_event(self.update_ready) + self.update_ready.clear() + return self.data[s] + + def update(self, timeout=-1): + self.update_called.set() + wait_for_event(self.update_ready) + self.update_ready.clear() + + def update_msgs(self, cur_time, msgs): + wait_for_event(self.update_called) + self.update_called.clear() + super().update_msgs(cur_time, msgs) + self.update_ready.set() + + def wait_for_update(self): + wait_for_event(self.update_called) + + +class FakePubMaster(messaging.PubMaster): + def __init__(self, services): # pylint: disable=super-init-not-called + self.data = {} + self.sock = {} + self.last_updated = None + for s in services: + try: + data = messaging.new_message(s) + except capnp.lib.capnp.KjException: + data = messaging.new_message(s, 0) + self.data[s] = data.as_reader() + self.sock[s] = DumbSocket() + self.send_called = threading.Event() + self.get_called = threading.Event() + + def send(self, s, dat): + self.last_updated = s + if isinstance(dat, bytes): + self.data[s] = log.Event.from_bytes(dat) + else: + self.data[s] = dat.as_reader() + self.send_called.set() + wait_for_event(self.get_called) + self.get_called.clear() - def __exit__(self, exc_type, exc_obj, exc_tb): - self.close_context() + def wait_for_msg(self): + wait_for_event(self.send_called) + self.send_called.clear() + dat = self.data[self.last_updated] + self.get_called.set() + return dat - def open_context(self): - messaging.toggle_fake_events(True) - messaging.set_fake_prefix(self.proc_name) - if self.main_pub is None: - self.events = {} - for pub in self.pubs: - self.events[pub] = messaging.fake_event_handle(pub, enable=True) - else: - self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)} - - def close_context(self): - del self.events - - messaging.toggle_fake_events(False) - messaging.delete_fake_prefix() - - @property - def all_recv_called_events(self): - return [man.recv_called_event for man in self.events.values()] - - @property - def all_recv_ready_events(self): - return [man.recv_ready_event for man in self.events.values()] - - def send_sync(self, pm, endpoint, dat): - self.events[endpoint].recv_called_event.wait() - self.events[endpoint].recv_called_event.clear() - pm.send(endpoint, dat) - self.events[endpoint].recv_ready_event.set() - - def unlock_sockets(self): - expected_sets = len(self.events) - while expected_sets > 0: - index = messaging.wait_for_one_event(self.all_recv_called_events) - self.all_recv_called_events[index].clear() - self.all_recv_ready_events[index].set() - expected_sets -= 1 - - def wait_for_recv_called(self): - messaging.wait_for_one_event(self.all_recv_called_events) - - def wait_for_next_recv(self, trigger_empty_recv): - index = messaging.wait_for_one_event(self.all_recv_called_events) - if self.main_pub is not None and self.main_pub_drained and trigger_empty_recv: - self.all_recv_called_events[index].clear() - self.all_recv_ready_events[index].set() - self.all_recv_called_events[index].wait() - - -@dataclass -class ProcessConfig: - proc_name: str - pubs: list[str] - subs: list[str] - ignore: list[str] - config_callback: Callable | None = None - init_callback: Callable | None = None - should_recv_callback: Callable | None = None - tolerance: float | None = None - processing_time: float = 0.001 - timeout: int = 30 - simulation: bool = True - # Set to service process receives on first - main_pub: str | None = None - main_pub_drained: bool = False - vision_pubs: list[str] = field(default_factory=list) - ignore_alive_pubs: list[str] = field(default_factory=list) - - def __post_init__(self): - # If the process is polling a service, we can just lock that one to speed up replay - if self.main_pub is None and isinstance(self.should_recv_callback, MessageBasedRcvCallback): - self.main_pub = self.should_recv_callback.trigger_msg_type - - -class ProcessContainer: - def __init__(self, cfg: ProcessConfig): - self.prefix = OpenpilotPrefix(create_dirs_on_enter=False, clean_dirs_on_exit=False) - self.cfg = copy.deepcopy(cfg) - self.process = copy.deepcopy(managed_processes[cfg.proc_name]) - self.msg_queue: list[capnp._DynamicStructReader] = [] - self.cnt = 0 - self.pm: messaging.PubMaster | None = None - self.sockets: list[messaging.SubSocket] | None = None - self.rc: ReplayContext | None = None - self.vipc_server: VisionIpcServer | None = None - self.environ_config: dict[str, Any] | None = None - self.capture: ProcessOutputCapture | None = None - - @property - def has_empty_queue(self) -> bool: - return len(self.msg_queue) == 0 - - @property - def pubs(self) -> list[str]: - return self.cfg.pubs - - @property - def subs(self) -> list[str]: - return self.cfg.subs - - def _clean_env(self): - for k in self.environ_config.keys(): - if k in os.environ: - del os.environ[k] - - for k in ["PROC_NAME", "SIMULATION"]: - if k in os.environ: - del os.environ[k] - - def _setup_env(self, params_config: dict[str, Any], environ_config: dict[str, Any]): - for k, v in environ_config.items(): - if len(v) != 0: - os.environ[k] = v - elif k in os.environ: - del os.environ[k] - - os.environ["PROC_NAME"] = self.cfg.proc_name - if self.cfg.simulation: - os.environ["SIMULATION"] = "1" - elif "SIMULATION" in os.environ: - del os.environ["SIMULATION"] - - params = Params() - for k, v in params_config.items(): - if isinstance(v, bool): - params.put_bool(k, v) - else: - params.put(k, v) - - self.environ_config = environ_config - - def _setup_vision_ipc(self, all_msgs: LogIterable, frs: dict[str, Any]): - assert len(self.cfg.vision_pubs) != 0 - - vipc_server = VisionIpcServer("camerad") - streams_metas = available_streams(all_msgs) - for meta in streams_metas: - if meta.camera_state in self.cfg.vision_pubs: - assert frs[meta.camera_state].pix_fmt == 'nv12' - frame_size = (frs[meta.camera_state].w, frs[meta.camera_state].h) - vipc_server.create_buffers(meta.stream, 2, *frame_size) - vipc_server.start_listener() - - self.vipc_server = vipc_server - self.cfg.vision_pubs = [meta.camera_state for meta in streams_metas if meta.camera_state in self.cfg.vision_pubs] - - def _start_process(self): - if self.capture is not None: - self.process.launcher = LauncherWithCapture(self.capture, self.process.launcher) - self.process.prepare() - self.process.start() - - def start( - self, params_config: dict[str, Any], environ_config: dict[str, Any], - all_msgs: LogIterable, frs: dict[str, FrameReader] | None, - fingerprint: str | None, capture_output: bool - ): - with self.prefix as p: - self.prefix.create_dirs() - self._setup_env(params_config, environ_config) - - if self.cfg.config_callback is not None: - params = Params() - self.cfg.config_callback(params, self.cfg, all_msgs) - - self.rc = ReplayContext(self.cfg) - self.rc.open_context() - - self.pm = messaging.PubMaster(self.cfg.pubs) - self.sockets = [messaging.sub_sock(s, timeout=100) for s in self.cfg.subs] - - if len(self.cfg.vision_pubs) != 0: - assert frs is not None - self._setup_vision_ipc(all_msgs, frs) - assert self.vipc_server is not None - - if capture_output: - self.capture = ProcessOutputCapture(self.cfg.proc_name, p.prefix) - - self._start_process() - - if self.cfg.init_callback is not None: - self.cfg.init_callback(self.rc, self.pm, all_msgs, fingerprint) - - def stop(self): - with self.prefix: - self.process.signal(signal.SIGKILL) - self.process.stop() - self.rc.close_context() - self.prefix.clean_dirs() - self._clean_env() - - def get_output_msgs(self, start_time: int): - assert self.rc and self.sockets - - output_msgs = [] - self.rc.wait_for_recv_called() - for socket in self.sockets: - ms = messaging.drain_sock(socket) - for m in ms: - m = m.as_builder() - m.logMonoTime = start_time + int(self.cfg.processing_time * 1e9) - output_msgs.append(m.as_reader()) - return output_msgs - - def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, FrameReader] | None) -> list[capnp._DynamicStructReader]: - assert self.rc and self.pm and self.sockets and self.process.proc - - output_msgs = [] - end_of_cycle = True - if self.cfg.should_recv_callback is not None: - end_of_cycle = self.cfg.should_recv_callback(msg, self.cfg, self.cnt) - - self.msg_queue.append(msg) - if end_of_cycle: - with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"): - # call recv to let sub-sockets reconnect, after we know the process is ready - if self.cnt == 0: - for s in self.sockets: - messaging.recv_one_or_none(s) - - # certain processes use drain_sock. need to cause empty recv to break from this loop - trigger_empty_recv = False - if self.cfg.main_pub and self.cfg.main_pub_drained: - trigger_empty_recv = any(m.which() == self.cfg.main_pub for m in self.msg_queue) - - # get output msgs from previous inputs - output_msgs = self.get_output_msgs(msg.logMonoTime) - - for m in self.msg_queue: - self.pm.send(m.which(), m.as_builder()) - # send frames if needed - if self.vipc_server is not None and m.which() in self.cfg.vision_pubs: - camera_state = getattr(m, m.which()) - camera_meta = meta_from_camera_state(m.which()) - assert frs is not None - img = frs[m.which()].get(camera_state.frameId) - self.vipc_server.send(camera_meta.stream, img.flatten().tobytes(), - camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) - self.msg_queue = [] - - self.rc.unlock_sockets() - if trigger_empty_recv: - self.rc.unlock_sockets() - self.cnt += 1 - assert self.process.proc.is_alive() - - return output_msgs - - -def card_fingerprint_callback(rc, pm, msgs, fingerprint): +def fingerprint(msgs, fsm, can_sock, fingerprint): print("start fingerprinting") - params = Params() - canmsgs = list(islice((m for m in msgs if m.which() == "can"), 300)) + fsm.wait_on_getitem = True - # card expects one arbitrary can and pandaState - rc.send_sync(pm, "can", messaging.new_message("can", 1)) - pm.send("pandaStates", messaging.new_message("pandaStates", 1)) - rc.send_sync(pm, "can", messaging.new_message("can", 1)) - rc.wait_for_next_recv(True) + # populate fake socket with data for fingerprinting + canmsgs = [msg for msg in msgs if msg.which() == "can"] + wait_for_event(can_sock.recv_called) + can_sock.recv_called.clear() + can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]] + can_sock.recv_ready.set() + can_sock.wait = False - # fingerprinting is done, when CarParams is set - while params.get("CarParams") is None: - if len(canmsgs) == 0: - raise ValueError("Fingerprinting failed. Run out of can msgs") + # we know fingerprinting is done when controlsd sets sm['lateralPlan'].sensorValid + wait_for_event(fsm.update_called) + fsm.update_called.clear() - m = canmsgs.pop(0) - rc.send_sync(pm, "can", m.as_builder().to_bytes()) - rc.wait_for_next_recv(True) + fsm.wait_on_getitem = False + can_sock.wait = True + can_sock.data = [] + fsm.update_ready.set() -def get_car_params_callback(rc, pm, msgs, fingerprint): - params = Params() + +def get_car_params(msgs, fsm, can_sock, fingerprint): if fingerprint: - CarInterface = interfaces[fingerprint] - CP = CarInterface.get_non_essential_params(fingerprint) + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_params(fingerprint) else: - can_msgs = ([CanData(can.address, can.dat, can.src) for can in m.can] for m in msgs if m.which() == "can") - cached_params_raw = params.get("CarParamsCache") - assert next(can_msgs, None), "CAN messages are required for fingerprinting" - assert os.environ.get("SKIP_FW_QUERY", False) or cached_params_raw is not None, \ - "CarParamsCache is required for fingerprinting. Make sure to keep carParams msgs in the logs." - - def can_recv(wait_for_one: bool = False) -> list[list[CanData]]: - return [next(can_msgs, [])] - - cached_params = None - if cached_params_raw is not None: - with car.CarParams.from_bytes(cached_params_raw) as _cached_params: - cached_params = _cached_params + can = FakeSocket(wait=False) + sendcan = FakeSocket(wait=False) - CP = get_car(can_recv, lambda _msgs: None, lambda obd: None, params.get_bool("AlphaLongitudinalEnabled"), False, cached_params=cached_params).CP + canmsgs = [msg for msg in msgs if msg.which() == 'can'] + for m in canmsgs[:300]: + can.send(m.as_builder().to_bytes()) + _, CP = get_car(can, sendcan) + Params().put("CarParams", CP.to_bytes()) - params.put("CarParams", CP.to_bytes()) - - -def card_rcv_callback(msg, cfg, frame): - # no sendcan until card is initialized - if msg.which() != "can": - return False - socks = [ - s for s in cfg.subs if - frame % int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency) == 0 - ] - if "sendcan" in socks and (frame - 1) < 2000: +def controlsd_rcv_callback(msg, CP, cfg, fsm): + # no sendcan until controlsd is initialized + socks = [s for s in cfg.pub_sub[msg.which()] if + (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0] + if "sendcan" in socks and fsm.frame < 2000: socks.remove("sendcan") - return len(socks) > 0 - - -class ModeldCameraSyncRcvCallback: - def __init__(self): - self.road_present = False - self.wide_road_present = False - self.is_dual_camera = True - - def __call__(self, msg, cfg, frame): - self.is_dual_camera = len(cfg.vision_pubs) == 2 - if msg.which() == "roadCameraState": - self.road_present = True - elif msg.which() == "wideRoadCameraState": - self.wide_road_present = True - - if self.road_present and self.wide_road_present: - self.road_present, self.wide_road_present = False, False - return True - elif self.road_present and not self.is_dual_camera: - self.road_present = False - return True - else: - return False - - -class MessageBasedRcvCallback: - def __init__(self, trigger_msg_type: str, first_frame: bool = False): - self.trigger_msg_type = trigger_msg_type - self.first_frame = first_frame + return socks, len(socks) > 0 - def __call__(self, msg, cfg, frame): - # publish on first frame or trigger msg - return ((frame - 1) == 0 and self.first_frame) or msg.which() == self.trigger_msg_type +def radar_rcv_callback(msg, CP, cfg, fsm): + if msg.which() != "can": + return [], False + elif CP.radarOffCan: + return ["radarState", "liveTracks"], True + + radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474], + "chrysler": [0x2d4]}.get(CP.carName, None) + + if radar_msgs is None: + raise NotImplementedError + + for m in msg.can: + if m.src == 1 and m.address in radar_msgs: + return ["radarState", "liveTracks"], True + return [], False + + +def calibration_rcv_callback(msg, CP, cfg, fsm): + # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. + # should_recv always true to increment frame + recv_socks = [] + frame = fsm.frame + 1 # incrementing hasn't happened yet in SubMaster + if frame == 0 or (msg.which() == 'cameraOdometry' and (frame % 5) == 0): + recv_socks = ["liveCalibration"] + return recv_socks, fsm.frame == 0 or msg.which() == 'cameraOdometry' + + +def ublox_rcv_callback(msg): + msg_class, msg_id = msg.ubloxRaw[2:4] + if (msg_class, msg_id) in {(1, 7 * 16)}: + return ["gpsLocationExternal"] + elif (msg_class, msg_id) in {(2, 1 * 16 + 5), (10, 9)}: + return ["ubloxGnss"] + else: + return [] -def selfdrived_config_callback(params, cfg, lr): - ublox = params.get_bool("UbloxAvailable") - sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) - cfg.pubs = set(cfg.pubs) - sub_keys +def laika_rcv_callback(msg, CP, cfg, fsm): + if msg.which() == 'ubloxGnss' and msg.ubloxGnss.which() == "measurementReport": + return ["gnssMeasurements"], True + else: + return [], True CONFIGS = [ - ProcessConfig( - proc_name="selfdrived", - pubs=[ - "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", - "longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2", - "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters", - "accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState", - "carControl", "driverAssistance", "alertDebug", "audioFeedback", - ], - subs=["selfdriveState", "onroadEvents"], - ignore=["logMonoTime"], - config_callback=selfdrived_config_callback, - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("carState", True), - tolerance=NUMPY_TOLERANCE, - processing_time=0.004, - ), ProcessConfig( proc_name="controlsd", - pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState", - "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput", - "driverMonitoringState", "onroadEvents", "driverAssistance"], - subs=["carControl", "controlsState"], - ignore=["logMonoTime", ], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("selfdriveState"), + pub_sub={ + "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], + "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], + "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [], + }, + ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], + init_callback=fingerprint, + should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, - ), - ProcessConfig( - proc_name="card", - pubs=["pandaStates", "carControl", "onroadEvents", "can"], - subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"], - ignore=["logMonoTime", "carState.cumLagMs"], - init_callback=card_fingerprint_callback, - should_recv_callback=card_rcv_callback, - tolerance=NUMPY_TOLERANCE, - processing_time=0.004, - main_pub="can", - main_pub_drained=True, + fake_pubsubmaster=True, + submaster_config={ + 'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz + 'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen + } ), ProcessConfig( proc_name="radard", - pubs=["liveTracks", "carState", "modelV2"], - subs=["radarState"], - ignore=["logMonoTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("modelV2"), + pub_sub={ + "can": ["radarState", "liveTracks"], + "liveParameters": [], "carState": [], "modelV2": [], + }, + ignore=["logMonoTime", "valid", "radarState.cumLagMs"], + init_callback=get_car_params, + should_recv_callback=radar_rcv_callback, + tolerance=None, + fake_pubsubmaster=True, ), ProcessConfig( proc_name="plannerd", - pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], - subs=["longitudinalPlan", "driverAssistance"], - ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("modelV2"), + pub_sub={ + "modelV2": ["lateralPlan", "longitudinalPlan"], + "carControl": [], "carState": [], "controlsState": [], "radarState": [], + }, + ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], + init_callback=get_car_params, + should_recv_callback=None, tolerance=NUMPY_TOLERANCE, + fake_pubsubmaster=True, ), ProcessConfig( proc_name="calibrationd", - pubs=["carState", "cameraOdometry"], - subs=["liveCalibration"], - ignore=["logMonoTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("cameraOdometry", True), + pub_sub={ + "carState": ["liveCalibration"], + "cameraOdometry": [], + "carParams": [], + }, + ignore=["logMonoTime", "valid"], + init_callback=get_car_params, + should_recv_callback=calibration_rcv_callback, + tolerance=None, + fake_pubsubmaster=True, ), ProcessConfig( proc_name="dmonitoringd", - pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"], - subs=["driverMonitoringState"], - ignore=["logMonoTime"], - should_recv_callback=MessageBasedRcvCallback("driverStateV2"), + pub_sub={ + "driverStateV2": ["driverMonitoringState"], + "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], + }, + ignore=["logMonoTime", "valid"], + init_callback=get_car_params, + should_recv_callback=None, tolerance=NUMPY_TOLERANCE, + fake_pubsubmaster=True, ), ProcessConfig( proc_name="locationd", - pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState" - ], - subs=["livePose"], - ignore=["logMonoTime"], - should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), + pub_sub={ + "cameraOdometry": ["liveLocationKalman"], + "sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [], + }, + ignore=["logMonoTime", "valid"], + init_callback=get_car_params, + should_recv_callback=None, tolerance=NUMPY_TOLERANCE, + fake_pubsubmaster=False, ), ProcessConfig( proc_name="paramsd", - pubs=["livePose", "liveCalibration", "carState"], - subs=["liveParameters"], - ignore=["logMonoTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose"), - tolerance=NUMPY_TOLERANCE, - processing_time=0.004, - ), - ProcessConfig( - proc_name="lagd", - pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"], - subs=["liveDelay"], - ignore=["logMonoTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose"), + pub_sub={ + "liveLocationKalman": ["liveParameters"], + "carState": [] + }, + ignore=["logMonoTime", "valid"], + init_callback=get_car_params, + should_recv_callback=None, tolerance=NUMPY_TOLERANCE, + fake_pubsubmaster=True, ), ProcessConfig( proc_name="ubloxd", - pubs=["ubloxRaw"], - subs=["ubloxGnss", "gpsLocationExternal"], + pub_sub={ + "ubloxRaw": ["ubloxGnss", "gpsLocationExternal"], + }, ignore=["logMonoTime"], + init_callback=None, + should_recv_callback=ublox_rcv_callback, + tolerance=None, + fake_pubsubmaster=False, ), ProcessConfig( - proc_name="torqued", - pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"], - subs=["liveTorqueParameters"], + proc_name="laikad", + subtest_name="Offline", + pub_sub={ + "ubloxGnss": ["gnssMeasurements"], + "clocks": [] + }, ignore=["logMonoTime"], - init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("livePose", True), + init_callback=get_car_params, + should_recv_callback=laika_rcv_callback, tolerance=NUMPY_TOLERANCE, + fake_pubsubmaster=True, + environ={"LAIKAD_NO_INTERNET": "1"}, ), ProcessConfig( - proc_name="modeld", - pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"], - subs=["modelV2", "drivingModelData", "cameraOdometry"], - ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"], - should_recv_callback=ModeldCameraSyncRcvCallback(), - tolerance=NUMPY_TOLERANCE, - processing_time=0.020, - main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("roadCameraState").stream), - vision_pubs=["roadCameraState", "wideRoadCameraState"], - ignore_alive_pubs=["wideRoadCameraState"], - init_callback=get_car_params_callback, - ), - ProcessConfig( - proc_name="dmonitoringmodeld", - pubs=["liveCalibration", "driverCameraState"], - subs=["driverStateV2"], - ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"], - should_recv_callback=MessageBasedRcvCallback("driverCameraState"), + proc_name="laikad", + pub_sub={ + "ubloxGnss": ["gnssMeasurements"], + "clocks": [] + }, + ignore=["logMonoTime"], + init_callback=get_car_params, + should_recv_callback=laika_rcv_callback, tolerance=NUMPY_TOLERANCE, - processing_time=0.020, - main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), - vision_pubs=["driverCameraState"], - ignore_alive_pubs=["driverCameraState"], + fake_pubsubmaster=True, ), ] -def get_process_config(name: str) -> ProcessConfig: - try: - return copy.deepcopy(next(c for c in CONFIGS if c.proc_name == name)) - except StopIteration as ex: - raise Exception(f"Cannot find process config with name: {name}") from ex - - -def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> dict[str, Any]: - """ - Use this to get custom params dict based on provided logs. - Useful when replaying following processes: calibrationd, paramsd, torqued - The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. - """ +def replay_process(cfg, lr, fingerprint=None): + with OpenpilotPrefix(): + if cfg.fake_pubsubmaster: + return python_replay_process(cfg, lr, fingerprint) + else: + return cpp_replay_process(cfg, lr, fingerprint) - car_params = [m for m in lr if m.which() == "carParams"] - live_calibration = [m for m in lr if m.which() == "liveCalibration"] - live_parameters = [m for m in lr if m.which() == "liveParameters"] - live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] - assert initial_state in ["first", "last"] - msg_index = 0 if initial_state == "first" else -1 +def setup_env(simulation=False, CP=None, cfg=None, controlsState=None): + params = Params() + params.clear_all() + params.put_bool("OpenpilotEnabledToggle", True) + params.put_bool("Passive", False) + params.put_bool("DisengageOnAccelerator", True) + params.put_bool("WideCameraOnly", False) + params.put_bool("DisableLogging", False) + + os.environ["NO_RADAR_SLEEP"] = "1" + os.environ["REPLAY"] = "1" + os.environ['SKIP_FW_QUERY'] = "" + os.environ['FINGERPRINT'] = "" + + if cfg is not None: + # Clear all custom processConfig environment variables + for config in CONFIGS: + for k, _ in config.environ.items(): + if k in os.environ: + del os.environ[k] + + os.environ.update(cfg.environ) + os.environ['PROC_NAME'] = cfg.proc_name + + if simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] + + # Initialize controlsd with a controlsState packet + if controlsState is not None: + params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) + else: + params.remove("ReplayControlsState") - assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute" - CP = car_params[msg_index].carParams + # Regen or python process + if CP is not None: + if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: + params.put_bool("DisengageOnAccelerator", False) - custom_params = { - "CarParamsPrevRoute": CP.as_builder().to_bytes() - } + if CP.fingerprintSource == "fw": + params.put("CarParamsCache", CP.as_builder().to_bytes()) + else: + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = CP.carFingerprint - if len(live_calibration) > 0: - custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() - if len(live_parameters) > 0: - 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() - return custom_params +def python_replay_process(cfg, lr, fingerprint=None): + sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] + pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] + fsm = FakeSubMaster(pub_sockets, **cfg.submaster_config) + fpm = FakePubMaster(sub_sockets) + args = (fsm, fpm) + if 'can' in list(cfg.pub_sub.keys()): + can_sock = FakeSocket() + args = (fsm, fpm, can_sock) -def replay_process_with_name(name: str | Iterable[str], lr: LogIterable, *args, **kwargs) -> list[capnp._DynamicStructReader]: - if isinstance(name, str): - cfgs = [get_process_config(name)] - elif isinstance(name, Iterable): - cfgs = [get_process_config(n) for n in name] - else: - raise ValueError("name must be str or collections of strings") + all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) + pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] - return replay_process(cfgs, lr, *args, **kwargs) + controlsState = None + initialized = False + for msg in lr: + if msg.which() == 'controlsState': + controlsState = msg.controlsState + if initialized: + break + elif msg.which() == 'carEvents': + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents] + assert controlsState is not None and initialized, "controlsState never initialized" -def replay_process( - cfg: ProcessConfig | Iterable[ProcessConfig], lr: LogIterable, frs: dict[str, FrameReader] | None = None, - fingerprint: str | None = None, return_all_logs: bool = False, custom_params: dict[str, Any] | None = None, - captured_output_store: dict[str, dict[str, str]] | None = None, disable_progress: bool = False -) -> list[capnp._DynamicStructReader]: - if isinstance(cfg, Iterable): - cfgs = list(cfg) - else: - cfgs = [cfg] - - all_msgs = migrate_all(lr, - manager_states=True, - panda_states=any("pandaStates" in cfg.pubs for cfg in cfgs), - camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs)) - process_logs = _replay_multi_process(cfgs, all_msgs, frs, fingerprint, custom_params, captured_output_store, disable_progress) - - if return_all_logs: - keys = {m.which() for m in process_logs} - modified_logs = [m for m in all_msgs if m.which() not in keys] - modified_logs.extend(process_logs) - modified_logs.sort(key=lambda m: int(m.logMonoTime)) - log_msgs = modified_logs + if fingerprint is not None: + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = fingerprint + setup_env(cfg=cfg, controlsState=controlsState) else: - log_msgs = process_logs + CP = [m for m in lr if m.which() == 'carParams'][0].carParams + setup_env(CP=CP, cfg=cfg, controlsState=controlsState) - return log_msgs + assert(type(managed_processes[cfg.proc_name]) is PythonProcess) + managed_processes[cfg.proc_name].prepare() + mod = importlib.import_module(managed_processes[cfg.proc_name].module) + thread = threading.Thread(target=mod.main, args=args) + thread.daemon = True + thread.start() -def _replay_multi_process( - cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, FrameReader] | None, fingerprint: str | None, - custom_params: dict[str, Any] | None, captured_output_store: dict[str, dict[str, str]] | None, disable_progress: bool -) -> list[capnp._DynamicStructReader]: - if fingerprint is not None: - params_config = generate_params_config(lr=lr, fingerprint=fingerprint, custom_params=custom_params) - env_config = generate_environ_config(fingerprint=fingerprint) - else: - CP = next((m.carParams for m in lr if m.which() == "carParams"), None) - params_config = generate_params_config(lr=lr, CP=CP, custom_params=custom_params) - env_config = generate_environ_config(CP=CP) - - # validate frs and vision pubs - all_vision_pubs = [pub for cfg in cfgs for pub in cfg.vision_pubs] - if len(all_vision_pubs) != 0: - assert frs is not None, "frs must be provided when replaying process using vision streams" - assert all(meta_from_camera_state(st) is not None for st in all_vision_pubs), \ - f"undefined vision stream spotted, probably misconfigured process: (vision pubs: {all_vision_pubs})" - required_vision_pubs = {m.camera_state for m in available_streams(lr)} & set(all_vision_pubs) - assert all(st in frs for st in required_vision_pubs), f"frs for this process must contain following vision streams: {required_vision_pubs}" + if cfg.init_callback is not None: + if 'can' not in list(cfg.pub_sub.keys()): + can_sock = None + cfg.init_callback(all_msgs, fsm, can_sock, fingerprint) - all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) - log_msgs = [] - containers = [] - try: - for cfg in cfgs: - container = ProcessContainer(cfg) - containers.append(container) - container.start(params_config, env_config, all_msgs, frs, fingerprint, captured_output_store is not None) - - all_pubs = {pub for container in containers for pub in container.pubs} - all_subs = {sub for container in containers for sub in container.subs} - lr_pubs = all_pubs - all_subs - pubs_to_containers = {pub: [container for container in containers if pub in container.pubs] for pub in all_pubs} - - pub_msgs = [msg for msg in all_msgs if msg.which() in lr_pubs] - # external queue for messages taken from logs; internal queue for messages generated by processes, which will be republished - external_pub_queue: list[capnp._DynamicStructReader] = pub_msgs.copy() - internal_pub_queue: list[capnp._DynamicStructReader] = [] - # heap for maintaining the order of messages generated by processes, where each element: (logMonoTime, index in internal_pub_queue) - internal_pub_index_heap: list[tuple[int, int]] = [] - - pbar = tqdm(total=len(external_pub_queue), disable=disable_progress) - while len(external_pub_queue) != 0 or (len(internal_pub_index_heap) != 0 and not all(c.has_empty_queue for c in containers)): - if len(internal_pub_index_heap) == 0 or (len(external_pub_queue) != 0 and external_pub_queue[0].logMonoTime < internal_pub_index_heap[0][0]): - msg = external_pub_queue.pop(0) - pbar.update(1) - else: - _, index = heapq.heappop(internal_pub_index_heap) - msg = internal_pub_queue[index] - - target_containers = pubs_to_containers[msg.which()] - for container in target_containers: - output_msgs = container.run_step(msg, frs) - for m in output_msgs: - if m.which() in all_pubs: - internal_pub_queue.append(m) - heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1)) - log_msgs.extend(output_msgs) - - # flush last set of messages from each process - for container in containers: - last_time = log_msgs[-1].logMonoTime if len(log_msgs) > 0 else int(time.monotonic() * 1e9) - log_msgs.extend(container.get_output_msgs(last_time)) - finally: - for container in containers: - container.stop() - if captured_output_store is not None: - assert container.capture is not None - out, err = container.capture.read_outerr() - captured_output_store[container.cfg.proc_name] = {"out": out, "err": err} + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - return log_msgs + # wait for started process to be ready + if 'can' in list(cfg.pub_sub.keys()): + can_sock.wait_for_recv() + else: + fsm.wait_for_update() + log_msgs, msg_queue = [], [] + for msg in pub_msgs: + if cfg.should_recv_callback is not None: + recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm) + else: + recv_socks = [s for s in cfg.pub_sub[msg.which()] if + (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0] + should_recv = bool(len(recv_socks)) -def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=None) -> dict[str, Any]: - params_dict = { - "OpenpilotEnabledToggle": True, - "DisengageOnAccelerator": True, - "DisableLogging": False, - } + if msg.which() == 'can': + can_sock.send(msg.as_builder().to_bytes()) + else: + msg_queue.append(msg.as_builder()) - if custom_params is not None: - params_dict.update(custom_params) - if lr is not None: - has_ublox = any(msg.which() == "ubloxGnss" for msg in lr) - params_dict["UbloxAvailable"] = has_ublox - is_rhd = next((msg.driverMonitoringState.isRHD for msg in lr if msg.which() == "driverMonitoringState"), False) - params_dict["IsRhdDetected"] = is_rhd + if should_recv: + fsm.update_msgs(msg.logMonoTime / 1e9, msg_queue) + msg_queue = [] - if CP is not None: - if fingerprint is None: - if CP.fingerprintSource == "fw": - params_dict["CarParamsCache"] = CP.as_builder().to_bytes() + recv_cnt = len(recv_socks) + while recv_cnt > 0: + m = fpm.wait_for_msg().as_builder() + m.logMonoTime = msg.logMonoTime + m = m.as_reader() - if CP.openpilotLongitudinalControl: - params_dict["AlphaLongitudinalEnabled"] = True + log_msgs.append(m) + recv_cnt -= m.which() in recv_socks + return log_msgs - if CP.notCar: - params_dict["JoystickDebugMode"] = True - return params_dict +def cpp_replay_process(cfg, lr, fingerprint=None): + sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] # We get responses here + pm = messaging.PubMaster(cfg.pub_sub.keys()) + all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) + pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] + log_msgs = [] -def generate_environ_config(CP=None, fingerprint=None, log_dir=None) -> dict[str, Any]: - environ_dict = {} - environ_dict["PARAMS_ROOT"] = f"{Paths.shm_path()}/params" - if log_dir is not None: - environ_dict["LOG_ROOT"] = log_dir + # We need to fake SubMaster alive since we can't inject a fake clock + setup_env(simulation=True, cfg=cfg) - environ_dict["REPLAY"] = "1" + managed_processes[cfg.proc_name].prepare() + managed_processes[cfg.proc_name].start() - # Regen or python process - if CP is not None and fingerprint is None: - if CP.fingerprintSource == "fw": - environ_dict['SKIP_FW_QUERY'] = "" - environ_dict['FINGERPRINT'] = "" - else: - environ_dict['SKIP_FW_QUERY'] = "1" - environ_dict['FINGERPRINT'] = CP.carFingerprint - elif fingerprint is not None: - environ_dict['SKIP_FW_QUERY'] = "1" - environ_dict['FINGERPRINT'] = fingerprint - else: - environ_dict["SKIP_FW_QUERY"] = "" - environ_dict["FINGERPRINT"] = "" + try: + with Timeout(TIMEOUT): + while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()): + time.sleep(0) + + # Make sure all subscribers are connected + sockets = {s: messaging.sub_sock(s, timeout=2000) for s in sub_sockets} + for s in sub_sockets: + messaging.recv_one_or_none(sockets[s]) + + for i, msg in enumerate(pub_msgs): + pm.send(msg.which(), msg.as_builder()) + + resp_sockets = cfg.pub_sub[msg.which()] if cfg.should_recv_callback is None else cfg.should_recv_callback(msg) + for s in resp_sockets: + response = messaging.recv_one(sockets[s]) + + if response is None: + print(f"Warning, no response received {i}") + else: + + response = response.as_builder() + response.logMonoTime = msg.logMonoTime + response = response.as_reader() + log_msgs.append(response) + + if not len(resp_sockets): # We only need to wait if we didn't already wait for a response + while not pm.all_readers_updated(msg.which()): + time.sleep(0) + finally: + managed_processes[cfg.proc_name].signal(signal.SIGKILL) + managed_processes[cfg.proc_name].stop() - return environ_dict + return log_msgs -def check_openpilot_enabled(msgs: LogIterable) -> bool: +def check_enabled(msgs): cur_enabled_count = 0 max_enabled_count = 0 for msg in msgs: if msg.which() == "carParams": if msg.carParams.notCar: return True - elif msg.which() == "selfdriveState": - if msg.selfdriveState.active: + elif msg.which() == "controlsState": + if msg.controlsState.active: cur_enabled_count += 1 else: cur_enabled_count = 0 max_enabled_count = max(max_enabled_count, cur_enabled_count) return max_enabled_count > int(10. / DT_CTRL) - - -def check_most_messages_valid(msgs: LogIterable, threshold: float = 0.9) -> bool: - relevant_services = {sock for cfg in CONFIGS for sock in cfg.subs} - msgs_counts = Counter(msg.which() for msg in msgs) - msgs_valid_counts = Counter(msg.which() for msg in msgs if msg.valid) - - most_valid_for_service = {} - for msg_type in msgs_counts.keys(): - if msg_type not in relevant_services: - continue - - valid_share = msgs_valid_counts.get(msg_type, 0) / msgs_counts[msg_type] - ok = valid_share >= threshold - if not ok: - print(f"WARNING: Service {msg_type} has {valid_share * 100:.2f}% valid messages, which is below threshold of {threshold * 100:.2f}%") - most_valid_for_service[msg_type] = ok - - return all(most_valid_for_service.values()) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 85b79391c3ad0e..f46f39dd273d36 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -67f3daf309dc6cbb6844fcbaeb83e6596637e551 \ No newline at end of file +48db2dee177706285226d1287912e191f1699865 diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index c501a4b25083c1..4e1cbfd30d8684 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -1,118 +1,308 @@ #!/usr/bin/env python3 +import bz2 import os -import argparse import time -import capnp - -from typing import Any -from collections.abc import Iterable - -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \ - check_openpilot_enabled, check_most_messages_valid, get_custom_params_from_lr -from openpilot.selfdrive.test.update_ci_routes import upload_route -from openpilot.tools.lib.framereader import FrameReader -from openpilot.tools.lib.logreader import LogReader, LogIterable, save_log -from openpilot.tools.lib.openpilotci import get_url - - -def regen_segment( - lr: LogIterable, frs: dict[str, Any] | None = None, - processes: Iterable[ProcessConfig] = CONFIGS, disable_tqdm: bool = False -) -> list[capnp._DynamicStructReader]: - all_msgs = sorted(lr, key=lambda m: m.logMonoTime) - custom_params = get_custom_params_from_lr(all_msgs) - - print("Replayed processes:", [p.proc_name for p in processes]) - print("\n\n", "*"*30, "\n\n", sep="") - - output_logs = replay_process(processes, all_msgs, frs, return_all_logs=True, custom_params=custom_params, disable_progress=disable_tqdm) - - return output_logs - - -def setup_data_readers( - route: str, sidx: int, needs_driver_cam: bool = True, needs_road_cam: bool = True, dummy_driver_cam: bool = False -) -> tuple[LogReader, dict[str, Any]]: - lr = LogReader(f"{route}/{sidx}/r") - frs = {} - if needs_road_cam: - frs['roadCameraState'] = FrameReader(get_url(route, str(sidx), "fcamera.hevc")) - if next((True for m in lr if m.which() == "wideRoadCameraState"), False): - frs['wideRoadCameraState'] = FrameReader(get_url(route, str(sidx), "ecamera.hevc")) - if needs_driver_cam: - if dummy_driver_cam: - frs['driverCameraState'] = FrameReader(get_url(route, str(sidx), "fcamera.hevc")) # Use fcam as dummy - else: - device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") - assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." - frs['driverCameraState'] = FrameReader(get_url(route, str(sidx), "dcamera.hevc")) - - return lr, frs - - -def regen_and_save( - route: str, sidx: int, processes: str | Iterable[str] = "all", outdir: str = FAKEDATA, - upload: bool = False, disable_tqdm: bool = False, dummy_driver_cam: bool = False -) -> str: - if not isinstance(processes, str) and not hasattr(processes, "__iter__"): - raise ValueError("whitelist_proc must be a string or iterable") - - if processes != "all": - if isinstance(processes, str): - raise ValueError(f"Invalid value for processes: {processes}") - - replayed_processes = [] - for d in processes: - cfg = get_process_config(d) - replayed_processes.append(cfg) +import multiprocessing +import argparse +from tqdm import tqdm +# run DM procs +os.environ["USE_WEBCAM"] = "1" + +import cereal.messaging as messaging +from cereal import car +from cereal.services import service_list +from cereal.visionipc import VisionIpcServer, VisionStreamType +from common.params import Params +from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot +from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size +from panda.python import Panda +from selfdrive.car.toyota.values import EPS_SCALE +from selfdrive.manager.process import ensure_running +from selfdrive.manager.process_config import managed_processes +from selfdrive.test.process_replay.process_replay import FAKEDATA, setup_env, check_enabled +from selfdrive.test.update_ci_routes import upload_route +from tools.lib.route import Route +from tools.lib.framereader import FrameReader +from tools.lib.logreader import LogReader + +def replay_panda_states(s, msgs): + pm = messaging.PubMaster([s, 'peripheralState']) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']] + + # TODO: new safety params from flags, remove after getting new routes for Toyota + safety_param_migration = { + "TOYOTA PRIUS 2017": EPS_SCALE["TOYOTA PRIUS 2017"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL, + "TOYOTA RAV4 2017": EPS_SCALE["TOYOTA RAV4 2017"] | Panda.FLAG_TOYOTA_ALT_BRAKE, + } + + # Migrate safety param base on carState + cp = [m for m in msgs if m.which() == 'carParams'][0].carParams + if cp.carFingerprint in safety_param_migration: + safety_param = safety_param_migration[cp.carFingerprint] + elif len(cp.safetyConfigs): + safety_param = cp.safetyConfigs[0].safetyParam + if cp.safetyConfigs[0].safetyParamDEPRECATED != 0: + safety_param = cp.safetyConfigs[0].safetyParamDEPRECATED else: - replayed_processes = CONFIGS + safety_param = cp.safetyParamDEPRECATED - all_vision_pubs = {pub for cfg in replayed_processes for pub in cfg.vision_pubs} - lr, frs = setup_data_readers(route, sidx, - needs_driver_cam="driverCameraState" in all_vision_pubs, - needs_road_cam="roadCameraState" in all_vision_pubs or "wideRoadCameraState" in all_vision_pubs, - dummy_driver_cam=dummy_driver_cam) - output_logs = regen_segment(lr, frs, replayed_processes, disable_tqdm=disable_tqdm) + while True: + for m in smsgs: + if m.which() == 'pandaStateDEPRECATED': + new_m = messaging.new_message('pandaStates', 1) + new_m.pandaStates[0] = m.pandaStateDEPRECATED + new_m.pandaStates[0].safetyParam = safety_param + pm.send(s, new_m) + else: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) - log_dir = os.path.join(outdir, time.strftime("%Y-%m-%d--%H-%M-%S--0", time.gmtime())) - rel_log_dir = os.path.relpath(log_dir) - rpath = os.path.join(log_dir, "rlog.zst") + new_m = messaging.new_message('peripheralState') + pm.send('peripheralState', new_m) - os.makedirs(log_dir) - save_log(rpath, output_logs, compress=True) + rk.keep_time() - print("\n\n", "*"*30, "\n\n", sep="") - print("New route:", rel_log_dir, "\n") - if not check_openpilot_enabled(output_logs): - raise Exception("Route did not engage for long enough") - if not check_most_messages_valid(output_logs): - raise Exception("Route has too many invalid messages") +def replay_manager_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - if upload: - upload_route(rel_log_dir) + while True: + new_m = messaging.new_message('managerState') + new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] + pm.send(s, new_m) + rk.keep_time() - return rel_log_dir +def replay_device_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + new_m.deviceState.freeSpacePercent = 50 + new_m.deviceState.memoryUsagePercent = 50 + pm.send(s, new_m) + rk.keep_time() + + +def replay_sensor_events(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + + for evt in new_m.sensorEvents: + evt.timestamp = new_m.logMonoTime + + pm.send(s, new_m) + rk.keep_time() + + +def replay_service(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) + rk.keep_time() + + +def replay_cameras(lr, frs, disable_tqdm=False): + eon_cameras = [ + ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_ROAD, True), + ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER, False), + ] + tici_cameras = [ + ("roadCameraState", DT_MDL, tici_f_frame_size, VisionStreamType.VISION_STREAM_ROAD, True), + ("driverCameraState", DT_MDL, tici_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER, False), + ] + + def replay_camera(s, stream, dt, vipc_server, frames, size, use_extra_client): + services = [(s, stream)] + if use_extra_client: + services.append(("wideRoadCameraState", VisionStreamType.VISION_STREAM_WIDE_ROAD)) + pm = messaging.PubMaster([s for s, _ in services]) + rk = Ratekeeper(1 / dt, print_delay_threshold=None) + + img = b"\x00" * int(size[0] * size[1] * 3 / 2) + while True: + if frames is not None: + img = frames[rk.frame % len(frames)] + + rk.keep_time() + + for s, stream in services: + m = messaging.new_message(s) + msg = getattr(m, s) + msg.frameId = rk.frame + msg.timestampSof = m.logMonoTime + msg.timestampEof = m.logMonoTime + pm.send(s, m) + + vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof) + + init_data = [m for m in lr if m.which() == 'initData'][0] + cameras = tici_cameras if (init_data.initData.deviceType == 'tici') else eon_cameras + + # init vipc server and cameras + p = [] + vs = VisionIpcServer("camerad") + for (s, dt, size, stream, use_extra_client) in cameras: + fr = frs.get(s, None) + + frames = None + if fr is not None: + print(f"Decompressing frames {s}") + frames = [] + for i in tqdm(range(fr.frame_count), disable=disable_tqdm): + img = fr.get(i, pix_fmt='nv12')[0] + frames.append(img.flatten().tobytes()) + + vs.create_buffers(stream, 40, False, size[0], size[1]) + if use_extra_client: + vs.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, size[0], size[1]) + p.append(multiprocessing.Process(target=replay_camera, + args=(s, stream, dt, vs, frames, size, use_extra_client))) + + vs.start_listener() + return vs, p -if __name__ == "__main__": - def comma_separated_list(string): - return string.split(",") - all_procs = [p.proc_name for p in CONFIGS] +def migrate_carparams(lr): + all_msgs = [] + for msg in lr: + if msg.which() == 'carParams': + CP = messaging.new_message('carParams') + CP.carParams = msg.carParams.as_builder() + for car_fw in CP.carParams.carFw: + car_fw.brand = CP.carParams.carName + msg = CP.as_reader() + all_msgs.append(msg) + + return all_msgs + + +def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): + lr = migrate_carparams(list(lr)) + if frs is None: + frs = dict() + + params = Params() + os.environ["LOG_ROOT"] = outdir + + # Get and setup initial state + CP = [m for m in lr if m.which() == 'carParams'][0].carParams + controlsState = [m for m in lr if m.which() == 'controlsState'][0].controlsState + liveCalibration = [m for m in lr if m.which() == 'liveCalibration'][0] + + setup_env(CP=CP, controlsState=controlsState) + params.put("CalibrationParams", liveCalibration.as_builder().to_bytes()) + + vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) + fake_daemons = { + 'sensord': [ + multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), + ], + 'pandad': [ + multiprocessing.Process(target=replay_service, args=('can', lr)), + multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), + multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), + ], + 'managerState': [ + multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), + ], + 'thermald': [ + multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), + ], + 'camerad': [ + *cam_procs, + ], + } + + try: + # TODO: make first run of onnxruntime CUDA provider fast + managed_processes["modeld"].start() + managed_processes["dmonitoringmodeld"].start() + time.sleep(5) + + # start procs up + ignore = list(fake_daemons.keys()) + ['ui', 'manage_athenad', 'uploader', 'soundd'] + ensure_running(managed_processes.values(), started=True, params=Params(), CP=car.CarParams(), not_run=ignore) + for procs in fake_daemons.values(): + for p in procs: + p.start() + + for _ in tqdm(range(60), disable=disable_tqdm): + # ensure all procs are running + for d, procs in fake_daemons.items(): + for p in procs: + if not p.is_alive(): + raise Exception(f"{d}'s {p.name} died") + time.sleep(1) + finally: + # kill everything + for p in managed_processes.values(): + p.stop() + for procs in fake_daemons.values(): + for p in procs: + p.terminate() + + del vs + + segment = params.get("CurrentRoute", encoding='utf-8') + "--0" + seg_path = os.path.join(outdir, segment) + # check to make sure openpilot is engaged in the route + if not check_enabled(LogReader(os.path.join(seg_path, "rlog"))): + raise Exception(f"Route did not engage for long enough: {segment}") + + return seg_path + + +def regen_and_save(route, sidx, upload=False, use_route_meta=False, outdir=FAKEDATA, disable_tqdm=False): + if use_route_meta: + r = Route(args.route) + lr = LogReader(r.log_paths()[args.seg]) + fr = FrameReader(r.camera_paths()[args.seg]) + else: + lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") + fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") + rpath = regen_segment(lr, {'roadCameraState': fr}, outdir=outdir, disable_tqdm=disable_tqdm) + + # compress raw rlog before uploading + with open(os.path.join(rpath, "rlog"), "rb") as f: + data = bz2.compress(f.read()) + with open(os.path.join(rpath, "rlog.bz2"), "wb") as f: + f.write(data) + os.remove(os.path.join(rpath, "rlog")) + + lr = LogReader(os.path.join(rpath, 'rlog.bz2')) + controls_state_active = [m.controlsState.active for m in lr if m.which() == 'controlsState'] + assert any(controls_state_active), "Segment did not engage" + + relr = os.path.relpath(rpath) + + print("\n\n", "*"*30, "\n\n") + print("New route:", relr, "\n") + if upload: + upload_route(relr, exclude_patterns=['*.hevc', ]) + return relr + + +if __name__ == "__main__": parser = argparse.ArgumentParser(description="Generate new segments from old ones") parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") - parser.add_argument("--outdir", help="log output dir", default=FAKEDATA) - parser.add_argument("--dummy-dcamera", action='store_true', help="Use dummy blank driver camera") - parser.add_argument("--whitelist-procs", type=comma_separated_list, default=all_procs, - help="Comma-separated whitelist of processes to regen (e.g. controlsd,radard)") - parser.add_argument("--blacklist-procs", type=comma_separated_list, default=[], - help="Comma-separated blacklist of processes to regen (e.g. controlsd,radard)") parser.add_argument("route", type=str, help="The source route") parser.add_argument("seg", type=int, help="Segment in source route") args = parser.parse_args() - - blacklist_set = set(args.blacklist_procs) - processes = [p for p in args.whitelist_procs if p not in blacklist_set] - regen_and_save(args.route, args.seg, processes=processes, upload=args.upload, outdir=args.outdir, dummy_driver_cam=args.dummy_dcamera) + regen_and_save(args.route, args.seg, args.upload) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py index 78a90b420c135e..f10d7ea03a6ced 100755 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -3,13 +3,12 @@ import concurrent.futures import os import random -import traceback from tqdm import tqdm -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.test.process_replay.regen import regen_and_save -from openpilot.selfdrive.test.process_replay.test_processes import FAKEDATA, source_segments as segments -from openpilot.tools.lib.route import SegmentName +from selfdrive.test.process_replay.helpers import OpenpilotPrefix +from selfdrive.test.process_replay.regen import regen_and_save +from selfdrive.test.process_replay.test_processes import FAKEDATA, original_segments as segments +from tools.lib.route import SegmentName def regen_job(segment, upload, disable_tqdm): @@ -17,37 +16,23 @@ def regen_job(segment, upload, disable_tqdm): sn = SegmentName(segment[1]) fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11)) try: - relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, - outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm, dummy_driver_cam=True) + relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False, outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm) relr = '|'.join(relr.split('/')[-2:]) return f' ("{segment[0]}", "{relr}"), ' except Exception as e: - err = f" {segment} failed: {str(e)}" - err += traceback.format_exc() - err += "\n\n" - return err + return f" {segment} failed: {str(e)}" if __name__ == "__main__": - all_cars = {car for car, _ in segments} - parser = argparse.ArgumentParser(description="Generate new segments from old ones") parser.add_argument("-j", "--jobs", type=int, default=1) parser.add_argument("--no-upload", action="store_true") - parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, - help="Whitelist given cars from the test (e.g. HONDA)") - parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], - help="Blacklist given cars from the test (e.g. HONDA)") args = parser.parse_args() - tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) - tested_cars = {c.upper() for c in tested_cars} - tested_segments = [(car, segment) for car, segment in segments if car in tested_cars] - with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: - p = pool.map(regen_job, tested_segments, [not args.no_upload] * len(tested_segments), [args.jobs > 1] * len(tested_segments)) + p = pool.map(regen_job, segments, [not args.no_upload] * len(segments), [args.jobs > 1] * len(segments)) msg = "Copy these new segments into test_processes.py:" - for seg in tqdm(p, desc="Generating segments", total=len(tested_segments)): + for seg in tqdm(p, desc="Generating segments", total=len(segments)): msg += "\n" + str(seg) print() print() diff --git a/selfdrive/test/process_replay/test_debayer.py b/selfdrive/test/process_replay/test_debayer.py new file mode 100755 index 00000000000000..1b3e0f112e5ad1 --- /dev/null +++ b/selfdrive/test/process_replay/test_debayer.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +import os +import sys +import bz2 +import numpy as np + +import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` + +from system.hardware import PC, TICI +from common.basedir import BASEDIR +from selfdrive.test.openpilotci import BASE_URL, get_url +from system.version import get_commit +from system.camerad.snapshot.snapshot import yuv_to_rgb +from tools.lib.logreader import LogReader +from tools.lib.filereader import FileReader + +TEST_ROUTE = "8345e3b82948d454|2022-05-04--13-45-33" +SEGMENT = 0 + +FRAME_WIDTH = 1928 +FRAME_HEIGHT = 1208 +FRAME_STRIDE = 2896 + +UV_WIDTH = FRAME_WIDTH // 2 +UV_HEIGHT = FRAME_HEIGHT // 2 +UV_SIZE = UV_WIDTH * UV_HEIGHT + + +def get_frame_fn(ref_commit, test_route, tici=True): + return f"{test_route}_debayer{'_tici' if tici else ''}_{ref_commit}.bz2" + + +def bzip_frames(frames): + data = bytes() + for y, u, v in frames: + data += y.tobytes() + data += u.tobytes() + data += v.tobytes() + return bz2.compress(data) + + +def unbzip_frames(url): + with FileReader(url) as f: + dat = f.read() + + data = bz2.decompress(dat) + + res = [] + for y_start in range(0, len(data), FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2): + u_start = y_start + FRAME_WIDTH * FRAME_HEIGHT + v_start = u_start + UV_SIZE + + y = np.frombuffer(data[y_start: u_start], dtype=np.uint8).reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = np.frombuffer(data[u_start: v_start], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + v = np.frombuffer(data[v_start: v_start + UV_SIZE], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) + + res.append((y, u, v)) + + return res + + +def init_kernels(frame_offset=0): + ctx = cl.create_some_context(interactive=False) + + with open(os.path.join(BASEDIR, 'system/camerad/cameras/real_debayer.cl')) as f: + build_args = ' -cl-fast-relaxed-math -cl-denorms-are-zero -cl-single-precision-constant' + \ + f' -DFRAME_STRIDE={FRAME_STRIDE} -DRGB_WIDTH={FRAME_WIDTH} -DRGB_HEIGHT={FRAME_HEIGHT} -DFRAME_OFFSET={frame_offset} -DCAM_NUM=0' + if PC: + build_args += ' -DHALF_AS_FLOAT=1 -cl-std=CL2.0' + debayer_prg = cl.Program(ctx, f.read()).build(options=build_args) + + return ctx, debayer_prg + +def debayer_frame(ctx, debayer_prg, data, rgb=False): + q = cl.CommandQueue(ctx) + + yuv_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2, dtype=np.uint8) + + cam_g = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=data) + yuv_g = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2) + + local_worksize = (20, 20) if TICI else (4, 4) + ev1 = debayer_prg.debayer10(q, (UV_WIDTH, UV_HEIGHT), local_worksize, cam_g, yuv_g) + cl.enqueue_copy(q, yuv_buff, yuv_g, wait_for=[ev1]).wait() + cl.enqueue_barrier(q) + + y = yuv_buff[:FRAME_WIDTH*FRAME_HEIGHT].reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT:FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE].reshape((UV_HEIGHT, UV_WIDTH)) + v = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE:].reshape((UV_HEIGHT, UV_WIDTH)) + + if rgb: + return yuv_to_rgb(y, u, v) + else: + return y, u, v + + +def debayer_replay(lr): + ctx, debayer_prg = init_kernels() + + frames = [] + for m in lr: + if m.which() == 'roadCameraState': + cs = m.roadCameraState + if cs.image: + data = np.frombuffer(cs.image, dtype=np.uint8) + img = debayer_frame(ctx, debayer_prg, data) + + frames.append(img) + + return frames + + +if __name__ == "__main__": + update = "--update" in sys.argv + replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "debayer_replay_ref_commit") + + # load logs + lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) + + # run replay + frames = debayer_replay(lr) + + # get diff + failed = False + diff = '' + yuv_i = ['y', 'u', 'v'] + if not update: + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + frame_fn = get_frame_fn(ref_commit, TEST_ROUTE, tici=TICI) + + try: + cmp_frames = unbzip_frames(BASE_URL + frame_fn) + + if len(frames) != len(cmp_frames): + failed = True + diff += 'amount of frames not equal\n' + + for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames)): + for j in range(3): + fr = frame[j] + cmp_f = cmp_frame[j] + if fr.shape != cmp_f.shape: + failed = True + diff += f'frame shapes not equal for ({i}, {yuv_i[j]})\n' + diff += f'{ref_commit}: {cmp_f.shape}\n' + diff += f'HEAD: {fr.shape}\n' + elif not np.array_equal(fr, cmp_f): + failed = True + if np.allclose(fr, cmp_f, atol=1): + diff += f'frames not equal for ({i}, {yuv_i[j]}), but are all close\n' + else: + diff += f'frames not equal for ({i}, {yuv_i[j]})\n' + + frame_diff = np.abs(np.subtract(fr, cmp_f)) + diff_len = len(np.nonzero(frame_diff)[0]) + if diff_len > 10000: + diff += f'different at a large amount of pixels ({diff_len})\n' + else: + diff += 'different at (frame, yuv, pixel, ref, HEAD):\n' + for k in zip(*np.nonzero(frame_diff)): + diff += f'{i}, {yuv_i[j]}, {k}, {cmp_f[k]}, {fr[k]}\n' + + if failed: + print(diff) + with open("debayer_diff.txt", "w") as f: + f.write(diff) + except Exception as e: + print(str(e)) + failed = True + + # upload new refs + if update or (failed and TICI): + from selfdrive.test.openpilotci import upload_file + + print("Uploading new refs") + + frames_bzip = bzip_frames(frames) + + new_commit = get_commit() + frame_fn = os.path.join(replay_dir, get_frame_fn(new_commit, TEST_ROUTE, tici=TICI)) + with open(frame_fn, "wb") as f2: + f2.write(frames_bzip) + + try: + upload_file(frame_fn, os.path.basename(frame_fn)) + except Exception as e: + print("failed to upload", e) + + if update: + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\nNew ref commit: ", new_commit) + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py old mode 100644 new mode 100755 index 723112163ebd73..28e0f70589cf57 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -1,31 +1,175 @@ -import copy -import os -from hypothesis import given, HealthCheck, Phase, settings +#!/usr/bin/env python3 +import sys +import unittest + import hypothesis.strategies as st -from parameterized import parameterized +import numpy as np +from hypothesis import given, settings, note from cereal import log -from opendbc.car.toyota.values import CAR as TOYOTA -from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator -import openpilot.selfdrive.test.process_replay.process_replay as pr - -# These processes currently fail because of unrealistic data breaking assumptions -# that openpilot makes causing error with NaN, inf, int size, array indexing ... -# TODO: Make each one testable -NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] - -TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] -MAX_EXAMPLES = int(os.environ.get("MAX_EXAMPLES", "10")) - -class TestFuzzProcesses: - - # TODO: make this faster and increase examples - @parameterized.expand(TEST_CASES) - @given(st.data()) - @settings(phases=[Phase.generate, Phase.target], max_examples=MAX_EXAMPLES, deadline=1000, - suppress_health_check=[HealthCheck.too_slow, HealthCheck.data_too_large]) - def test_fuzz_process(self, proc_name, cfg, data): - msgs = FuzzyGenerator.get_random_event_msg(data.draw, events=cfg.pubs, real_floats=True) - lr = [log.Event.new_message(**m).as_reader() for m in msgs] - cfg.timeout = 5 - pr.replay_process(cfg, lr, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2, disable_progress=True) +from selfdrive.car.toyota.values import CAR as TOYOTA +import selfdrive.test.process_replay.process_replay as pr + + +def get_process_config(process): + return [cfg for cfg in pr.CONFIGS if cfg.proc_name == process][0] + + +def get_event_union_strategy(r, name): + return st.fixed_dictionaries({ + 'valid': st.just(True), + 'logMonoTime': st.integers(min_value=0, max_value=2**64-1), + name: r[name[0].upper() + name[1:]], + }) + + +def get_strategy_for_events(event_types, finite=False): + # TODO: generate automatically based on capnp definitions + def floats(**kwargs): + allow_nan = False if finite else None + allow_infinity = False if finite else None + return st.floats(**kwargs, allow_nan=allow_nan, allow_infinity=allow_infinity) + + r = {} + r['liveLocationKalman.Measurement'] = st.fixed_dictionaries({ + 'value': st.lists(floats(), min_size=3, max_size=3), + 'std': st.lists(floats(), min_size=3, max_size=3), + 'valid': st.just(True), + }) + r['LiveLocationKalman'] = st.fixed_dictionaries({ + 'angularVelocityCalibrated': r['liveLocationKalman.Measurement'], + 'inputsOK': st.booleans(), + 'posenetOK': st.booleans(), + }) + r['CarState'] = st.fixed_dictionaries({ + 'vEgo': floats(width=32), + 'vEgoRaw': floats(width=32), + 'steeringPressed': st.booleans(), + 'steeringAngleDeg': floats(width=32), + }) + r['CameraOdometry'] = st.fixed_dictionaries({ + 'frameId': st.integers(min_value=0, max_value=2**32 - 1), + 'timestampEof': st.integers(min_value=0, max_value=2**64 - 1), + 'trans': st.lists(floats(width=32), min_size=3, max_size=3), + 'rot': st.lists(floats(width=32), min_size=3, max_size=3), + 'transStd': st.lists(floats(width=32), min_size=3, max_size=3), + 'rotStd': st.lists(floats(width=32), min_size=3, max_size=3), + }) + r['SensorEventData.SensorVec'] = st.fixed_dictionaries({ + 'v': st.lists(floats(width=32), min_size=3, max_size=3), + 'status': st.just(1), + }) + r['SensorEventData_gyro'] = st.fixed_dictionaries({ + 'version': st.just(1), + 'sensor': st.just(5), + 'type': st.just(16), + 'timestamp': st.integers(min_value=0, max_value=2**63 - 1), + 'source': st.just(8), # BMX055 + 'gyroUncalibrated': r['SensorEventData.SensorVec'], + }) + r['SensorEventData_accel'] = st.fixed_dictionaries({ + 'version': st.just(1), + 'sensor': st.just(1), + 'type': st.just(1), + 'timestamp': st.integers(min_value=0, max_value=2**63 - 1), + 'source': st.just(8), # BMX055 + 'acceleration': r['SensorEventData.SensorVec'], + }) + r['SensorEvents'] = st.lists(st.one_of(r['SensorEventData_gyro'], r['SensorEventData_accel']), min_size=1) + r['GpsLocationExternal'] = st.fixed_dictionaries({ + 'flags': st.just(1), + 'latitude': floats(), + 'longitude': floats(), + 'altitude': floats(), + 'speed': floats(width=32), + 'bearingDeg': floats(width=32), + 'accuracy': floats(width=32), + 'timestamp': st.integers(min_value=0, max_value=2**63 - 1), + 'source': st.just(6), # Ublox + 'vNED': st.lists(floats(width=32), min_size=3, max_size=3), + 'verticalAccuracy': floats(width=32), + 'bearingAccuracyDeg': floats(width=32), + 'speedAccuracy': floats(width=32), + }) + r['LiveCalibration'] = st.fixed_dictionaries({ + 'calStatus': st.integers(min_value=0, max_value=1), + 'rpyCalib': st.lists(floats(width=32), min_size=3, max_size=3), + }) + + return st.lists(st.one_of(*[get_event_union_strategy(r, n) for n in event_types])) + + +def get_strategy_for_process(process, finite=False): + return get_strategy_for_events(get_process_config(process).pub_sub.keys(), finite) + + +def convert_to_lr(msgs): + return [log.Event.new_message(**m).as_reader() for m in msgs] + + +def is_finite(d, exclude=[], prefix=""): # pylint: disable=dangerous-default-value + ret = True + for k, v in d.items(): + name = prefix + f"{k}" + if name in exclude: + continue + + if isinstance(v, dict): + if not is_finite(v, exclude, name + "."): + ret = False + else: + try: + if not np.isfinite(v).all(): + note((name, v)) + ret = False + except TypeError: + pass + + return ret + + +def test_process(dat, name): + cfg = get_process_config(name) + lr = convert_to_lr(dat) + pr.TIMEOUT = 0.1 + return pr.replay_process(cfg, lr, TOYOTA.COROLLA_TSS2) + + +class TestFuzzy(unittest.TestCase): + @given(get_strategy_for_process('paramsd')) + @settings(deadline=1000) + def test_paramsd(self, dat): + for r in test_process(dat, 'paramsd'): + d = r.liveParameters.to_dict() + assert is_finite(d) + + @given(get_strategy_for_process('locationd', finite=True)) + @settings(deadline=1000) + def test_locationd(self, dat): + exclude = [ + 'positionGeodetic.std', + 'velocityNED.std', + 'orientationNED.std', + 'calibratedOrientationECEF.std', + ] + for r in test_process(dat, 'locationd'): + d = r.liveLocationKalman.to_dict() + assert is_finite(d, exclude) + + +if __name__ == "__main__": + procs = { + 'locationd': TestFuzzy().test_locationd, + 'paramsd': TestFuzzy().test_paramsd, + } + + if len(sys.argv) != 2: + print("Usage: ./test_fuzzy.py ") + sys.exit(0) + + proc = sys.argv[1] + if proc not in procs: + print(f"{proc} not available") + sys.exit(0) + else: + procs[proc]() diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 59e1ae054e3b86..e8c2e1dc9494e6 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -5,68 +5,61 @@ import sys from collections import defaultdict from tqdm import tqdm -from typing import Any - -from opendbc.car.car_helpers import interface_names -from openpilot.common.git import get_commit -from openpilot.tools.lib.openpilotci import get_url, upload_file -from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \ - check_most_messages_valid -from openpilot.tools.lib.filereader import FileReader -from openpilot.tools.lib.logreader import LogReader, save_log - -source_segments = [ - ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.HYUNDAI_SONATA - ("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.HYUNDAI_KIA_EV6 (+ QCOM GPS) - ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.TOYOTA_PRIUS - ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.TOYOTA_RAV4 - ("TOYOTA3", "8011d605be1cbb77|000000cc--8e8d8ec716--6"), # TOYOTA.TOYOTA_COROLLA_TSS2 - ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.HONDA_CIVIC (NIDEC) - ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.HONDA_ACCORD (BOSCH) - ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID - ("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), # CHRYSLER.RAM_1500_5TH_GEN - ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.SUBARU_OUTBACK - ("GM", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.CHEVROLET_BOLT_EUV - ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.NISSAN_XTRAIL - ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.VOLKSWAGEN_GOLF - # FIXME the sensor timings are bad in mazda segment, we're not fully testing it, but it should be replaced - ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.MAZDA_CX9_2021 - ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.FORD_BRONCO_SPORT_MK1 - ("RIVIAN", "bc095dc92e101734|000000db--ee9fe46e57--1"), # RIVIAN.RIVIAN_R1_GEN1 - ("TESLA", "2c912ca5de3b1ee9|0000025d--6eb6bcbca4--4"), # TESLA.TESLA_MODEL_Y +from typing import Any, DefaultDict, Dict + +from selfdrive.car.car_helpers import interface_names +from selfdrive.test.openpilotci import get_url, upload_file +from selfdrive.test.process_replay.compare_logs import compare_logs, save_log +from selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, check_enabled, replay_process +from system.version import get_commit +from tools.lib.filereader import FileReader +from tools.lib.logreader import LogReader + +original_segments = [ + ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY + ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA + ("HYUNDAI", "d824e27e8c60172c|2022-08-19--17-58-07--2"), # HYUNDAI.KIA_EV6 + ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) + ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) + ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2 + ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) + ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) + ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA + ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"), # CHRYSLER.RAM_1500 + ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA + ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT + ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL + ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF + ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), # MAZDA.CX9_2021 # Enable when port is tested and dashcamOnly is no longer set - #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.VOLKSWAGEN_PASSAT_NMS + #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS + #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.PASSAT_NMS ] segments = [ - ("HYUNDAI", "regenAA0FC4ED71E|2025-04-08--22-57-50--0"), - ("HYUNDAI2", "regenAFB9780D823|2025-04-08--23-00-34--0"), - ("TOYOTA", "regen218A4DCFAA1|2025-04-08--22-57-51--0"), - # TODO: get new RAV4 route without enableDsu - # ("TOYOTA2", "regen107352E20EB|2025-04-08--22-57-46--0"), - ("TOYOTA3", "regen1455E3B4BDF|2025-04-09--03-26-06--0"), - ("HONDA", "regenB328FF8BA0A|2025-04-08--22-57-45--0"), - ("HONDA2", "regen6170C8C9A35|2025-04-08--22-57-46--0"), - ("CHRYSLER", "regen5B28FC2A437|2025-04-08--23-04-24--0"), - ("RAM", "regenBF81EA96E08|2025-04-08--23-06-54--0"), - ("SUBARU", "regen7366F13F6A1|2025-04-08--23-07-07--0"), - ("GM", "regen1271097D038|2025-04-09--03-26-00--0"), - ("NISSAN", "regen15D60604EAB|2025-04-08--23-06-59--0"), - ("VOLKSWAGEN", "regen0F2F06C9539|2025-04-08--23-06-56--0"), - ("MAZDA", "regenACF84CCF482|2024-08-30--03-21-55--0"), - ("FORD", "regen755D8CB1E1F|2025-04-08--23-13-43--0"), - ("RIVIAN", "regen5FCAC896BBE|2025-04-08--23-13-35--0"), - ("TESLA", "2c912ca5de3b1ee9|0000025d--6eb6bcbca4--4"), + ("BODY", "regen660D86654BA|2022-07-06--14-27-15--0"), + ("HYUNDAI", "regen114E5FF24D8|2022-07-14--17-08-47--0"), + ("HYUNDAI", "d824e27e8c60172c|2022-08-19--17-58-07--2"), + ("TOYOTA", "regenBA97410FBEC|2022-07-06--14-26-49--0"), + ("TOYOTA2", "regenDEDB1D9C991|2022-07-06--14-54-08--0"), + ("TOYOTA3", "regenDDC1FE60734|2022-07-06--14-32-06--0"), + ("HONDA", "regenE62960EEC38|2022-07-14--19-33-24--0"), + ("HONDA2", "regenC3EBD92F029|2022-07-14--19-29-47--0"), + ("CHRYSLER", "regen38346FB33D0|2022-07-14--18-05-26--0"), + ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"), + ("SUBARU", "regen54A1E2BE5AA|2022-07-14--18-07-50--0"), + ("GM", "regen76027B408B7|2022-08-16--19-56-58--0"), + ("NISSAN", "regenCA0B0DC946E|2022-07-14--18-10-17--0"), + ("VOLKSWAGEN", "regen007098CA0EF|2022-07-06--15-01-26--0"), + ("MAZDA", "regen61BA413D53B|2022-07-06--14-39-42--0"), ] # dashcamOnly makes don't need to be tested until a full port is done -excluded_interfaces = ["mock", "body", "psa"] +excluded_interfaces = ["mock", "ford", "mazda", "tesla"] BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") -EXCLUDED_PROCS = {"modeld", "dmonitoringmodeld"} def run_test_process(data): @@ -74,7 +67,7 @@ def run_test_process(data): res = None if not args.upload_only: lr = LogReader.from_bytes(lr_dat) - res, log_msgs = test_process(cfg, lr, segment, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs) + res, log_msgs = test_process(cfg, lr, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs) # save logs so we can upload when updating refs save_log(cur_log_fn, log_msgs) @@ -83,16 +76,16 @@ def run_test_process(data): assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}" upload_file(cur_log_fn, os.path.basename(cur_log_fn)) os.remove(cur_log_fn) - return (segment, cfg.proc_name, res) + return (segment, cfg.proc_name, cfg.subtest_name, res) def get_log_data(segment): r, n = segment.rsplit("--", 1) - with FileReader(get_url(r, n, "rlog.zst")) as f: + with FileReader(get_url(r, n)) as f: return (segment, f.read()) -def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None): +def test_process(cfg, lr, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None): if ignore_fields is None: ignore_fields = [] if ignore_msgs is None: @@ -100,32 +93,61 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non ref_log_msgs = list(LogReader(ref_log_path)) - try: - log_msgs = replay_process(cfg, lr, disable_progress=True) - except Exception as e: - raise Exception("failed on segment: " + segment) from e + log_msgs = replay_process(cfg, lr) - if not check_most_messages_valid(log_msgs): - return f"Route did not have enough valid messages: {new_log_path}", log_msgs - - # skip this check if the segment is using qcom gps - if cfg.proc_name != 'ubloxd' or any(m.which() in cfg.pubs for m in lr): - seen_msgs = {m.which() for m in log_msgs} - expected_msgs = set(cfg.subs) - if seen_msgs != expected_msgs: - return f"Expected messages: {expected_msgs}, but got: {seen_msgs}", log_msgs + # check to make sure openpilot is engaged in the route + if cfg.proc_name == "controlsd": + if not check_enabled(log_msgs): + return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs try: - return compare_logs(ref_log_msgs, log_msgs, ignore_fields + cfg.ignore, ignore_msgs, cfg.tolerance), log_msgs + return compare_logs(ref_log_msgs, log_msgs, ignore_fields + cfg.ignore, ignore_msgs, cfg.tolerance, cfg.field_tolerances), log_msgs except Exception as e: return str(e), log_msgs +def format_diff(results, log_paths, ref_commit): + diff1, diff2 = "", "" + diff2 += f"***** tested against commit {ref_commit} *****\n" + + failed = False + for segment, result in list(results.items()): + diff1 += f"***** results for segment {segment} *****\n" + diff2 += f"***** differences for segment {segment} *****\n" + + for proc, diff in list(result.items()): + # long diff + diff2 += f"*** process: {proc} ***\n" + diff2 += f"\tref: {log_paths[segment][proc]['ref']}\n" + diff2 += f"\tnew: {log_paths[segment][proc]['new']}\n\n" + + # short diff + diff1 += f" {proc}\n" + if isinstance(diff, str): + diff1 += f" ref: {log_paths[segment][proc]['ref']}\n" + diff1 += f" new: {log_paths[segment][proc]['new']}\n\n" + diff1 += f" {diff}\n" + failed = True + elif len(diff): + diff1 += f" ref: {log_paths[segment][proc]['ref']}\n" + diff1 += f" new: {log_paths[segment][proc]['new']}\n\n" + + cnt: Dict[str, int] = {} + for d in diff: + diff2 += f"\t{str(d)}\n" + + k = str(d[1]) + cnt[k] = 1 if k not in cnt else cnt[k] + 1 + + for k, v in sorted(cnt.items()): + diff1 += f" {k}: {v}\n" + failed = True + return diff1, diff2, failed + + if __name__ == "__main__": all_cars = {car for car, _ in segments} - all_procs = {cfg.proc_name for cfg in CONFIGS if cfg.proc_name not in EXCLUDED_PROCS} - - cpu_count = os.cpu_count() or 1 + all_procs = {cfg.proc_name for cfg in CONFIGS} parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") parser.add_argument("--whitelist-procs", type=str, nargs="*", default=all_procs, @@ -137,15 +159,14 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], help="Blacklist given cars from the test (e.g. HONDA)") parser.add_argument("--ignore-fields", type=str, nargs="*", default=[], - help="Extra fields or msgs to ignore (e.g. driverMonitoringState.events)") + help="Extra fields or msgs to ignore (e.g. carState.events)") parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[], help="Msgs to ignore (e.g. carEvents)") parser.add_argument("--update-refs", action="store_true", help="Updates reference logs using current commit") parser.add_argument("--upload-only", action="store_true", help="Skips testing processes and uploads logs from previous test run") - parser.add_argument("-j", "--jobs", type=int, default=max(cpu_count - 2, 1), - help="Max amount of parallel jobs") + parser.add_argument("-j", "--jobs", type=int, default=1) args = parser.parse_args() tested_procs = set(args.whitelist_procs) - set(args.blacklist_procs) @@ -160,14 +181,13 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non assert full_test, "Need to run full test when updating refs" try: - with open(REF_COMMIT_FN) as f: - ref_commit = f.read().strip() + ref_commit = open(REF_COMMIT_FN).read().strip() except FileNotFoundError: print("Couldn't find reference commit") sys.exit(1) cur_commit = get_commit() - if not cur_commit: + if cur_commit is None: raise Exception("Couldn't get current commit") print(f"***** testing against commit {ref_commit} *****") @@ -177,11 +197,11 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non untested = (set(interface_names) - set(excluded_interfaces)) - {c.lower() for c in tested_cars} assert len(untested) == 0, f"Cars missing routes: {str(untested)}" - log_paths: defaultdict[str, dict[str, dict[str, str]]] = defaultdict(lambda: defaultdict(dict)) + log_paths: DefaultDict[str, Dict[str, Dict[str, str]]] = defaultdict(lambda: defaultdict(dict)) with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: if not args.upload_only: download_segments = [seg for car, seg in segments if car in tested_cars] - log_data: dict[str, LogReader] = {} + log_data: Dict[str, LogReader] = {} p1 = pool.map(get_log_data, download_segments) for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): log_data[segment] = lr @@ -195,34 +215,30 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non if cfg.proc_name not in tested_procs: continue - # to speed things up, we only test all segments on card - if cfg.proc_name not in ('card', 'controlsd', 'lagd') and car_brand not in ('HYUNDAI', 'TOYOTA'): - continue - - cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst") + cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}{cfg.subtest_name}_{cur_commit}.bz2") if args.update_refs: # reference logs will not exist if routes were just regenerated - ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.zst") + ref_log_path = get_url(*segment.rsplit("--", 1)) else: - ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.zst") + ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}{cfg.subtest_name}_{ref_commit}.bz2") ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) dat = None if args.upload_only else log_data[segment] pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) - log_paths[segment][cfg.proc_name]['ref'] = ref_log_path - log_paths[segment][cfg.proc_name]['new'] = cur_log_fn + log_paths[segment][cfg.proc_name + cfg.subtest_name]['ref'] = ref_log_path + log_paths[segment][cfg.proc_name + cfg.subtest_name]['new'] = cur_log_fn results: Any = defaultdict(dict) p2 = pool.map(run_test_process, pool_args) - for (segment, proc, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): + for (segment, proc, subtest_name, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): if not args.upload_only: - results[segment][proc] = result + results[segment][proc + subtest_name] = result - diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) + diff1, diff2, failed = format_diff(results, log_paths, ref_commit) if not upload: with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f: - f.write(diff_long) - print(diff_short) + f.write(diff2) + print(diff1) if failed: print("TEST FAILED") diff --git a/selfdrive/test/process_replay/test_regen.py b/selfdrive/test/process_replay/test_regen.py deleted file mode 100644 index 5f26daf786cdd0..00000000000000 --- a/selfdrive/test/process_replay/test_regen.py +++ /dev/null @@ -1,37 +0,0 @@ -from parameterized import parameterized - -from openpilot.selfdrive.test.process_replay.regen import regen_segment -from openpilot.selfdrive.test.process_replay.process_replay import check_openpilot_enabled -from openpilot.tools.lib.openpilotci import get_url -from openpilot.tools.lib.logreader import LogReader -from openpilot.tools.lib.framereader import FrameReader - -TESTED_SEGMENTS = [ - ("PRIUS_C2", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.TOYOTA_PRIUS: NEO, pandaStateDEPRECATED, no peripheralState, sensorEventsDEPRECATED - # Enable these once regen on CI becomes faster or use them for different tests running controlsd in isolation - # ("MAZDA_C3", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021: TICI, incomplete managerState - # ("FORD_C3", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1: TICI -] - - -def ci_setup_data_readers(route, sidx): - lr = LogReader(get_url(route, sidx, "rlog.bz2")) - frs = { - 'roadCameraState': FrameReader(get_url(route, sidx, "fcamera.hevc")), - 'driverCameraState': FrameReader(get_url(route, sidx, "fcamera.hevc")), - } - if next((True for m in lr if m.which() == "wideRoadCameraState"), False): - frs["wideRoadCameraState"] = FrameReader(get_url(route, sidx, "ecamera.hevc")) - - return lr, frs - - -class TestRegen: - @parameterized.expand(TESTED_SEGMENTS) - def test_engaged(self, case_name, segment): - route, sidx = segment.rsplit("--", 1) - lr, frs = ci_setup_data_readers(route, sidx) - output_logs = regen_segment(lr, frs, disable_tqdm=True) - - engaged = check_openpilot_enabled(output_logs) - assert engaged, f"openpilot not engaged in {case_name}" diff --git a/selfdrive/test/process_replay/vision_meta.py b/selfdrive/test/process_replay/vision_meta.py deleted file mode 100644 index 12deb587244134..00000000000000 --- a/selfdrive/test/process_replay/vision_meta.py +++ /dev/null @@ -1,43 +0,0 @@ -from collections import namedtuple -from msgq.visionipc import VisionStreamType -from openpilot.common.realtime import DT_MDL, DT_DMON -from openpilot.common.transformations.camera import DEVICE_CAMERAS - -VideoStreamMeta = namedtuple("VideoStreamMeta", ["camera_state", "encode_index", "stream", "dt", "frame_sizes"]) -ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} -WIDE_ROAD_CAMERA_FRAME_SIZES = {k: (v.ecam.width, v.ecam.height) for k, v in DEVICE_CAMERAS.items() if v.ecam is not None} -DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} -VIPC_STREAM_METADATA = [ - # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes) - ("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_FRAME_SIZES), - ("wideRoadCameraState", "wideRoadEncodeIdx", VisionStreamType.VISION_STREAM_WIDE_ROAD, DT_MDL, WIDE_ROAD_CAMERA_FRAME_SIZES), - ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_CAMERA_FRAME_SIZES), -] - - -def meta_from_camera_state(state): - meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[0] == state), None) - return meta - - -def meta_from_encode_index(encode_index): - meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[1] == encode_index), None) - return meta - - -def meta_from_stream_type(stream_type): - meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[2] == stream_type), None) - return meta - - -def available_streams(lr=None): - if lr is None: - return [VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA] - - result = [] - for meta in VIPC_STREAM_METADATA: - has_cam_state = next((True for m in lr if m.which() == meta[0]), False) - if has_cam_state: - result.append(VideoStreamMeta(*meta)) - - return result diff --git a/selfdrive/test/profiling/.gitignore b/selfdrive/test/profiling/.gitignore new file mode 100644 index 00000000000000..76acac7f93ea12 --- /dev/null +++ b/selfdrive/test/profiling/.gitignore @@ -0,0 +1,2 @@ +cachegrind.out.* +*.prof diff --git a/selfdrive/test/profiling/__init__.py b/selfdrive/test/profiling/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/test/profiling/lib.py b/selfdrive/test/profiling/lib.py new file mode 100644 index 00000000000000..f28346f3f04db8 --- /dev/null +++ b/selfdrive/test/profiling/lib.py @@ -0,0 +1,91 @@ +from collections import defaultdict, deque +from cereal.services import service_list +import cereal.messaging as messaging +import capnp + + +class ReplayDone(Exception): + pass + + +class SubSocket(): + def __init__(self, msgs, trigger): + self.i = 0 + self.trigger = trigger + self.msgs = [m.as_builder().to_bytes() for m in msgs if m.which() == trigger] + self.max_i = len(self.msgs) - 1 + + def receive(self, non_blocking=False): + if non_blocking: + return None + + if self.i == self.max_i: + raise ReplayDone + + while True: + msg = self.msgs[self.i] + self.i += 1 + return msg + + +class PubSocket(): + def send(self, data): + pass + + +class SubMaster(messaging.SubMaster): + def __init__(self, msgs, trigger, services, check_averag_freq=False): # pylint: disable=super-init-not-called + self.frame = 0 + self.data = {} + self.ignore_alive = [] + + self.alive = {s: True for s in services} + self.updated = {s: False for s in services} + self.rcv_time = {s: 0. for s in services} + self.rcv_frame = {s: 0 for s in services} + self.valid = {s: True for s in services} + self.freq_ok = {s: True for s in services} + self.recv_dts = {s: deque([0.0] * messaging.AVG_FREQ_HISTORY, maxlen=messaging.AVG_FREQ_HISTORY) for s in services} + self.logMonoTime = {} + self.sock = {} + self.freq = {} + self.check_average_freq = check_averag_freq + self.non_polled_services = [] + self.ignore_average_freq = [] + + # TODO: specify multiple triggers for service like plannerd that poll on more than one service + cur_msgs = [] + self.msgs = [] + msgs = [m for m in msgs if m.which() in services] + + for msg in msgs: + cur_msgs.append(msg) + if msg.which() == trigger: + self.msgs.append(cur_msgs) + cur_msgs = [] + + self.msgs = list(reversed(self.msgs)) + + for s in services: + self.freq[s] = service_list[s].frequency + try: + data = messaging.new_message(s) + except capnp.lib.capnp.KjException: + # lists + data = messaging.new_message(s, 0) + + self.data[s] = getattr(data, s) + self.logMonoTime[s] = 0 + self.sock[s] = SubSocket(msgs, s) + + def update(self, timeout=None): + if not len(self.msgs): + raise ReplayDone + + cur_msgs = self.msgs.pop() + self.update_msgs(cur_msgs[0].logMonoTime, self.msgs.pop()) + + +class PubMaster(messaging.PubMaster): + def __init__(self): # pylint: disable=super-init-not-called + self.sock = defaultdict(PubSocket) diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py new file mode 100755 index 00000000000000..91226fc577620b --- /dev/null +++ b/selfdrive/test/profiling/profiler.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +import os +import sys +import cProfile # pylint: disable=import-error +import pprofile # pylint: disable=import-error +import pyprof2calltree # pylint: disable=import-error + +from common.params import Params +from tools.lib.logreader import LogReader +from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone +from selfdrive.test.process_replay.process_replay import CONFIGS +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.honda.values import CAR as HONDA +from selfdrive.car.volkswagen.values import CAR as VW + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" + +CARS = { + 'toyota': ("0982d79ebb0de295|2021-01-03--20-03-36/6", TOYOTA.RAV4), + 'honda': ("0982d79ebb0de295|2021-01-08--10-13-10/6", HONDA.CIVIC), + "vw": ("ef895f46af5fd73f|2021-05-22--14-06-35/6", VW.AUDI_A3_MK3), +} + + +def get_inputs(msgs, process, fingerprint): + for config in CONFIGS: + if config.proc_name == process: + sub_socks = list(config.pub_sub.keys()) + trigger = sub_socks[0] + break + + # some procs block on CarParams + for msg in msgs: + if msg.which() == 'carParams': + m = msg.as_builder() + m.carParams.carFingerprint = fingerprint + Params().put("CarParams", m.carParams.copy().to_bytes()) + break + + sm = SubMaster(msgs, trigger, sub_socks) + pm = PubMaster() + if 'can' in sub_socks: + can_sock = SubSocket(msgs, 'can') + else: + can_sock = None + return sm, pm, can_sock + + +def profile(proc, func, car='toyota'): + segment, fingerprint = CARS[car] + segment = segment.replace('|', '/') + rlog_url = f"{BASE_URL}{segment}/rlog.bz2" + msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1")) + + os.environ['FINGERPRINT'] = fingerprint + os.environ['REPLAY'] = "1" + + def run(sm, pm, can_sock): + try: + if can_sock is not None: + func(sm, pm, can_sock) + else: + func(sm, pm) + except ReplayDone: + pass + + # Statistical + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with pprofile.StatisticalProfile()(period=0.00001) as pr: + run(sm, pm, can_sock) + pr.dump_stats(f'cachegrind.out.{proc}_statistical') + + # Deterministic + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with cProfile.Profile() as pr: + run(sm, pm, can_sock) + pyprof2calltree.convert(pr.getstats(), f'cachegrind.out.{proc}_deterministic') + + +if __name__ == '__main__': + from selfdrive.controls.controlsd import main as controlsd_thread + from selfdrive.controls.radard import radard_thread + from selfdrive.locationd.paramsd import main as paramsd_thread + from selfdrive.controls.plannerd import main as plannerd_thread + from selfdrive.locationd.laikad import main as laikad_thread + + procs = { + 'radard': radard_thread, + 'controlsd': controlsd_thread, + 'paramsd': paramsd_thread, + 'plannerd': plannerd_thread, + 'laikad': laikad_thread, + } + + proc = sys.argv[1] + if proc not in procs: + print(f"{proc} not available") + sys.exit(0) + else: + profile(proc, procs[proc]) diff --git a/selfdrive/test/scons_build_test.sh b/selfdrive/test/scons_build_test.sh deleted file mode 100755 index 5ffdb4379e004c..00000000000000 --- a/selfdrive/test/scons_build_test.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env bash -set -e - -SCRIPT_DIR=$(dirname "$0") -BASEDIR=$(realpath "$SCRIPT_DIR/../../") -cd $BASEDIR - -# tests that our build system's dependencies are configured properly, -# needs a machine with lots of cores - -# helpful commands: -# scons -Q --tree=derived - -cd $BASEDIR/opendbc_repo/ -scons --clean -scons --no-cache --random -j$(nproc) -if ! scons -q; then - echo "FAILED: all build products not up to date after first pass." - exit 1 -fi diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index 2a1442a20cce13..bf2f93e1c35aef 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -1,7 +1,6 @@ -#!/usr/bin/env bash +#!/usr/bin/bash set -e -set -x if [ -z "$SOURCE_DIR" ]; then echo "SOURCE_DIR must be set" @@ -18,18 +17,13 @@ if [ -z "$TEST_DIR" ]; then exit 1 fi -# prevent storage from filling up -rm -rf /data/media/0/realdata/* - -rm -rf /data/safe_staging/ || true -if [ -d /data/safe_staging/ ]; then - sudo umount /data/safe_staging/merged/ || true - rm -rf /data/safe_staging/ || true -fi +umount /data/safe_staging/merged/ || true +sudo umount /data/safe_staging/merged/ || true +rm -rf /data/safe_staging/* || true CONTINUE_PATH="/data/continue.sh" tee $CONTINUE_PATH << EOF -#!/usr/bin/env bash +#!/usr/bin/bash sudo abctl --set_success @@ -38,23 +32,14 @@ sudo mount -o rw,remount / sudo sed -i "s,/data/params/d/GithubSshKeys,/usr/comma/setup_keys," /etc/ssh/sshd_config sudo systemctl daemon-reload sudo systemctl restart ssh -sudo systemctl restart NetworkManager sudo systemctl disable ssh-param-watcher.path sudo systemctl disable ssh-param-watcher.service sudo mount -o ro,remount / -sudo systemctl stop power_monitor while true; do if ! sudo systemctl is-active -q ssh; then sudo systemctl start ssh fi - - #if ! pgrep -f 'ciui.py' > /dev/null 2>&1; then - # echo 'starting UI' - # cp $SOURCE_DIR/selfdrive/test/ciui.py /data/ - # /data/ciui.py & - #fi - sleep 5s done @@ -62,73 +47,27 @@ sleep infinity EOF chmod +x $CONTINUE_PATH -safe_checkout() { - # completely clean TEST_DIR - - cd $SOURCE_DIR - - # cleanup orphaned locks - find .git -type f -name "*.lock" -exec rm {} + - - git reset --hard - git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT - find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; - git reset --hard $GIT_COMMIT - git checkout $GIT_COMMIT - git clean -xdff - git submodule sync - git submodule foreach --recursive "git reset --hard && git clean -xdff" - git submodule update --init --recursive - git submodule foreach --recursive "git reset --hard && git clean -xdff" - - git lfs pull - (ulimit -n 65535 && git lfs prune) - - echo "git checkout done, t=$SECONDS" - du -hs $SOURCE_DIR $SOURCE_DIR/.git - - rsync -a --delete $SOURCE_DIR $TEST_DIR -} - -unsafe_checkout() {( set -e - # checkout directly in test dir, leave old build products - - cd $TEST_DIR - - # cleanup orphaned locks - find .git -type f -name "*.lock" -exec rm {} + - - git fetch --no-tags --no-recurse-submodules -j8 --verbose --depth 1 origin $GIT_COMMIT - git checkout --force --no-recurse-submodules $GIT_COMMIT - git reset --hard $GIT_COMMIT - git clean -dff - git submodule sync - git submodule foreach --recursive "git reset --hard && git clean -df" - git submodule update --init --recursive - git submodule foreach --recursive "git reset --hard && git clean -df" - - git lfs pull - (ulimit -n 65535 && git lfs prune) -)} - -export GIT_PACK_THREADS=8 - # set up environment if [ ! -d "$SOURCE_DIR" ]; then git clone https://github.com/commaai/openpilot.git $SOURCE_DIR fi +cd $SOURCE_DIR -if [ ! -z "$UNSAFE" ]; then - echo "trying unsafe checkout" - set +e - unsafe_checkout - if [[ "$?" -ne 0 ]]; then - safe_checkout - fi - set -e -else - echo "doing safe checkout" - safe_checkout -fi +rm -f .git/index.lock +git reset --hard +git fetch --verbose origin $GIT_COMMIT +find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; +git reset --hard $GIT_COMMIT +git checkout $GIT_COMMIT +git clean -xdf +git submodule update --init --recursive +git submodule foreach --recursive "git reset --hard && git clean -xdf" + +git lfs pull +(ulimit -n 65535 && git lfs prune) + +echo "git checkout done, t=$SECONDS" + +rsync -a --delete $SOURCE_DIR $TEST_DIR echo "$TEST_DIR synced with $GIT_COMMIT, t=$SECONDS" diff --git a/selfdrive/test/setup_vsound.sh b/selfdrive/test/setup_vsound.sh deleted file mode 100755 index aab14997448b4a..00000000000000 --- a/selfdrive/test/setup_vsound.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env bash - -{ - #start pulseaudio daemon - sudo pulseaudio -D - - # create a virtual null audio and set it to default device - sudo pactl load-module module-null-sink sink_name=virtual_audio - sudo pactl set-default-sink virtual_audio -} > /dev/null 2>&1 diff --git a/selfdrive/test/setup_xvfb.sh b/selfdrive/test/setup_xvfb.sh deleted file mode 100755 index 692b84d65f0aea..00000000000000 --- a/selfdrive/test/setup_xvfb.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env bash - -# Sets up a virtual display for running map renderer and simulator without an X11 display - -DISP_ID=99 -export DISPLAY=:$DISP_ID - -sudo Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null & - -# check for x11 socket for the specified display ID -while [ ! -S /tmp/.X11-unix/X$DISP_ID ] -do - echo "Waiting for Xvfb..." - sleep 1 -done - -touch ~/.Xauthority -export XDG_SESSION_TYPE="x11" -xset -q \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py old mode 100644 new mode 100755 index f57751c0674802..c5fc0395d3b4f4 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -1,73 +1,49 @@ -import math +#!/usr/bin/env python3 import json import os -import pytest -import shutil import subprocess import time import numpy as np +import unittest from collections import Counter, defaultdict from pathlib import Path -from tabulate import tabulate -from cereal import log +from cereal import car import cereal.messaging as messaging -from cereal.services import SERVICE_LIST -from openpilot.common.basedir import BASEDIR -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.hw import Paths -from openpilot.tools.lib.logreader import LogReader -from openpilot.tools.lib.log_time_series import msgs_to_time_series - -""" -CPU usage budget -* each process is entitled to at least 8% -* total CPU usage of openpilot (sum(PROCS.values()) - should not exceed MAX_TOTAL_CPU -""" - -TEST_DURATION = 25 -LOG_OFFSET = 8 - -MAX_TOTAL_CPU = 350. # total for all 8 cores +from cereal.services import service_list +from common.basedir import BASEDIR +from common.timeout import Timeout +from common.params import Params +from selfdrive.controls.lib.events import EVENTS, ET +from selfdrive.loggerd.config import ROOT +from selfdrive.test.helpers import set_params_enabled, release_only +from tools.lib.logreader import LogReader + +# Baseline CPU usage by process PROCS = { - # Baseline CPU usage by process - "selfdrive.controls.controlsd": 16.0, - "selfdrive.selfdrived.selfdrived": 16.0, - "selfdrive.car.card": 26.0, - "./loggerd": 14.0, - "./encoderd": 13.0, - "./camerad": 10.0, - "selfdrive.controls.plannerd": 8.0, - "selfdrive.ui.ui": 40.0, - "system.sensord.sensord": 13.0, - "selfdrive.controls.radard": 2.0, - "selfdrive.modeld.modeld": 22.0, - "selfdrive.modeld.dmonitoringmodeld": 18.0, - "system.hardware.hardwared": 4.0, - "selfdrive.locationd.calibrationd": 2.0, - "selfdrive.locationd.torqued": 5.0, - "selfdrive.locationd.locationd": 25.0, + "selfdrive.controls.controlsd": 35.0, + "./loggerd": 10.0, + "./encoderd": 17.0, + "./camerad": 14.5, + "./locationd": 9.1, + "selfdrive.controls.plannerd": 11.7, + "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, - "selfdrive.locationd.lagd": 11.0, - "selfdrive.ui.soundd": 3.0, - "selfdrive.ui.feedback.feedbackd": 1.0, + "./_sensord": 6.17, + "selfdrive.controls.radard": 4.5, + "./_modeld": 4.48, + "./boardd": 3.63, + "./_dmonitoringmodeld": 5.0, + "selfdrive.thermald.thermald": 3.87, + "selfdrive.locationd.calibrationd": 2.0, + "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 4.0, - "system.proclogd": 3.0, - "system.logmessaged": 1.0, - "system.tombstoned": 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, + "./proclogd": 1.54, + "system.logmessaged": 0.2, + "./clocksd": 0.02, + "./ubloxd": 0.02, + "selfdrive.tombstoned": 0, + "./logcatd": 0, } TIMINGS = { @@ -79,376 +55,236 @@ "carState": [2.5, 0.35], "carControl": [2.5, 0.35], "controlsState": [2.5, 0.35], + "lateralPlan": [2.5, 0.5], "longitudinalPlan": [2.5, 0.5], - "driverAssistance": [2.5, 0.5], "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], "driverStateV2": [2.5, 0.40], - "livePose": [2.5, 0.35], - "liveParameters": [2.5, 0.35], + "liveLocationKalman": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } -LOGS_SIZE = { # MB per segment - "qlog.zst": 0.5, - "rlog.zst": 8.1, - "qcamera.ts": 2.3, -} -LOGS_SIZE.update(dict.fromkeys(['ecamera.hevc', 'fcamera.hevc', 'dcamera.hevc'], 76.5)) - def cputime_total(ct): return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem -@pytest.mark.tici -@pytest.mark.skip_tici_setup -class TestOnroad: +def check_cpu_usage(proclogs): + result = "\n" + result += "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + plogs_by_proc = defaultdict(list) + for pl in proclogs: + for x in pl.procLog.procs: + if len(x.cmdline) > 0: + n = list(x.cmdline)[0] + plogs_by_proc[n].append(x) + + print(plogs_by_proc.keys()) + + r = True + dt = (proclogs[-1].logMonoTime - proclogs[0].logMonoTime) / 1e9 + for proc_name, expected_cpu in PROCS.items(): + err = "" + cpu_usage = 0. + x = plogs_by_proc[proc_name] + if len(x) > 2: + cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) + cpu_usage = cpu_time / dt * 100. + if cpu_usage > max(expected_cpu * 1.15, expected_cpu + 5.0): + # cpu usage is high while playing sounds + if not (proc_name == "./_soundd" and cpu_usage < 65.): + err = "using more CPU than normal" + elif cpu_usage < min(expected_cpu * 0.65, max(expected_cpu - 1.0, 0.0)): + err = "using less CPU than normal" + else: + err = "NO METRICS FOUND" + + result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({expected_cpu:5.2f}%) {err}\n" + if len(err) > 0: + r = False + + result += "------------------------------------------------\n" + print(result) + return r + + +class TestOnroad(unittest.TestCase): @classmethod - def setup_class(cls): + def setUpClass(cls): if "DEBUG" in os.environ: - segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog.zst")), Path(Paths.log_root()).iterdir()) + segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(ROOT).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) - cls.lr = list(LogReader(os.path.join(segs[-1], "rlog.zst"))) - cls.ts = msgs_to_time_series(cls.lr) + print(segs[-1]) + cls.lr = list(LogReader(os.path.join(segs[-1], "rlog"))) return # setup env + os.environ['REPLAY'] = "1" + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" + os.environ['LOGPRINT'] = 'debug' + params = Params() - params.remove("CurrentRoute") - params.put_bool("RecordFront", True) + params.clear_all() set_params_enabled() - os.environ['REPLAY'] = '1' - os.environ['MSGQ_PREALLOC'] = '1' - os.environ['TESTING_CLOSET'] = '1' - if os.path.exists(Paths.log_root()): - shutil.rmtree(Paths.log_root()) - # start manager and run openpilot for TEST_DURATION + # Make sure athena isn't running + os.system("pkill -9 -f athena") + + # start manager and run openpilot for a minute proc = None try: - manager_path = os.path.join(BASEDIR, "system/manager/manager.py") - cls.manager_st = time.monotonic() + manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") proc = subprocess.Popen(["python", manager_path]) sm = messaging.SubMaster(['carState']) - with Timeout(30, "controls didn't start"): - while not sm.seen['carState']: + with Timeout(150, "controls didn't start"): + while sm.rcv_frame['carState'] < 0: sm.update(1000) - route = params.get("CurrentRoute") - assert route is not None + # make sure we get at least two full segments + route = None + cls.segments = [] + with Timeout(300, "timed out waiting for logs"): + while route is None: + route = params.get("CurrentRoute", encoding="utf-8") + time.sleep(0.1) + + while len(cls.segments) < 3: + segs = set() + if Path(ROOT).exists(): + segs = set(Path(ROOT).glob(f"{route}--*")) + cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1])) + time.sleep(2) - segs = list(Path(Paths.log_root()).glob(f"{route}--*")) - assert len(segs) == 1 + # chop off last, incomplete segment + cls.segments = cls.segments[:-1] - time.sleep(TEST_DURATION) finally: if proc is not None: proc.terminate() if proc.wait(60) is None: proc.kill() - cls.lr = list(LogReader(os.path.join(str(segs[0]), "rlog.zst"))) - st = time.monotonic() - cls.ts = msgs_to_time_series(cls.lr) - print("msgs to time series", time.monotonic() - st) - log_path = segs[0] + cls.lrs = [list(LogReader(os.path.join(str(s), "rlog"))) for s in cls.segments] - cls.log_sizes = {} - for f in log_path.iterdir(): - assert f.is_file() - cls.log_sizes[f] = f.stat().st_size / 1e6 - - cls.msgs = defaultdict(list) - for m in cls.lr: - cls.msgs[m.which()].append(m) - - def test_service_frequencies(self, subtests): - for s, msgs in self.msgs.items(): - if s in ('initData', 'sentinel'): - continue - - # skip gps services for now - if s in ('ubloxGnss', 'ubloxRaw', 'gnssMeasurements', 'gpsLocation', 'gpsLocationExternal', 'qcomGnss'): - continue - - with subtests.test(service=s): - assert len(msgs) >= math.floor(SERVICE_LIST[s].frequency*int(TEST_DURATION*0.8)) - - def test_manager_starting_time(self): - st = self.ts['managerState']['t'][0] - assert (st - self.manager_st) < 15.0, f"manager.py took {st - self.manager_st}s to publish the first 'managerState' msg" + # use the second segment by default as it's the first full segment + cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog"))) def test_cloudlog_size(self): - msgs = self.msgs['logMessage'] + msgs = [m for m in self.lr if m.which() == 'logMessage'] total_size = sum(len(m.as_builder().to_bytes()) for m in msgs) - assert total_size < 3.5e5 + self.assertLess(total_size, 3.5e5) cnt = Counter(json.loads(m.logMessage)['filename'] for m in msgs) big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] - assert len(big_logs) == 0, f"Log spam: {big_logs}" - - def test_log_sizes(self, subtests): - # TODO: this isn't super stable between different devices - for f, sz in self.log_sizes.items(): - rate = LOGS_SIZE[f.name]/60. - minn = rate * TEST_DURATION * 0.5 - maxx = rate * TEST_DURATION * 1.5 - with subtests.test(file=f.name): - assert minn < sz < maxx - - def test_ui_timings(self): - result = "\n" - result += "------------------------------------------------\n" - result += "-------------- UI Draw Timing ------------------\n" - result += "------------------------------------------------\n" + self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}") - # other processes preempt ui while starting up - offset = int(20 * LOG_OFFSET) - ts = self.ts['uiDebug']['drawTimeMillis'][offset:] - result += f"min {min(ts):.2f}ms\n" - result += f"max {max(ts):.2f}ms\n" - result += f"std {np.std(ts):.2f}ms\n" - result += f"mean {np.mean(ts):.2f}ms\n" - result += "------------------------------------------------\n" - print(result) + def test_cpu_usage(self): + proclogs = [m for m in self.lr if m.which() == 'procLog'] + self.assertGreater(len(proclogs), service_list['procLog'].frequency * 45, "insufficient samples") + cpu_ok = check_cpu_usage(proclogs) + self.assertTrue(cpu_ok) - assert max(ts) < 250. - assert np.mean(ts) < 20. # TODO: ~6-11ms, increase consistency - #self.assertLess(np.std(ts), 5.) - - # some slow frames are expected since camerad/modeld can preempt ui - veryslow = [x for x in ts if x > 40.] - assert len(veryslow) < 5, f"Too many slow frame draw times: {veryslow}" - - def test_cpu_usage(self, subtests): - print("\n------------------------------------------------") - print("------------------ CPU Usage -------------------") - print("------------------------------------------------") - - plogs_by_proc = defaultdict(list) - for pl in self.msgs['procLog']: - for x in pl.procLog.procs: - if len(x.cmdline) > 0: - n = list(x.cmdline)[0] - plogs_by_proc[n].append(x) - - cpu_ok = True - dt = (self.msgs['procLog'][-1].logMonoTime - self.msgs['procLog'][0].logMonoTime) / 1e9 - header = ['process', 'usage', 'expected', 'max allowed', 'test result'] - rows = [] - for proc_name, expected in PROCS.items(): - - error = "" - usage = 0. - x = plogs_by_proc[proc_name] - if len(x) > 2: - cpu_time = cputime_total(x[-1]) - cputime_total(x[0]) - usage = cpu_time / dt * 100. - - max_allowed = max(expected * 1.8, expected + 5.0) - if usage > max_allowed: - error = "❌ USING MORE CPU THAN EXPECTED ❌" - cpu_ok = False - - else: - error = "❌ NO METRICS FOUND ❌" - cpu_ok = False - - rows.append([proc_name, usage, expected, max_allowed, error or "✅"]) - print(tabulate(rows, header, tablefmt="simple_grid", stralign="center", numalign="center", floatfmt=".2f")) - - # Ensure there's no missing procs - all_procs = {p.name for p in self.msgs['managerState'][0].managerState.processes if p.shouldBeRunning} - for p in all_procs: - with subtests.test(proc=p): - assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}" - - # total CPU check - procs_tot = sum([(max(x) if isinstance(x, tuple) else x) for x in PROCS.values()]) - with subtests.test(name="total CPU"): - assert procs_tot < MAX_TOTAL_CPU, "Total CPU budget exceeded" - print("------------------------------------------------") - print(f"Total allocated CPU usage is {procs_tot}%, budget is {MAX_TOTAL_CPU}%, {MAX_TOTAL_CPU-procs_tot:.1f}% left") - print("------------------------------------------------") - - assert cpu_ok - - def test_memory_usage(self): - print("\n------------------------------------------------") - print("--------------- Memory Usage -------------------") - print("------------------------------------------------") - offset = int(SERVICE_LIST['deviceState'].frequency * LOG_OFFSET) - mems = [m.deviceState.memoryUsagePercent for m in self.msgs['deviceState'][offset:]] - print("Overall memory usage: ", mems) - print("MSGQ (/dev/shm/) usage: ", subprocess.check_output(["du", "-hs", "/dev/shm"]).split()[0].decode()) - - # check for big leaks. note that memory usage is - # expected to go up while the MSGQ buffers fill up - assert np.average(mems) <= 80, "Average memory usage too high" - assert np.max(np.diff(mems)) <= 4, "Max memory increase too high" - assert np.average(np.diff(mems)) <= 1, "Average memory increase too high" - - def test_camera_frame_timings(self, subtests): - # test timing within a single camera + def test_camera_processing_time(self): result = "\n" result += "------------------------------------------------\n" - result += "----------------- SOF Timing ------------------\n" + result += "-------------- Debayer Timing ------------------\n" result += "------------------------------------------------\n" - for name in ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']: - ts = self.ts[name]['timestampSof'] - d_ms = np.diff(ts) / 1e6 - d50 = np.abs(d_ms-50) - result += f"{name} sof delta vs 50ms: min {min(d50):.2f}ms\n" - result += f"{name} sof delta vs 50ms: max {max(d50):.2f}ms\n" - result += f"{name} sof delta vs 50ms: mean {d50.mean():.2f}ms\n" - with subtests.test(camera=name): - assert max(d50) < 5.0, f"high SOF delta vs 50ms: {max(d50)}" + + ts = [getattr(getattr(m, m.which()), "processingTime") for m in self.lr if 'CameraState' in m.which()] + self.assertLess(min(ts), 0.025, f"high execution time: {min(ts)}") + result += f"execution time: min {min(ts):.5f}s\n" + result += f"execution time: max {max(ts):.5f}s\n" + result += f"execution time: mean {np.mean(ts):.5f}s\n" result += "------------------------------------------------\n" print(result) - def test_camera_sync(self, subtests): - cam_states = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] - encode_cams = ['roadEncodeIdx', 'wideRoadEncodeIdx', 'driverEncodeIdx'] - for cams in (cam_states, encode_cams): - with subtests.test(cams=cams): - # sanity checks within a single cam - for cam in cams: - with subtests.test(test="frame_skips", camera=cam): - assert set(np.diff(self.ts[cam]['frameId'])) == {1, }, "Frame ID skips" - - # EOF > SOF - eof_sof_diff = self.ts[cam]['timestampEof'] - self.ts[cam]['timestampSof'] - assert np.all(eof_sof_diff > 0) - assert np.all(eof_sof_diff < 50*1e6) - - first_fid = {min(self.ts[c]['frameId']) for c in cams} - assert len(first_fid) == 1, "Cameras don't start on same frame ID" - if cam.endswith('CameraState'): - # camerad guarantees that all cams start on frame ID 0 - # (note loggerd also needs to start up fast enough to catch it) - assert next(iter(first_fid)) < 100, "Cameras start on frame ID too high" - - # we don't do a full segment rotation, so these might not match exactly - last_fid = {max(self.ts[c]['frameId']) for c in cams} - assert max(last_fid) - min(last_fid) < 10 - - start, end = min(first_fid), min(last_fid) - for i in range(end-start): - ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams} - diff = (max(ts.values()) - min(ts.values())) - assert diff < 2, f"Cameras not synced properly: frame_id={start+i}, {diff=:.1f}ms, {ts=}" - - def test_camera_encoder_matches(self, subtests): - # sanity check that the frame metadata is consistent with the encoded frames - pairs = [('roadCameraState', 'roadEncodeIdx'), - ('wideRoadCameraState', 'wideRoadEncodeIdx'), - ('driverCameraState', 'driverEncodeIdx')] - for cam, enc in pairs: - with subtests.test(camera=cam, encoder=enc): - cam_frames = {fid: (sof, eof) for fid, sof, eof in zip( - self.ts[cam]['frameId'], - self.ts[cam]['timestampSof'], - self.ts[cam]['timestampEof'], - strict=True, - )} - for i, fid in enumerate(self.ts[enc]['frameId']): - cam_sof, cam_eof = cam_frames[fid] - enc_sof, enc_eof = self.ts[enc]['timestampSof'][i], self.ts[enc]['timestampEof'][i] - assert enc_sof == cam_sof, f"SOF mismatch: frameId={fid}, enc_sof={enc_sof}, cam_sof={cam_sof}" - assert enc_eof == cam_eof, f"EOF mismatch: frameId={fid}, enc_eof={enc_eof}, cam_eof={cam_eof}" - def test_mpc_execution_timings(self): result = "\n" result += "------------------------------------------------\n" result += "----------------- MPC Timing ------------------\n" result += "------------------------------------------------\n" - cfgs = [("longitudinalPlan", 0.05, 0.05),] + cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(m, s).solverExecutionTime for m in self.msgs[s]] - assert max(ts) < instant_max, f"high '{s}' execution time: {max(ts)}" - assert np.mean(ts) < avg_max, f"high avg '{s}' execution time: {np.mean(ts)}" + ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" result += f"'{s}' execution time: max {max(ts):.5f}s\n" result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" result += "------------------------------------------------\n" print(result) - def test_model_execution_timings(self, subtests): + def test_model_execution_timings(self): result = "\n" result += "------------------------------------------------\n" result += "----------------- Model Timing -----------------\n" result += "------------------------------------------------\n" + # TODO: this went up when plannerd cpu usage increased, why? cfgs = [ - # since multiple processes use the GPU and can preempt each other, - # these numbers are not fully self-contained. - ("modelV2", 0.06, 0.040), - - # can miss cycles here and there, just important the avg frequency is 20Hz - ("driverStateV2", 0.3, 0.05), + ("modelV2", 0.050, 0.036), + ("driverStateV2", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: - ts = [getattr(m, s).modelExecutionTime for m in self.msgs[s]] - # TODO some init can happen in first iteration - ts = ts[1:] + ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] + self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") result += f"'{s}' execution time: min {min(ts):.5f}s\n" result += f"'{s}' execution time: max {max(ts):.5f}s\n" result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" - with subtests.test(s): - assert max(ts) < instant_max, f"high '{s}' execution time: {max(ts)}" - assert np.mean(ts) < avg_max, f"high avg '{s}' execution time: {np.mean(ts)}" result += "------------------------------------------------\n" print(result) def test_timings(self): passed = True - print("\n------------------------------------------------") - print("----------------- Service Timings --------------") - print("------------------------------------------------") - - header = ['service', 'max', 'min', 'mean', 'expected mean', 'rsd', 'max allowed rsd', 'test result'] - rows = [] + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- Service Timings --------------\n" + result += "------------------------------------------------\n" for s, (maxmin, rsd) in TIMINGS.items(): - offset = int(SERVICE_LIST[s].frequency * LOG_OFFSET) - msgs = [m.logMonoTime for m in self.msgs[s][offset:]] + msgs = [m.logMonoTime for m in self.lr if m.which() == s] if not len(msgs): raise Exception(f"missing {s}") ts = np.diff(msgs) / 1e9 - dt = 1 / SERVICE_LIST[s].frequency - - errors = [] - if not np.allclose(np.mean(ts), dt, rtol=0.03, atol=0): - errors.append("❌ FAILED MEAN TIMING CHECK ❌") - if not np.allclose([np.max(ts), np.min(ts)], dt, rtol=maxmin, atol=0): - errors.append("❌ FAILED MAX/MIN TIMING CHECK ❌") - if (np.std(ts)/dt) > rsd: - errors.append("❌ FAILED RSD TIMING CHECK ❌") - passed = not errors and passed - rows.append([s, *(np.array([np.max(ts), np.min(ts), np.mean(ts), dt])*1e3), np.std(ts)/dt, rsd, "\n".join(errors) or "✅"]) - - print(tabulate(rows, header, tablefmt="simple_grid", stralign="center", numalign="center", floatfmt=".2f")) - assert passed + dt = 1 / service_list[s].frequency + + try: + np.testing.assert_allclose(np.mean(ts), dt, rtol=0.03, err_msg=f"{s} - failed mean timing check") + np.testing.assert_allclose([np.max(ts), np.min(ts)], dt, rtol=maxmin, err_msg=f"{s} - failed max/min timing check") + except Exception as e: + result += str(e) + "\n" + passed = False + + if np.std(ts) / dt > rsd: + result += f"{s} - failed RSD timing check\n" + passed = False + + result += f"{s.ljust(40)}: {np.array([np.mean(ts), np.max(ts), np.min(ts)])*1e3}\n" + result += f"{''.ljust(40)} {np.max(np.absolute([np.max(ts)/dt, np.min(ts)/dt]))} {np.std(ts)/dt}\n" + result += "="*67 + print(result) + self.assertTrue(passed) @release_only def test_startup(self): - startup_alert = self.ts['selfdriveState']['alertText1'][0] - expected = EVENTS[log.OnroadEvent.EventName.startup][ET.PERMANENT].alert_text_1 - assert startup_alert == expected, "wrong startup alert" - - def test_engagable(self): - no_entries = Counter() - for m in self.msgs['onroadEvents']: - for evt in m.onroadEvents: - if evt.noEntry: - no_entries[evt.name] += 1 - - offset = int(SERVICE_LIST['selfdriveState'].frequency * LOG_OFFSET) - eng = [m.selfdriveState.engageable for m in self.msgs['selfdriveState'][offset:]] - assert all(eng), \ - f"Not engageable for whole segment:\n- selfdriveState.engageable: {Counter(eng)}\n- No entry events: {no_entries}" + startup_alert = None + for msg in self.lrs[0]: + # can't use carEvents because the first msg can be dropped while loggerd is starting up + if msg.which() == "controlsState": + startup_alert = msg.controlsState.alertText1 + break + expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1 + self.assertEqual(startup_alert, expected, "wrong startup alert") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/test_updated.py b/selfdrive/test/test_updated.py old mode 100644 new mode 100755 index 95365640f5a612..aab8b256acf62d --- a/selfdrive/test/test_updated.py +++ b/selfdrive/test/test_updated.py @@ -1,21 +1,21 @@ +#!/usr/bin/env python3 import datetime import os -import pytest import time import tempfile +import unittest import shutil import signal import subprocess import random -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params +from common.basedir import BASEDIR +from common.params import Params -@pytest.mark.tici -class TestUpdated: +class TestUpdated(unittest.TestCase): - def setup_method(self): + def setUp(self): self.updated_proc = None self.tmp_dir = tempfile.TemporaryDirectory() @@ -57,7 +57,7 @@ def setup_method(self): self.params.clear_all() os.sync() - def teardown_method(self): + def tearDown(self): try: if self.updated_proc is not None: self.updated_proc.terminate() @@ -88,7 +88,7 @@ def _get_updated_proc(self): os.environ["UPDATER_STAGING_ROOT"] = self.staging_dir os.environ["UPDATER_NEOS_VERSION"] = self.neos_version os.environ["UPDATER_NEOSUPDATE_DIR"] = self.neosupdate_dir - updated_path = os.path.join(self.basedir, "system/updated.py") + updated_path = os.path.join(self.basedir, "selfdrive/updated.py") return subprocess.Popen(updated_path, env=os.environ) def _start_updater(self, offroad=True, nosleep=False): @@ -105,7 +105,7 @@ def _read_param(self, key, timeout=1): ret = None start_time = time.monotonic() while ret is None: - ret = self.params.get(key) + ret = self.params.get(key, encoding='utf8') if time.monotonic() - start_time > timeout: break time.sleep(0.01) @@ -162,9 +162,10 @@ def _make_commit(self): def _check_update_state(self, update_available): # make sure LastUpdateTime is recent - last_update_time = self._read_param("LastUpdateTime") - td = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - last_update_time - assert td.total_seconds() < 10 + t = self._read_param("LastUpdateTime") + last_update_time = datetime.datetime.fromisoformat(t) + td = datetime.datetime.utcnow() - last_update_time + self.assertLess(td.total_seconds(), 10) self.params.remove("LastUpdateTime") # wait a bit for the rest of the params to be written @@ -172,13 +173,13 @@ def _check_update_state(self, update_available): # check params update = self._read_param("UpdateAvailable") - assert update == "1" == update_available, f"UpdateAvailable: {repr(update)}" - assert self._read_param("UpdateFailedCount") == 0 + self.assertEqual(update == "1", update_available, f"UpdateAvailable: {repr(update)}") + self.assertEqual(self._read_param("UpdateFailedCount"), "0") # TODO: check that the finalized update actually matches remote # check the .overlay_init and .overlay_consistent flags - assert os.path.isfile(os.path.join(self.basedir, ".overlay_init")) - assert os.path.isfile(os.path.join(self.finalized_dir, ".overlay_consistent")) == update_available + self.assertTrue(os.path.isfile(os.path.join(self.basedir, ".overlay_init"))) + self.assertEqual(os.path.isfile(os.path.join(self.finalized_dir, ".overlay_consistent")), update_available) # *** test cases *** @@ -211,7 +212,7 @@ def test_update(self): self._check_update_state(True) # Let the updater run for 10 cycles, and write an update every cycle - @pytest.mark.skip("need to make this faster") + @unittest.skip("need to make this faster") def test_update_loop(self): self._start_updater() @@ -240,12 +241,12 @@ def test_overlay_reinit(self): # run another cycle, should have a new mtime self._wait_for_update(clear_param=True) second_mtime = os.path.getmtime(overlay_init_fn) - assert first_mtime != second_mtime + self.assertTrue(first_mtime != second_mtime) # run another cycle, mtime should be same as last cycle self._wait_for_update(clear_param=True) new_mtime = os.path.getmtime(overlay_init_fn) - assert second_mtime == new_mtime + self.assertTrue(second_mtime == new_mtime) # Make sure updated exits if another instance is running def test_multiple_instances(self): @@ -257,7 +258,7 @@ def test_multiple_instances(self): # start another instance second_updated = self._get_updated_proc() ret_code = second_updated.wait(timeout=5) - assert ret_code is not None + self.assertTrue(ret_code is not None) # *** test cases with NEOS updates *** @@ -274,10 +275,10 @@ def test_clear_neos_cache(self): self._start_updater() self._wait_for_update(clear_param=True) self._check_update_state(False) - assert not os.path.isdir(self.neosupdate_dir) + self.assertFalse(os.path.isdir(self.neosupdate_dir)) # Let the updater run with no update for a cycle, then write an update - @pytest.mark.skip("TODO: only runs on device") + @unittest.skip("TODO: only runs on device") def test_update_with_neos_update(self): # bump the NEOS version and commit it self._run([ @@ -292,4 +293,8 @@ def test_update_with_neos_update(self): self._check_update_state(True) # TODO: more comprehensive check - assert os.path.isdir(self.neosupdate_dir) + self.assertTrue(os.path.isdir(self.neosupdate_dir)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/test_valgrind_replay.py b/selfdrive/test/test_valgrind_replay.py new file mode 100755 index 00000000000000..238b822ec9c4e9 --- /dev/null +++ b/selfdrive/test/test_valgrind_replay.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +import os +import threading +import time +import unittest +import subprocess +import signal + +if "CI" in os.environ: + def tqdm(x): + return x +else: + from tqdm import tqdm # type: ignore + +import cereal.messaging as messaging +from collections import namedtuple +from tools.lib.logreader import LogReader +from selfdrive.test.openpilotci import get_url +from common.basedir import BASEDIR + +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'command', 'path', 'segment', 'wait_for_response']) + +CONFIGS = [ + ProcessConfig( + proc_name="ubloxd", + pub_sub={ + "ubloxRaw": ["ubloxGnss", "gpsLocationExternal"], + }, + ignore=[], + command="./ubloxd", + path="selfdrive/locationd/", + segment="0375fdf7b1ce594d|2019-06-13--08-32-25--3", + wait_for_response=True + ), +] + + +class TestValgrind(unittest.TestCase): + def extract_leak_sizes(self, log): + if "All heap blocks were freed -- no leaks are possible" in log: + return (0,0,0) + + log = log.replace(",","") # fixes casting to int issue with large leaks + err_lost1 = log.split("definitely lost: ")[1] + err_lost2 = log.split("indirectly lost: ")[1] + err_lost3 = log.split("possibly lost: ")[1] + definitely_lost = int(err_lost1.split(" ")[0]) + indirectly_lost = int(err_lost2.split(" ")[0]) + possibly_lost = int(err_lost3.split(" ")[0]) + return (definitely_lost, indirectly_lost, possibly_lost) + + def valgrindlauncher(self, arg, cwd): + os.chdir(os.path.join(BASEDIR, cwd)) + # Run valgrind on a process + command = "valgrind --leak-check=full " + arg + p = subprocess.Popen(command, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid) # pylint: disable=W1509 + + while not self.replay_done: + time.sleep(0.1) + + # Kill valgrind and extract leak output + os.killpg(os.getpgid(p.pid), signal.SIGINT) + _, err = p.communicate() + error_msg = str(err, encoding='utf-8') + with open(os.path.join(BASEDIR, "selfdrive/test/valgrind_logs.txt"), "a") as f: + f.write(error_msg) + f.write(5 * "\n") + definitely_lost, indirectly_lost, possibly_lost = self.extract_leak_sizes(error_msg) + if max(definitely_lost, indirectly_lost, possibly_lost) > 0: + self.leak = True + print("LEAKS from", arg, "\nDefinitely lost:", definitely_lost, "\nIndirectly lost", indirectly_lost, "\nPossibly lost", possibly_lost) + else: + self.leak = False + + def replay_process(self, config, logreader): + pub_sockets = [s for s in config.pub_sub.keys()] # We dump data from logs here + sub_sockets = [s for _, sub in config.pub_sub.items() for s in sub] # We get responses here + pm = messaging.PubMaster(pub_sockets) + sm = messaging.SubMaster(sub_sockets) + + print("Sorting logs") + all_msgs = sorted(logreader, key=lambda msg: msg.logMonoTime) + pub_msgs = [msg for msg in all_msgs if msg.which() in list(config.pub_sub.keys())] + + thread = threading.Thread(target=self.valgrindlauncher, args=(config.command, config.path)) + thread.daemon = True + thread.start() + + while not all(pm.all_readers_updated(s) for s in config.pub_sub.keys()): + time.sleep(0) + + for msg in tqdm(pub_msgs): + pm.send(msg.which(), msg.as_builder()) + if config.wait_for_response: + sm.update(100) + + self.replay_done = True + + def test_config(self): + open(os.path.join(BASEDIR, "selfdrive/test/valgrind_logs.txt"), "w").close() + + for cfg in CONFIGS: + self.leak = None + self.replay_done = False + + r, n = cfg.segment.rsplit("--", 1) + lr = LogReader(get_url(r, n)) + self.replay_process(cfg, lr) + + while self.leak is None: + time.sleep(0.1) # Wait for the valgrind to finish + + self.assertFalse(self.leak) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py index 54e1c88718f9f8..99a63b8dfd6eff 100755 --- a/selfdrive/test/update_ci_routes.py +++ b/selfdrive/test/update_ci_routes.py @@ -1,55 +1,55 @@ #!/usr/bin/env python3 -import os -import re -import subprocess import sys -from collections.abc import Iterable - -from tqdm import tqdm +import subprocess +from azure.storage.blob import BlockBlobService # pylint: disable=import-error -from opendbc.car.tests.routes import routes as test_car_models_routes -from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments -from openpilot.tools.lib.azure_container import AzureContainer -from openpilot.tools.lib.openpilotcontainers import DataCIContainer, DataProdContainer, OpenpilotCIContainer +from selfdrive.car.tests.routes import routes as test_car_models_routes +from selfdrive.test.process_replay.test_processes import original_segments as replay_segments +from xx.chffr.lib import azureutil # pylint: disable=import-error +from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION # pylint: disable=import-error -SOURCES: list[AzureContainer] = [ - DataProdContainer, - DataCIContainer +SOURCES = [ + (_DATA_ACCOUNT_PRODUCTION, _DATA_BUCKET_PRODUCTION), + (_DATA_ACCOUNT_PRODUCTION, "preserve"), + (_DATA_ACCOUNT_CI, "commadataci"), ] -DEST = OpenpilotCIContainer +DEST_KEY = azureutil.get_user_token(_DATA_ACCOUNT_CI, "openpilotci") +SOURCE_KEYS = [azureutil.get_user_token(account, bucket) for account, bucket in SOURCES] +SERVICE = BlockBlobService(_DATA_ACCOUNT_CI, sas_token=DEST_KEY) -def upload_route(path: str, exclude_patterns: Iterable[str] | None = None) -> None: +def upload_route(path, exclude_patterns=None): if exclude_patterns is None: - exclude_patterns = [r'dcamera\.hevc'] + exclude_patterns = ['*/dcamera.hevc'] r, n = path.rsplit("--", 1) r = '/'.join(r.split('/')[-2:]) # strip out anything extra in the path destpath = f"{r}/{n}" - for file in os.listdir(path): - if any(re.search(pattern, file) for pattern in exclude_patterns): - continue - DEST.upload_file(os.path.join(path, file), f"{destpath}/{file}") - - -def sync_to_ci_public(route: str) -> bool: - dest_container, dest_key = DEST.get_client_and_key() + cmd = [ + "azcopy", + "copy", + f"{path}/*", + f"https://{_DATA_ACCOUNT_CI}.blob.core.windows.net/openpilotci/{destpath}?{DEST_KEY}", + "--recursive=false", + "--overwrite=false", + ] + [f"--exclude-pattern={p}" for p in exclude_patterns] + subprocess.check_call(cmd) + +def sync_to_ci_public(route): key_prefix = route.replace('|', '/') dongle_id = key_prefix.split('/')[0] - if next(dest_container.list_blob_names(name_starts_with=key_prefix), None) is not None: + if next(azureutil.list_all_blobs(SERVICE, "openpilotci", prefix=key_prefix), None) is not None: return True print(f"Uploading {route}") - for source_container in SOURCES: - # assumes az login has been run - print(f"Trying {source_container.ACCOUNT}/{source_container.CONTAINER}") - _, source_key = source_container.get_client_and_key() + for (source_account, source_bucket), source_key in zip(SOURCES, SOURCE_KEYS): + print(f"Trying {source_account}/{source_bucket}") cmd = [ "azcopy", "copy", - f"{source_container.BASE_URL}{key_prefix}?{source_key}", - f"{DEST.BASE_URL}{dongle_id}?{dest_key}", + f"https://{source_account}.blob.core.windows.net/{source_bucket}/{key_prefix}?{source_key}", + f"https://{_DATA_ACCOUNT_CI}.blob.core.windows.net/openpilotci/{dongle_id}?{DEST_KEY}", "--recursive=true", "--overwrite=false", "--exclude-pattern=*/dcamera.hevc", @@ -76,7 +76,7 @@ def sync_to_ci_public(route: str) -> bool: to_sync.extend([rt.route for rt in test_car_models_routes]) to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments]) - for r in tqdm(to_sync): + for r in to_sync: if not sync_to_ci_public(r): failed_routes.append(r) diff --git a/selfdrive/thermald/__init__.py b/selfdrive/thermald/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py new file mode 100644 index 00000000000000..2094faeaa73c04 --- /dev/null +++ b/selfdrive/thermald/fan_controller.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +from abc import ABC, abstractmethod + +from common.realtime import DT_TRML +from common.numpy_fast import interp +from system.swaglog import cloudlog +from selfdrive.controls.lib.pid import PIDController + +class BaseFanController(ABC): + @abstractmethod + def update(self, max_cpu_temp: float, ignition: bool) -> int: + pass + + +class TiciFanController(BaseFanController): + def __init__(self) -> None: + super().__init__() + cloudlog.info("Setting up TICI fan handler") + + self.last_ignition = False + self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + + def update(self, max_cpu_temp: float, ignition: bool) -> int: + self.controller.neg_limit = -(80 if ignition else 30) + self.controller.pos_limit = -(30 if ignition else 0) + + if ignition != self.last_ignition: + self.controller.reset() + + error = 70 - max_cpu_temp + fan_pwr_out = -int(self.controller.update( + error=error, + feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80]) + )) + + self.last_ignition = ignition + return fan_pwr_out + diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py new file mode 100644 index 00000000000000..7834569088d288 --- /dev/null +++ b/selfdrive/thermald/power_monitoring.py @@ -0,0 +1,126 @@ +import threading +from typing import Optional + +from cereal import log +from common.params import Params, put_nonblocking +from common.realtime import sec_since_boot +from system.hardware import HARDWARE +from system.swaglog import cloudlog +from selfdrive.statsd import statlog + +CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) + +# A C2 uses about 1W while idling, and 30h seens like a good shutoff for most cars +# While driving, a battery charges completely in about 30-60 minutes +CAR_BATTERY_CAPACITY_uWh = 30e6 +CAR_CHARGING_RATE_W = 45 + +VBATT_PAUSE_CHARGING = 11.0 # Lower limit on the LPF car battery voltage +VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss +MAX_TIME_OFFROAD_S = 30*3600 +MIN_ON_TIME_S = 3600 + +class PowerMonitoring: + def __init__(self): + self.params = Params() + self.last_measurement_time = None # Used for integration delta + self.last_save_time = 0 # Used for saving current value in a param + self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad + self.next_pulsed_measurement_time = None + self.car_voltage_mV = 12e3 # Low-passed version of peripheralState voltage + self.car_voltage_instant_mV = 12e3 # Last value of peripheralState voltage + self.integration_lock = threading.Lock() + + car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") + if car_battery_capacity_uWh is None: + car_battery_capacity_uWh = 0 + + # Reset capacity if it's low + self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) + + # Calculation tick + def calculate(self, peripheralState, ignition): + try: + now = sec_since_boot() + + # If peripheralState is None, we're probably not in a car, so we don't care + if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown: + with self.integration_lock: + self.last_measurement_time = None + self.next_pulsed_measurement_time = None + self.power_used_uWh = 0 + return + + # Low-pass battery voltage + self.car_voltage_instant_mV = peripheralState.voltage + self.car_voltage_mV = ((peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + statlog.gauge("car_voltage", self.car_voltage_mV / 1e3) + + # Cap the car battery power and save it in a param every 10-ish seconds + self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) + self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh) + if now - self.last_save_time >= 10: + put_nonblocking("CarBatteryCapacity", str(int(self.car_battery_capacity_uWh))) + self.last_save_time = now + + # First measurement, set integration time + with self.integration_lock: + if self.last_measurement_time is None: + self.last_measurement_time = now + return + + if ignition: + # If there is ignition, we integrate the charging rate of the car + with self.integration_lock: + self.power_used_uWh = 0 + integration_time_h = (now - self.last_measurement_time) / 3600 + if integration_time_h < 0: + raise ValueError(f"Negative integration time: {integration_time_h}h") + self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h) + self.last_measurement_time = now + else: + # Get current power draw somehow + current_power = HARDWARE.get_current_power_draw() + + # Do the integration + self._perform_integration(now, current_power) + except Exception: + cloudlog.exception("Power monitoring calculation failed") + + def _perform_integration(self, t: float, current_power: float) -> None: + with self.integration_lock: + try: + if self.last_measurement_time: + integration_time_h = (t - self.last_measurement_time) / 3600 + power_used = (current_power * 1000000) * integration_time_h + if power_used < 0: + raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") + self.power_used_uWh += power_used + self.car_battery_capacity_uWh -= power_used + self.last_measurement_time = t + except Exception: + cloudlog.exception("Integration failed") + + # Get the power usage + def get_power_used(self) -> int: + return int(self.power_used_uWh) + + def get_car_battery_capacity(self) -> int: + return int(self.car_battery_capacity_uWh) + + # See if we need to shutdown + def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: Optional[float], started_seen: bool): + if offroad_timestamp is None: + return False + + now = sec_since_boot() + should_shutdown = False + should_shutdown |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S + should_shutdown |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) + should_shutdown |= (self.car_battery_capacity_uWh <= 0) + should_shutdown &= not ignition + should_shutdown &= (not self.params.get_bool("DisablePowerDown")) + should_shutdown &= in_car + should_shutdown |= self.params.get_bool("ForcePowerDown") + should_shutdown &= started_seen or (now > MIN_ON_TIME_S) + return should_shutdown diff --git a/selfdrive/thermald/tests/__init__.py b/selfdrive/thermald/tests/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/thermald/tests/test_fan_controller.py b/selfdrive/thermald/tests/test_fan_controller.py new file mode 100755 index 00000000000000..857866f64eac60 --- /dev/null +++ b/selfdrive/thermald/tests/test_fan_controller.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 +import unittest +from unittest.mock import Mock, patch +from parameterized import parameterized + +from selfdrive.thermald.fan_controller import TiciFanController + +ALL_CONTROLLERS = [(TiciFanController,)] + +def patched_controller(controller_class): + with patch("os.system", new=Mock()): + return controller_class() + +class TestFanController(unittest.TestCase): + def wind_up(self, controller, ignition=True): + for _ in range(1000): + controller.update(max_cpu_temp=100, ignition=ignition) + + def wind_down(self, controller, ignition=False): + for _ in range(1000): + controller.update(max_cpu_temp=10, ignition=ignition) + + @parameterized.expand(ALL_CONTROLLERS) + def test_hot_onroad(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller) + self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 70) + + @parameterized.expand(ALL_CONTROLLERS) + def test_offroad_limits(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller) + self.assertLessEqual(controller.update(max_cpu_temp=100, ignition=False), 30) + + @parameterized.expand(ALL_CONTROLLERS) + def test_no_fan_wear(self, controller_class): + controller = patched_controller(controller_class) + self.wind_down(controller) + self.assertEqual(controller.update(max_cpu_temp=10, ignition=False), 0) + + @parameterized.expand(ALL_CONTROLLERS) + def test_limited(self, controller_class): + controller = patched_controller(controller_class) + self.wind_up(controller, ignition=True) + self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 80) + + @parameterized.expand(ALL_CONTROLLERS) + def test_windup_speed(self, controller_class): + controller = patched_controller(controller_class) + self.wind_down(controller, ignition=True) + for _ in range(10): + controller.update(max_cpu_temp=90, ignition=True) + self.assertGreaterEqual(controller.update(max_cpu_temp=90, ignition=True), 60) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py new file mode 100755 index 00000000000000..5d7463d455d1f9 --- /dev/null +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python3 +import unittest +from unittest.mock import patch +from parameterized import parameterized + +from cereal import log +import cereal.messaging as messaging +from common.params import Params +params = Params() + +# Create fake time +ssb = 0 +def mock_sec_since_boot(): + global ssb + ssb += 1 + return ssb + +with patch("common.realtime.sec_since_boot", new=mock_sec_since_boot): + with patch("common.params.put_nonblocking", new=params.put): + from selfdrive.thermald.power_monitoring import PowerMonitoring, CAR_BATTERY_CAPACITY_uWh, \ + CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING + +TEST_DURATION_S = 50 +ALL_PANDA_TYPES = [(log.PandaState.PandaType.dos,)] + +def pm_patch(name, value, constant=False): + if constant: + return patch(f"selfdrive.thermald.power_monitoring.{name}", value) + return patch(f"selfdrive.thermald.power_monitoring.{name}", return_value=value) + +class TestPowerMonitoring(unittest.TestCase): + def setUp(self): + # Clear stored capacity before each test + params.remove("CarBatteryCapacity") + params.remove("DisablePowerDown") + + def mock_peripheralState(self, hw_type, car_voltage=12): + ps = messaging.new_message('peripheralState').peripheralState + ps.pandaType = hw_type + ps.voltage = car_voltage * 1e3 + return ps + + # Test to see that it doesn't do anything when pandaState is None + def test_pandaState_present(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(None, None) + self.assertEqual(pm.get_power_used(), 0) + self.assertEqual(pm.get_car_battery_capacity(), (CAR_BATTERY_CAPACITY_uWh / 10)) + + # Test to see that it doesn't integrate offroad when ignition is True + @parameterized.expand(ALL_PANDA_TYPES) + def test_offroad_ignition(self, hw_type): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(self.mock_peripheralState(hw_type), True) + self.assertEqual(pm.get_power_used(), 0) + + # Test to see that it integrates with discharging battery + @parameterized.expand(ALL_PANDA_TYPES) + def test_offroad_integration_discharging(self, hw_type): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_peripheralState(hw_type), False) + expected_power_usage = ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) + + # Test to check positive integration of car_battery_capacity + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_onroad(self, hw_type): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_peripheralState(hw_type), True) + expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check positive integration upper limit + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_upper_limit(self, hw_type): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_peripheralState(hw_type), True) + estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check negative integration of car_battery_capacity + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_offroad(self, hw_type): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_peripheralState(hw_type), False) + expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check negative integration lower limit + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_lower_limit(self, hw_type): + POWER_DRAW = 4 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_peripheralState(hw_type), False) + estimated_capacity = 0 - ((1/3600) * POWER_DRAW * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S + @parameterized.expand(ALL_PANDA_TYPES) + def test_max_time_offroad(self, hw_type): + MOCKED_MAX_OFFROAD_TIME = 3600 + POWER_DRAW = 0 # To stop shutting down for other reasons + with pm_patch("MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True), pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + start_time = ssb + ignition = False + peripheralState = self.mock_peripheralState(hw_type) + while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: + pm.calculate(peripheralState, ignition) + if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: + self.assertFalse(pm.should_shutdown(ignition, True, start_time, False)) + self.assertTrue(pm.should_shutdown(ignition, True, start_time, False)) + + # Test to check policy of stopping charging when the car voltage is too low + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_voltage(self, hw_type): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + peripheralState = self.mock_peripheralState(hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(peripheralState, ignition) + if i % 10 == 0: + self.assertEqual(pm.should_shutdown(ignition, True, ssb, True), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3)) + self.assertTrue(pm.should_shutdown(ignition, True, ssb, True)) + + # Test to check policy of not stopping charging when DisablePowerDown is set + def test_disable_power_down(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + params.put_bool("DisablePowerDown", True) + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(peripheralState, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + + # Test to check policy of not stopping charging when ignition + def test_ignition(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = True + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(peripheralState, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, True, ssb, False)) + + # Test to check policy of not stopping charging when harness is not connected + def test_harness_connection(self): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("HARDWARE.get_current_power_draw", POWER_DRAW): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + + ignition = False + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(peripheralState, ignition) + if i % 10 == 0: + self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) + self.assertFalse(pm.should_shutdown(ignition, False, ssb, False)) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py new file mode 100755 index 00000000000000..5c2fbd68253573 --- /dev/null +++ b/selfdrive/thermald/thermald.py @@ -0,0 +1,426 @@ +#!/usr/bin/env python3 +import datetime +import os +import queue +import threading +import time +from collections import OrderedDict, namedtuple +from pathlib import Path +from typing import Dict, Optional, Tuple + +import psutil + +import cereal.messaging as messaging +from cereal import log +from common.dict_helpers import strip_deprecated_keys +from common.filter_simple import FirstOrderFilter +from common.params import Params +from common.realtime import DT_TRML, sec_since_boot +from selfdrive.controls.lib.alertmanager import set_offroad_alert +from system.hardware import HARDWARE, TICI, AGNOS +from selfdrive.loggerd.config import get_available_percent +from selfdrive.statsd import statlog +from system.swaglog import cloudlog +from selfdrive.thermald.power_monitoring import PowerMonitoring +from selfdrive.thermald.fan_controller import TiciFanController +from system.version import terms_version, training_version + +ThermalStatus = log.DeviceState.ThermalStatus +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength +CURRENT_TAU = 15. # 15s time constant +TEMP_TAU = 5. # 5s time constant +DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert +PANDA_STATES_TIMEOUT = int(1000 * 1.5 * DT_TRML) # 1.5x the expected pandaState frequency + +ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) +HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', 'network_metered', 'nvme_temps', 'modem_temps']) + +# List of thermal bands. We will stay within this region as long as we are within the bounds. +# When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict. +THERMAL_BANDS = OrderedDict({ + ThermalStatus.green: ThermalBand(None, 80.0), + ThermalStatus.yellow: ThermalBand(75.0, 96.0), + ThermalStatus.red: ThermalBand(80.0, 107.), + ThermalStatus.danger: ThermalBand(94.0, None), +}) + +# Override to highest thermal band when offroad and above this temp +OFFROAD_DANGER_TEMP = 79.5 + +prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} + +tz_by_type: Optional[Dict[str, int]] = None +def populate_tz_by_type(): + global tz_by_type + tz_by_type = {} + for n in os.listdir("/sys/devices/virtual/thermal"): + if not n.startswith("thermal_zone"): + continue + with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f: + tz_by_type[f.read().strip()] = int(n.lstrip("thermal_zone")) + +def read_tz(x): + if x is None: + return 0 + + if isinstance(x, str): + if tz_by_type is None: + populate_tz_by_type() + x = tz_by_type[x] + + try: + with open(f"/sys/devices/virtual/thermal/thermal_zone{x}/temp") as f: + return int(f.read()) + except FileNotFoundError: + return 0 + + +def read_thermal(thermal_config): + dat = messaging.new_message('deviceState') + dat.deviceState.cpuTempC = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]] + dat.deviceState.gpuTempC = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]] + dat.deviceState.memoryTempC = read_tz(thermal_config.mem[0]) / thermal_config.mem[1] + dat.deviceState.ambientTempC = read_tz(thermal_config.ambient[0]) / thermal_config.ambient[1] + dat.deviceState.pmicTempC = [read_tz(z) / thermal_config.pmic[1] for z in thermal_config.pmic[0]] + return dat + + +def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None): + if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): + return + prev_offroad_states[offroad_alert] = (show_alert, extra_text) + set_offroad_alert(offroad_alert, show_alert, extra_text) + + +def hw_state_thread(end_event, hw_queue): + """Handles non critical hardware state, and sends over queue""" + count = 0 + prev_hw_state = None + + modem_version = None + modem_nv = None + modem_configured = False + + while not end_event.is_set(): + # these are expensive calls. update every 10s + if (count % int(10. / DT_TRML)) == 0: + try: + network_type = HARDWARE.get_network_type() + modem_temps = HARDWARE.get_modem_temperatures() + if len(modem_temps) == 0 and prev_hw_state is not None: + modem_temps = prev_hw_state.modem_temps + + # Log modem version once + if AGNOS and ((modem_version is None) or (modem_nv is None)): + modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none + modem_nv = HARDWARE.get_modem_nv() # pylint: disable=assignment-from-none + + if (modem_version is not None) and (modem_nv is not None): + cloudlog.event("modem version", version=modem_version, nv=modem_nv) + + tx, rx = HARDWARE.get_modem_data_usage() + + hw_state = HardwareState( + network_type=network_type, + network_info=HARDWARE.get_network_info(), + network_strength=HARDWARE.get_network_strength(network_type), + network_stats={'wwanTx': tx, 'wwanRx': rx}, + network_metered=HARDWARE.get_network_metered(network_type), + nvme_temps=HARDWARE.get_nvme_temperatures(), + modem_temps=modem_temps, + ) + + try: + hw_queue.put_nowait(hw_state) + except queue.Full: + pass + + # TODO: remove this once the config is in AGNOS + if not modem_configured and len(HARDWARE.get_sim_info().get('sim_id', '')) > 0: + cloudlog.warning("configuring modem") + HARDWARE.configure_modem() + modem_configured = True + + prev_hw_state = hw_state + except Exception: + cloudlog.exception("Error getting hardware state") + + count += 1 + time.sleep(DT_TRML) + + +def thermald_thread(end_event, hw_queue): + pm = messaging.PubMaster(['deviceState']) + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"]) + + count = 0 + + onroad_conditions: Dict[str, bool] = { + "ignition": False, + } + startup_conditions: Dict[str, bool] = {} + startup_conditions_prev: Dict[str, bool] = {} + + off_ts = None + started_ts = None + started_seen = False + thermal_status = ThermalStatus.green + + last_hw_state = HardwareState( + network_type=NetworkType.none, + network_info=None, + network_metered=False, + network_strength=NetworkStrength.unknown, + network_stats={'wwanTx': -1, 'wwanRx': -1}, + nvme_temps=[], + modem_temps=[], + ) + + temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) + should_start_prev = False + in_car = False + engaged_prev = False + + params = Params() + power_monitor = PowerMonitoring() + + HARDWARE.initialize_hardware() + thermal_config = HARDWARE.get_thermal_config() + + fan_controller = None + + while not end_event.is_set(): + sm.update(PANDA_STATES_TIMEOUT) + + pandaStates = sm['pandaStates'] + peripheralState = sm['peripheralState'] + + msg = read_thermal(thermal_config) + + if sm.updated['pandaStates'] and len(pandaStates) > 0: + + # Set ignition based on any panda connected + onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown) + + pandaState = pandaStates[0] + + in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected + + # Setup fan handler on first connect to panda + if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: + if TICI: + fan_controller = TiciFanController() + + elif (sec_since_boot() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT: + if onroad_conditions["ignition"]: + onroad_conditions["ignition"] = False + cloudlog.error("panda timed out onroad") + + try: + last_hw_state = hw_queue.get_nowait() + except queue.Empty: + pass + + msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) + msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) + msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] + msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) + + msg.deviceState.networkType = last_hw_state.network_type + msg.deviceState.networkMetered = last_hw_state.network_metered + msg.deviceState.networkStrength = last_hw_state.network_strength + msg.deviceState.networkStats = last_hw_state.network_stats + if last_hw_state.network_info is not None: + msg.deviceState.networkInfo = last_hw_state.network_info + + msg.deviceState.nvmeTempC = last_hw_state.nvme_temps + msg.deviceState.modemTempC = last_hw_state.modem_temps + + msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() + + max_comp_temp = temp_filter.update( + max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) + ) + + if fan_controller is not None: + msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"]) + + is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) + if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: + # If device is offroad we want to cool down before going onroad + # since going onroad increases load and can make temps go over 107 + thermal_status = ThermalStatus.danger + else: + current_band = THERMAL_BANDS[thermal_status] + band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) + if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: + thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] + elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: + thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] + + # **** starting logic **** + + # Ensure date/time are valid + now = datetime.datetime.utcnow() + startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10) + set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) + + startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") + startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") + startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version + + # with 2% left, we killall, otherwise the phone will take a long time to boot + startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 + startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ + params.get_bool("Passive") + startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled") + startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot") + # if any CPU gets above 107 or the battery gets above 63, kill all processes + # controls will warn with CPU above 95 or battery above 60 + onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger + set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"])) + + # TODO: this should move to TICI.initialize_hardware, but we currently can't import params there + if 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) + else: + # check for bad NVMe + try: + with open("/sys/block/nvme0n1/device/model") as f: + model = f.read().strip() + if not model.startswith("Samsung SSD 980") and params.get("Offroad_BadNvme") is None: + set_offroad_alert_if_changed("Offroad_BadNvme", True) + cloudlog.event("Unsupported NVMe", model=model, error=True) + except Exception: + pass + + # Handle offroad/onroad transition + should_start = all(onroad_conditions.values()) + if started_ts is None: + should_start = should_start and all(startup_conditions.values()) + + if should_start != should_start_prev or (count == 0): + params.put_bool("IsOnroad", should_start) + params.put_bool("IsOffroad", not should_start) + + params.put_bool("IsEngaged", False) + engaged_prev = False + HARDWARE.set_power_save(not should_start) + + if sm.updated['controlsState']: + engaged = sm['controlsState'].enabled + if engaged != engaged_prev: + params.put_bool("IsEngaged", engaged) + engaged_prev = engaged + + try: + with open('/dev/kmsg', 'w') as kmsg: + kmsg.write(f"<3>[thermald] engaged: {engaged}\n") + except Exception: + pass + + if should_start: + off_ts = None + if started_ts is None: + started_ts = sec_since_boot() + started_seen = True + else: + if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): + cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions) + + started_ts = None + if off_ts is None: + off_ts = sec_since_boot() + + # Offroad power monitoring + power_monitor.calculate(peripheralState, onroad_conditions["ignition"]) + msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() + msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) + current_power_draw = HARDWARE.get_current_power_draw() + statlog.sample("power_draw", current_power_draw) + msg.deviceState.powerDrawW = current_power_draw + + som_power_draw = HARDWARE.get_som_power_draw() + statlog.sample("som_power_draw", som_power_draw) + msg.deviceState.somPowerDrawW = som_power_draw + + # Check if we need to shut down + if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen): + cloudlog.warning(f"shutting device down, offroad since {off_ts}") + params.put_bool("DoShutdown", True) + + msg.deviceState.started = started_ts is not None + msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) + + last_ping = params.get("LastAthenaPingTime") + if last_ping is not None: + msg.deviceState.lastAthenaPingTime = int(last_ping) + + msg.deviceState.thermalStatus = thermal_status + pm.send("deviceState", msg) + + should_start_prev = should_start + startup_conditions_prev = startup_conditions.copy() + + # Log to statsd + statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) + statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) + statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent) + for i, usage in enumerate(msg.deviceState.cpuUsagePercent): + statlog.gauge(f"cpu{i}_usage_percent", usage) + for i, temp in enumerate(msg.deviceState.cpuTempC): + statlog.gauge(f"cpu{i}_temperature", temp) + for i, temp in enumerate(msg.deviceState.gpuTempC): + statlog.gauge(f"gpu{i}_temperature", temp) + statlog.gauge("memory_temperature", msg.deviceState.memoryTempC) + statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC) + for i, temp in enumerate(msg.deviceState.pmicTempC): + statlog.gauge(f"pmic{i}_temperature", temp) + for i, temp in enumerate(last_hw_state.nvme_temps): + statlog.gauge(f"nvme_temperature{i}", temp) + for i, temp in enumerate(last_hw_state.modem_temps): + statlog.gauge(f"modem_temperature{i}", temp) + statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired) + statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent) + + # report to server once every 10 minutes + if (count % int(600. / DT_TRML)) == 0: + cloudlog.event("STATUS_PACKET", + count=count, + pandaStates=[strip_deprecated_keys(p.to_dict()) for p in pandaStates], + peripheralState=strip_deprecated_keys(peripheralState.to_dict()), + location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), + deviceState=strip_deprecated_keys(msg.to_dict())) + + count += 1 + + +def main(): + hw_queue = queue.Queue(maxsize=1) + end_event = threading.Event() + + threads = [ + threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), + threading.Thread(target=thermald_thread, args=(end_event, hw_queue)), + ] + + for t in threads: + t.start() + + try: + while True: + time.sleep(1) + if not all(t.is_alive() for t in threads): + break + finally: + end_event.set() + + for t in threads: + t.join() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py new file mode 100755 index 00000000000000..0045e0766c0895 --- /dev/null +++ b/selfdrive/tombstoned.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +import datetime +import os +import re +import shutil +import signal +import subprocess +import time +import glob +from typing import NoReturn + +from common.file_helpers import mkdirs_exists_ok +from selfdrive.loggerd.config import ROOT +import selfdrive.sentry as sentry +from system.swaglog import cloudlog +from system.version import get_commit + +MAX_SIZE = 1_000_000 * 100 # allow up to 100M +MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") + +TOMBSTONE_DIR = "/data/tombstones/" +APPORT_DIR = "/var/crash/" + + +def safe_fn(s): + extra = ['_'] + return "".join(c for c in s if c.isalnum() or c in extra).rstrip() + + +def clear_apport_folder(): + for f in glob.glob(APPORT_DIR + '*'): + try: + os.remove(f) + except Exception: + pass + + +def get_apport_stacktrace(fn): + try: + cmd = f'apport-retrace -s <(cat <(echo "Package: openpilot") "{fn}")' + return subprocess.check_output(cmd, shell=True, encoding='utf8', timeout=30, executable='/bin/bash') # pylint: disable=unexpected-keyword-arg + except subprocess.CalledProcessError: + return "Error getting stacktrace" + except subprocess.TimeoutExpired: + return "Timeout getting stacktrace" + + +def get_tombstones(): + """Returns list of (filename, ctime) for all tombstones in /data/tombstones + and apport crashlogs in /var/crash""" + files = [] + for folder in [TOMBSTONE_DIR, APPORT_DIR]: + if os.path.exists(folder): + with os.scandir(folder) as d: + + # Loop over first 1000 directory entries + for _, f in zip(range(1000), d): + if f.name.startswith("tombstone"): + files.append((f.path, int(f.stat().st_ctime))) + elif f.name.endswith(".crash") and f.stat().st_mode == 0o100640: + files.append((f.path, int(f.stat().st_ctime))) + return files + + +def report_tombstone_android(fn): + f_size = os.path.getsize(fn) + if f_size > MAX_SIZE: + cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") + return + + with open(fn, encoding='ISO-8859-1') as f: + contents = f.read() + + message = " ".join(contents.split('\n')[5:7]) + + # Cut off pid/tid, since that varies per run + name_idx = message.find('name') + if name_idx >= 0: + message = message[name_idx:] + + executable = "" + start_exe_idx = message.find('>>> ') + end_exe_idx = message.find(' <<<') + if start_exe_idx >= 0 and end_exe_idx >= 0: + executable = message[start_exe_idx + 4:end_exe_idx] + + # Cut off fault addr + fault_idx = message.find(', fault addr') + if fault_idx >= 0: + message = message[:fault_idx] + + sentry.report_tombstone(fn, message, contents) + + # Copy crashlog to upload folder + clean_path = executable.replace('./', '').replace('/', '_') + date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") + + new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] + + crashlog_dir = os.path.join(ROOT, "crash") + mkdirs_exists_ok(crashlog_dir) + + shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) + + +def report_tombstone_apport(fn): + f_size = os.path.getsize(fn) + if f_size > MAX_SIZE: + cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") + return + + message = "" # One line description of the crash + contents = "" # Full file contents without coredump + path = "" # File path relative to openpilot directory + + proc_maps = False + + with open(fn) as f: + for line in f: + if "CoreDump" in line: + break + elif "ProcMaps" in line: + proc_maps = True + elif "ProcStatus" in line: + proc_maps = False + + if not proc_maps: + contents += line + + if "ExecutablePath" in line: + path = line.strip().split(': ')[-1] + path = path.replace('/data/openpilot/', '') + message += path + elif "Signal" in line: + message += " - " + line.strip() + + try: + sig_num = int(line.strip().split(': ')[-1]) + message += " (" + signal.Signals(sig_num).name + ")" # pylint: disable=no-member + except ValueError: + pass + + stacktrace = get_apport_stacktrace(fn) + stacktrace_s = stacktrace.split('\n') + crash_function = "No stacktrace" + + if len(stacktrace_s) > 2: + found = False + + # Try to find first entry in openpilot, fall back to first line + for line in stacktrace_s: + if "at selfdrive/" in line: + crash_function = line + found = True + break + + if not found: + crash_function = stacktrace_s[1] + + # Remove arguments that can contain pointers to make sentry one-liner unique + crash_function = " ".join(x for x in crash_function.split(' ')[1:] if not x.startswith('0x')) + crash_function = re.sub(r'\(.*?\)', '', crash_function) + + contents = stacktrace + "\n\n" + contents + message = message + " - " + crash_function + sentry.report_tombstone(fn, message, contents) + + # Copy crashlog to upload folder + clean_path = path.replace('/', '_') + date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") + + new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] + + crashlog_dir = os.path.join(ROOT, "crash") + mkdirs_exists_ok(crashlog_dir) + + # Files could be on different filesystems, copy, then delete + shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) + + try: + os.remove(fn) + except PermissionError: + pass + + +def main() -> NoReturn: + sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE) + + # Clear apport folder on start, otherwise duplicate crashes won't register + clear_apport_folder() + initial_tombstones = set(get_tombstones()) + + while True: + now_tombstones = set(get_tombstones()) + + for fn, _ in (now_tombstones - initial_tombstones): + try: + cloudlog.info(f"reporting new tombstone {fn}") + if fn.endswith(".crash"): + report_tombstone_apport(fn) + else: + report_tombstone_android(fn) + except Exception: + cloudlog.exception(f"Error reporting tombstone {fn}") + + initial_tombstones = now_tombstones + time.sleep(5) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore index 945928f6178b7b..e5b27adce537fe 100644 --- a/selfdrive/ui/.gitignore +++ b/selfdrive/ui/.gitignore @@ -1 +1,12 @@ +moc_* +*.moc + +_mui +watch3 installer/installers/* +qt/text +qt/spinner +qt/setup/setup +qt/setup/reset +qt/setup/wifi +qt/setup/updater diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 0de3e13c011b81..c62a6b19d9eab6 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,71 +1,123 @@ import os -import re -import json -from pathlib import Path -Import('env', 'arch', 'common') - -# build the fonts -generator = File("#selfdrive/assets/fonts/process.py") -source_files = Glob("#selfdrive/assets/fonts/*.ttf") + Glob("#selfdrive/assets/fonts/*.otf") -output_files = [ - (f.abspath.split('.')[0] + ".fnt", f.abspath.split('.')[0] + ".png") - for f in source_files - if "NotoColor" not in f.name -] -env.Command( - target=output_files, - source=[generator, source_files], - action=f"python3 {generator}", -) - -# compile gettext .po -> .mo translations -with open(File("translations/languages.json").abspath) as f: - languages = json.loads(f.read()) - -po_sources = [f"#selfdrive/ui/translations/app_{l}.po" for l in languages.values()] -po_sources = [src for src in po_sources if os.path.exists(File(src).abspath)] -mo_targets = [src.replace(".po", ".mo") for src in po_sources] -mo_build = [] -for src, tgt in zip(po_sources, mo_targets): - mo_build.append(env.Command(tgt, src, "msgfmt -o $TARGET $SOURCE")) -mo_alias = env.Alias('mo', mo_build) -env.AlwaysBuild(mo_alias) +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', + 'cereal', 'transformations') + +base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', + 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] + +if arch == 'larch64': + base_libs.append('EGL') + +maps = arch in ['larch64', 'x86_64'] + +if maps and arch == 'x86_64': + rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath] + qt_env["RPATH"] += rpath + +if arch == "Darwin": + del base_libs[base_libs.index('OpenCL')] + qt_env['FRAMEWORKS'] += ['OpenCL'] + +qt_util = qt_env.Library("qt_util", ["#selfdrive/ui/qt/api.cc", "#selfdrive/ui/qt/util.cc"], LIBS=base_libs) +widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/drive_stats.cc", + "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", + "qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc", + "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc", + "qt/request_repeater.cc", "qt/qt_window.cc", "qt/offroad/networking.cc", "qt/offroad/wifiManager.cc"] + +qt_env['CPPDEFINES'] = [] +if maps: + base_libs += ['qmapboxgl'] + widgets_src += ["qt/maps/map_helpers.cc", "qt/maps/map_settings.cc", "qt/maps/map.cc"] + qt_env['CPPDEFINES'] += ["ENABLE_MAPS"] + +widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) +qt_libs = [widgets, qt_util] + base_libs + +# build assets +assets = "#selfdrive/assets/assets.cc" +assets_src = "#selfdrive/assets/assets.qrc" +qt_env.Command(assets, assets_src, f"rcc $SOURCES -o $TARGET") +qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, "#selfdrive/assets/assets.o"])) +asset_obj = qt_env.Object("assets", assets) + +# build soundd +qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) +if GetOption('test'): + qt_env.Program("tests/playsound", "tests/playsound.cc", LIBS=base_libs) + qt_env.Program('tests/test_sound', ['tests/test_runner.cc', 'soundd/sound.cc', 'tests/test_sound.cc'], LIBS=qt_libs) + +qt_env.SharedLibrary("qt/python_helpers", ["qt/qt_window.cc"], LIBS=qt_libs) + +# spinner and text window +qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs) +qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs) + +# build main UI +qt_src = ["main.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/body.cc", + "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", + "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc"] +qt_env.Program("_ui", qt_src + [asset_obj], LIBS=qt_libs) +if GetOption('test'): + qt_src.remove("main.cc") # replaced by test_runner + qt_env.Program('tests/test_translations', [asset_obj, 'tests/test_runner.cc', 'tests/test_translations.cc'] + qt_src, LIBS=qt_libs) + + +# build translation files +translation_sources = Glob("#selfdrive/ui/translations/*.ts", strings=True) +translation_targets = [src.replace(".ts", ".qm") for src in translation_sources] +lrelease = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease' +qt_env.Command(translation_targets, translation_sources, f"{lrelease} $SOURCES") + + +# setup and factory resetter +if GetOption('extras'): + qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs) + qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc", asset_obj], + LIBS=qt_libs + ['curl', 'common', 'json11']) + if GetOption('extras'): + # build updater UI + qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs) + + # build mui + qt_env.Program("_mui", ["mui.cc"], LIBS=qt_libs) + # build installers - if arch != "Darwin": - raylib_env = env.Clone() - raylib_env['LIBPATH'] += [f'#third_party/raylib/{arch}/'] - raylib_env['LINKFLAGS'].append('-Wl,-strip-debug') - - raylib_libs = common + ["raylib"] - if arch == "larch64": - raylib_libs += ["GLESv2", "EGL", "gbm", "drm"] - else: - raylib_libs += ["GL"] - - release = "release3" - installers = [ - ("openpilot", release), - ("openpilot_test", f"{release}-staging"), - ("openpilot_nightly", "nightly"), - ("openpilot_internal", "nightly-dev"), - ] - - cont = raylib_env.Command("installer/continue_openpilot.o", "installer/continue_openpilot.sh", - "ld -r -b binary -o $TARGET $SOURCE") - inter = raylib_env.Command("installer/inter_ttf.o", "installer/inter-ascii.ttf", + senv = qt_env.Clone() + senv['LINKFLAGS'].append('-Wl,-strip-debug') + + release = "release3" + dashcam = "dashcam3" + installers = [ + ("openpilot", release), + ("openpilot_test", f"{release}-staging"), + ("openpilot_nightly", "master-ci"), + ("openpilot_internal", "master"), + ("dashcam", dashcam), + ("dashcam_test", f"{dashcam}-staging"), + ] + + cont = {} + for brand in ("openpilot", "dashcam"): + cont[brand] = senv.Command(f"installer/continue_{brand}.o", f"installer/continue_{brand}.sh", "ld -r -b binary -o $TARGET $SOURCE") - inter_bold = raylib_env.Command("installer/inter_bold.o", "../assets/fonts/Inter-Bold.ttf", - "ld -r -b binary -o $TARGET $SOURCE") - inter_light = raylib_env.Command("installer/inter_light.o", "../assets/fonts/Inter-Light.ttf", - "ld -r -b binary -o $TARGET $SOURCE") - for name, branch in installers: - d = {'BRANCH': f"'\"{branch}\"'"} - if "internal" in name: - d['INTERNAL'] = "1" - - obj = raylib_env.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d) - f = raylib_env.Program(f"installer/installers/installer_{name}", [obj, cont, inter, inter_bold, inter_light], LIBS=raylib_libs) - # keep installers small - assert f[0].get_size() < 1900*1e3, f[0].get_size() + for name, branch in installers: + brand = "dashcam" if "dashcam" in branch else "openpilot" + d = {'BRANCH': f"'\"{branch}\"'", 'BRAND': f"'\"{brand}\"'"} + if "internal" in name: + d['INTERNAL'] = "1" + + import requests + r = requests.get("https://github.com/commaci2.keys") + r.raise_for_status() + d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"' + obj = senv.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d) + f = senv.Program(f"installer/installers/installer_{name}", [obj, cont[brand]], LIBS=qt_libs) + # keep installers small + assert f[0].get_size() < 300*1e3 + +# build watch3 +if arch in ['x86_64', 'Darwin'] or GetOption('extras'): + qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'zmq', 'visionipc', 'messaging']) diff --git a/selfdrive/ui/__init__.py b/selfdrive/ui/__init__.py deleted file mode 100644 index b07e842f1a3bee..00000000000000 --- a/selfdrive/ui/__init__.py +++ /dev/null @@ -1 +0,0 @@ -UI_BORDER_SIZE = 30 diff --git a/selfdrive/ui/feedback/feedbackd.py b/selfdrive/ui/feedback/feedbackd.py deleted file mode 100755 index 2d131a0d5ed0ea..00000000000000 --- a/selfdrive/ui/feedback/feedbackd.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python3 -import cereal.messaging as messaging -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from cereal import car -from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER - -FEEDBACK_MAX_DURATION = 10.0 -ButtonType = car.CarState.ButtonEvent.Type - - -def main(): - params = Params() - pm = messaging.PubMaster(['userBookmark', 'audioFeedback']) - sm = messaging.SubMaster(['rawAudioData', 'bookmarkButton', 'carState']) - should_record_audio = False - block_num = 0 - waiting_for_release = False - early_stop_triggered = False - - while True: - sm.update() - should_send_bookmark = False - - # TODO: https://github.com/commaai/openpilot/issues/36015 - if False and sm.updated['carState'] and sm['carState'].canValid: - for be in sm['carState'].buttonEvents: - if be.type == ButtonType.lkas: - if be.pressed: - if not should_record_audio: - if params.get_bool("RecordAudioFeedback"): # Start recording on first press if toggle set - should_record_audio = True - block_num = 0 - waiting_for_release = False - early_stop_triggered = False - cloudlog.info("LKAS button pressed - starting 10-second audio feedback") - else: - should_send_bookmark = True # immediately send bookmark if toggle false - cloudlog.info("LKAS button pressed - bookmarking") - elif should_record_audio and not waiting_for_release: # Wait for release of second press to stop recording early - waiting_for_release = True - elif waiting_for_release: # Second press released - waiting_for_release = False - early_stop_triggered = True - cloudlog.info("LKAS button released - ending recording early") - - if should_record_audio and sm.updated['rawAudioData']: - raw_audio = sm['rawAudioData'] - msg = messaging.new_message('audioFeedback', valid=True) - msg.audioFeedback.audio.data = raw_audio.data - msg.audioFeedback.audio.sampleRate = raw_audio.sampleRate - msg.audioFeedback.blockNum = block_num - block_num += 1 - if (block_num * SAMPLE_BUFFER / SAMPLE_RATE) >= FEEDBACK_MAX_DURATION or early_stop_triggered: # Check for timeout or early stop - should_send_bookmark = True # send bookmark at end of audio segment - should_record_audio = False - early_stop_triggered = False - cloudlog.info("10-second recording completed or second button press - stopping audio feedback") - pm.send('audioFeedback', msg) - - if sm.updated['bookmarkButton']: - cloudlog.info("Bookmark button pressed!") - should_send_bookmark = True - - if should_send_bookmark: - msg = messaging.new_message('userBookmark', valid=True) - pm.send('userBookmark', msg) - - -if __name__ == '__main__': - main() diff --git a/selfdrive/ui/installer/continue_dashcam.sh b/selfdrive/ui/installer/continue_dashcam.sh new file mode 100755 index 00000000000000..25233fff1156b4 --- /dev/null +++ b/selfdrive/ui/installer/continue_dashcam.sh @@ -0,0 +1,4 @@ +#!/usr/bin/bash + +cd /data/openpilot +exec ./launch_chffrplus.sh diff --git a/selfdrive/ui/installer/continue_openpilot.sh b/selfdrive/ui/installer/continue_openpilot.sh index ed41ab6f3fd16f..3da67313eb949c 100755 --- a/selfdrive/ui/installer/continue_openpilot.sh +++ b/selfdrive/ui/installer/continue_openpilot.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash cd /data/openpilot exec ./launch_openpilot.sh diff --git a/selfdrive/ui/installer/installer.cc b/selfdrive/ui/installer/installer.cc index 072fa4e24b0d29..7d8bbf74e0ecef 100644 --- a/selfdrive/ui/installer/installer.cc +++ b/selfdrive/ui/installer/installer.cc @@ -1,16 +1,18 @@ -#include -#include +#include +#include + +#include #include #include -#include "common/swaglog.h" -#include "common/util.h" -#include "system/hardware/hw.h" -#include "third_party/raylib/include/raylib.h" +#include +#include +#include +#include -int freshClone(); -int cachedFetch(const std::string &cache); -int executeGitCommand(const std::string &cmd); +#include "selfdrive/ui/installer/installer.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" std::string get_str(std::string const s) { std::string::size_type pos = s.find('?'); @@ -25,54 +27,20 @@ const std::string BRANCH_STR = get_str(BRANCH "? #define GIT_SSH_URL "git@github.com:commaai/openpilot.git" #define CONTINUE_PATH "/data/continue.sh" -const std::string INSTALL_PATH = "/data/openpilot"; -const std::string VALID_CACHE_PATH = "/data/.openpilot_cache"; +const QString CACHE_PATH = "/data/openpilot.cache"; +#define INSTALL_PATH "/data/openpilot" #define TMP_INSTALL_PATH "/data/tmppilot" -const int FONT_SIZE = 120; - -extern const uint8_t str_continue[] asm("_binary_selfdrive_ui_installer_continue_openpilot_sh_start"); -extern const uint8_t str_continue_end[] asm("_binary_selfdrive_ui_installer_continue_openpilot_sh_end"); -extern const uint8_t inter_ttf[] asm("_binary_selfdrive_ui_installer_inter_ascii_ttf_start"); -extern const uint8_t inter_ttf_end[] asm("_binary_selfdrive_ui_installer_inter_ascii_ttf_end"); -extern const uint8_t inter_light_ttf[] asm("_binary_selfdrive_assets_fonts_Inter_Light_ttf_start"); -extern const uint8_t inter_light_ttf_end[] asm("_binary_selfdrive_assets_fonts_Inter_Light_ttf_end"); -extern const uint8_t inter_bold_ttf[] asm("_binary_selfdrive_assets_fonts_Inter_Bold_ttf_start"); -extern const uint8_t inter_bold_ttf_end[] asm("_binary_selfdrive_assets_fonts_Inter_Bold_ttf_end"); - -Font font_inter; -Font font_roman; -Font font_display; - -const bool tici_device = Hardware::get_device_type() == cereal::InitData::DeviceType::TICI || - Hardware::get_device_type() == cereal::InitData::DeviceType::TIZI; - -std::vector tici_prebuilt_branches = {"release3", "release-tizi", "release3-staging", "nightly", "nightly-dev"}; -std::string migrated_branch; - -void branchMigration() { - migrated_branch = BRANCH_STR; - cereal::InitData::DeviceType device_type = Hardware::get_device_type(); - if (device_type == cereal::InitData::DeviceType::TICI) { - if (std::find(tici_prebuilt_branches.begin(), tici_prebuilt_branches.end(), BRANCH_STR) != tici_prebuilt_branches.end()) { - migrated_branch = "release-tici"; - } else if (BRANCH_STR == "master") { - migrated_branch = "master-tici"; - } - } else if (device_type == cereal::InitData::DeviceType::TIZI) { - if (BRANCH_STR == "release3") { - migrated_branch = "release-tizi"; - } else if (BRANCH_STR == "release3-staging") { - migrated_branch = "release-tizi-staging"; - } - } else if (device_type == cereal::InitData::DeviceType::MICI) { - if (BRANCH_STR == "release3") { - migrated_branch = "release-mici"; - } else if (BRANCH_STR == "release3-staging") { - migrated_branch = "release-mici-staging"; - } - } +extern const uint8_t str_continue[] asm("_binary_selfdrive_ui_installer_continue_" BRAND "_sh_start"); +extern const uint8_t str_continue_end[] asm("_binary_selfdrive_ui_installer_continue_" BRAND "_sh_end"); + +bool time_valid() { + time_t rawtime; + time(&rawtime); + + struct tm * sys_time = gmtime(&rawtime); + return (1900 + sys_time->tm_year) >= 2020; } void run(const char* cmd) { @@ -80,135 +48,141 @@ void run(const char* cmd) { assert(err == 0); } -void finishInstall() { - BeginDrawing(); - ClearBackground(BLACK); - if (tici_device) { - const char *m = "Finishing install..."; - int text_width = MeasureText(m, FONT_SIZE); - DrawTextEx(font_display, m, (Vector2){(float)(GetScreenWidth() - text_width)/2 + FONT_SIZE, (float)(GetScreenHeight() - FONT_SIZE)/2}, FONT_SIZE, 0, WHITE); - } else { - DrawTextEx(font_display, "finishing setup", (Vector2){8, 10}, 82, 0, WHITE); - } - EndDrawing(); - util::sleep_for(60 * 1000); -} +Installer::Installer(QWidget *parent) : QWidget(parent) { + QVBoxLayout *layout = new QVBoxLayout(this); + layout->setContentsMargins(150, 290, 150, 150); + layout->setSpacing(0); + + QLabel *title = new QLabel(tr("Installing...")); + title->setStyleSheet("font-size: 90px; font-weight: 600;"); + layout->addWidget(title, 0, Qt::AlignTop); + + layout->addSpacing(170); + + bar = new QProgressBar(); + bar->setRange(0, 100); + bar->setTextVisible(false); + bar->setFixedHeight(72); + layout->addWidget(bar, 0, Qt::AlignTop); + + layout->addSpacing(30); + + val = new QLabel("0%"); + val->setStyleSheet("font-size: 70px; font-weight: 300;"); + layout->addWidget(val, 0, Qt::AlignTop); + + layout->addStretch(); + + QObject::connect(&proc, QOverload::of(&QProcess::finished), this, &Installer::cloneFinished); + QObject::connect(&proc, &QProcess::readyReadStandardError, this, &Installer::readProgress); -void renderProgress(int progress) { - BeginDrawing(); - ClearBackground(BLACK); - if (tici_device) { - DrawTextEx(font_inter, "Installing...", (Vector2){150, 290}, 110, 0, WHITE); - Rectangle bar = {150, 570, (float)GetScreenWidth() - 300, 72}; - DrawRectangleRec(bar, (Color){41, 41, 41, 255}); - progress = std::clamp(progress, 0, 100); - bar.width *= progress / 100.0f; - DrawRectangleRec(bar, (Color){70, 91, 234, 255}); - DrawTextEx(font_inter, (std::to_string(progress) + "%").c_str(), (Vector2){150, 670}, 85, 0, WHITE); - } else { - DrawTextEx(font_display, "installing", (Vector2){8, 10}, 82, 0, WHITE); - const std::string percent_str = std::to_string(progress) + "%"; - DrawTextEx(font_roman, percent_str.c_str(), (Vector2){6, (float)(GetScreenHeight() - 128 + 18)}, 128, 0, - (Color){255, 255, 255, (unsigned char)(255 * 0.9 * 0.35)}); + QTimer::singleShot(100, this, &Installer::doInstall); + + setStyleSheet(R"( + * { + font-family: Inter; + color: white; + background-color: black; + } + QProgressBar { + border: none; + background-color: #292929; } + QProgressBar::chunk { + background-color: #364DEF; + } + )"); +} - EndDrawing(); +void Installer::updateProgress(int percent) { + bar->setValue(percent); + val->setText(QString("%1%").arg(percent)); + update(); } -int doInstall() { +void Installer::doInstall() { // wait for valid time - while (!util::system_time_valid()) { - util::sleep_for(500); - LOGD("Waiting for valid time"); + while (!time_valid()) { + usleep(500 * 1000); + qDebug() << "Waiting for valid time"; } // cleanup previous install attempts - run("rm -rf " TMP_INSTALL_PATH); + run("rm -rf " TMP_INSTALL_PATH " " INSTALL_PATH); // do the install - if (util::file_exists(INSTALL_PATH) && util::file_exists(VALID_CACHE_PATH)) { - return cachedFetch(INSTALL_PATH); + if (QDir(CACHE_PATH).exists()) { + cachedFetch(CACHE_PATH); } else { - return freshClone(); + freshClone(); } } -int freshClone() { - LOGD("Doing fresh clone"); - std::string cmd = util::string_format("git clone --progress %s -b %s --depth=1 --recurse-submodules %s 2>&1", - GIT_URL.c_str(), migrated_branch.c_str(), TMP_INSTALL_PATH); - return executeGitCommand(cmd); +void Installer::freshClone() { + qDebug() << "Doing fresh clone"; + proc.start("git", {"clone", "--progress", GIT_URL.c_str(), "-b", BRANCH_STR.c_str(), + "--depth=1", "--recurse-submodules", TMP_INSTALL_PATH}); } -int cachedFetch(const std::string &cache) { - LOGD("Fetching with cache: %s", cache.c_str()); +void Installer::cachedFetch(const QString &cache) { + qDebug() << "Fetching with cache: " << cache; - run(util::string_format("cp -rp %s %s", cache.c_str(), TMP_INSTALL_PATH).c_str()); - run(util::string_format("cd %s && git remote set-branches --add origin %s", TMP_INSTALL_PATH, migrated_branch.c_str()).c_str()); + run(QString("cp -rp %1 %2").arg(cache, TMP_INSTALL_PATH).toStdString().c_str()); + int err = chdir(TMP_INSTALL_PATH); + assert(err == 0); + run(("git remote set-branches --add origin " + BRANCH_STR).c_str()); - renderProgress(10); + updateProgress(10); - return executeGitCommand(util::string_format("cd %s && git fetch --progress origin %s 2>&1", TMP_INSTALL_PATH, migrated_branch.c_str())); + proc.setWorkingDirectory(TMP_INSTALL_PATH); + proc.start("git", {"fetch", "--progress", "origin", BRANCH_STR.c_str()}); } -int executeGitCommand(const std::string &cmd) { - static const std::array stages = { +void Installer::readProgress() { + const QVector> stages = { // prefix, weight in percentage - std::pair{"Receiving objects: ", 91}, - std::pair{"Resolving deltas: ", 2}, - std::pair{"Updating files: ", 7}, + {tr("Receiving objects: "), 91}, + {tr("Resolving deltas: "), 2}, + {tr("Updating files: "), 7}, }; - FILE *pipe = popen(cmd.c_str(), "r"); - if (!pipe) return -1; - - char buffer[512]; - while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { - std::string line(buffer); - int base = 0; - for (const auto &[text, weight] : stages) { - if (line.find(text) != std::string::npos) { - size_t percentPos = line.find("%"); - if (percentPos != std::string::npos && percentPos >= 3) { - int percent = std::stoi(line.substr(percentPos - 3, 3)); - int progress = base + int(percent / 100. * weight); - renderProgress(progress); - } - break; - } - base += weight; + auto line = QString(proc.readAllStandardError()); + + int base = 0; + for (const QPair kv : stages) { + if (line.startsWith(kv.first)) { + auto perc = line.split(kv.first)[1].split("%")[0]; + int p = base + int(perc.toFloat() / 100. * kv.second); + updateProgress(p); + break; } + base += kv.second; } - return pclose(pipe); } -void cloneFinished(int exitCode) { - LOGD("git finished with %d", exitCode); +void Installer::cloneFinished(int exitCode, QProcess::ExitStatus exitStatus) { + qDebug() << "git finished with " << exitCode; assert(exitCode == 0); - renderProgress(100); + updateProgress(100); // ensure correct branch is checked out int err = chdir(TMP_INSTALL_PATH); assert(err == 0); - run(("git checkout " + migrated_branch).c_str()); - run(("git reset --hard origin/" + migrated_branch).c_str()); + run(("git checkout " + BRANCH_STR).c_str()); + run(("git reset --hard origin/" + BRANCH_STR).c_str()); run("git submodule update --init"); // move into place - run(("rm -f " + VALID_CACHE_PATH).c_str()); - run(("rm -rf " + INSTALL_PATH).c_str()); - run(util::string_format("mv %s %s", TMP_INSTALL_PATH, INSTALL_PATH.c_str()).c_str()); + run("mv " TMP_INSTALL_PATH " " INSTALL_PATH); #ifdef INTERNAL run("mkdir -p /data/params/d/"); - // https://github.com/commaci2.keys - const std::string ssh_keys = "ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIMX2kU8eBZyEWmbq0tjMPxksWWVuIV/5l64GabcYbdpI"; std::map params = { {"SshEnabled", "1"}, {"RecordFrontLock", "1"}, - {"GithubSshKeys", ssh_keys}, + {"GithubSshKeys", SSH_KEYS}, }; for (const auto& [key, value] : params) { std::ofstream param; @@ -216,9 +190,9 @@ void cloneFinished(int exitCode) { param << value; param.close(); } - run(("cd " + INSTALL_PATH + " && " + run("cd " INSTALL_PATH " && " "git remote set-url origin --push " GIT_SSH_URL " && " - "git config --replace-all remote.origin.fetch \"+refs/heads/*:refs/remotes/origin/*\"").c_str()); + "git config --replace-all remote.origin.fetch \"+refs/heads/*:refs/remotes/origin/*\""); #endif // write continue.sh @@ -234,36 +208,13 @@ void cloneFinished(int exitCode) { run("mv /data/continue.sh.new " CONTINUE_PATH); // wait for the installed software's UI to take over - finishInstall(); + QTimer::singleShot(60 * 1000, &QCoreApplication::quit); } int main(int argc, char *argv[]) { - if (tici_device) { - InitWindow(2160, 1080, "Installer"); - } else { - InitWindow(536, 240, "Installer"); - } - - font_inter = LoadFontFromMemory(".ttf", inter_ttf, inter_ttf_end - inter_ttf, FONT_SIZE, NULL, 0); - font_roman = LoadFontFromMemory(".ttf", inter_light_ttf, inter_light_ttf_end - inter_light_ttf, FONT_SIZE, NULL, 0); - font_display = LoadFontFromMemory(".ttf", inter_bold_ttf, inter_bold_ttf_end - inter_bold_ttf, FONT_SIZE, NULL, 0); - SetTextureFilter(font_inter.texture, TEXTURE_FILTER_BILINEAR); - SetTextureFilter(font_roman.texture, TEXTURE_FILTER_BILINEAR); - SetTextureFilter(font_display.texture, TEXTURE_FILTER_BILINEAR); - - branchMigration(); - - if (util::file_exists(CONTINUE_PATH)) { - finishInstall(); - } else { - renderProgress(0); - int result = doInstall(); - cloneFinished(result); - } - - CloseWindow(); - UnloadFont(font_inter); - UnloadFont(font_roman); - UnloadFont(font_display); - return 0; + initApp(argc, argv); + QApplication a(argc, argv); + Installer installer; + setMainWindow(&installer); + return a.exec(); } diff --git a/selfdrive/ui/installer/installer.h b/selfdrive/ui/installer/installer.h new file mode 100644 index 00000000000000..de3af0ff395021 --- /dev/null +++ b/selfdrive/ui/installer/installer.h @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +class Installer : public QWidget { + Q_OBJECT + +public: + explicit Installer(QWidget *parent = 0); + +private slots: + void updateProgress(int percent); + + void readProgress(); + void cloneFinished(int exitCode, QProcess::ExitStatus exitStatus); + +private: + QLabel *val; + QProgressBar *bar; + QProcess proc; + + void doInstall(); + void freshClone(); + void cachedFetch(const QString &cache); +}; diff --git a/selfdrive/ui/installer/inter-ascii.ttf b/selfdrive/ui/installer/inter-ascii.ttf deleted file mode 100644 index 5d480c515a6ed1..00000000000000 --- a/selfdrive/ui/installer/inter-ascii.ttf +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1ef26a4099ef867f3493389379d882381a2491cdbfa41a086be8899a2154dcb3 -size 26160 diff --git a/selfdrive/ui/layouts/home.py b/selfdrive/ui/layouts/home.py deleted file mode 100644 index a8404a20c3b49c..00000000000000 --- a/selfdrive/ui/layouts/home.py +++ /dev/null @@ -1,232 +0,0 @@ -import time -import pyray as rl -from collections.abc import Callable -from enum import IntEnum -from openpilot.common.params import Params -from openpilot.selfdrive.ui.widgets.offroad_alerts import UpdateAlert, OffroadAlert -from openpilot.selfdrive.ui.widgets.exp_mode_button import ExperimentalModeButton -from openpilot.selfdrive.ui.widgets.prime import PrimeWidget -from openpilot.selfdrive.ui.widgets.setup import SetupWidget -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos -from openpilot.system.ui.lib.multilang import tr, trn -from openpilot.system.ui.widgets.label import gui_label -from openpilot.system.ui.widgets import Widget - -HEADER_HEIGHT = 80 -HEAD_BUTTON_FONT_SIZE = 40 -CONTENT_MARGIN = 40 -SPACING = 25 -RIGHT_COLUMN_WIDTH = 750 -REFRESH_INTERVAL = 10.0 - - -class HomeLayoutState(IntEnum): - HOME = 0 - UPDATE = 1 - ALERTS = 2 - - -class HomeLayout(Widget): - def __init__(self): - super().__init__() - self.params = Params() - - self.update_alert = UpdateAlert() - self.offroad_alert = OffroadAlert() - - self._layout_widgets = {HomeLayoutState.UPDATE: self.update_alert, HomeLayoutState.ALERTS: self.offroad_alert} - - self.current_state = HomeLayoutState.HOME - self.last_refresh = 0 - self.settings_callback: Callable[[], None] | None = None - - self.update_available = False - self.alert_count = 0 - self._version_text = "" - self._prev_update_available = False - self._prev_alerts_present = False - - self.header_rect = rl.Rectangle(0, 0, 0, 0) - self.content_rect = rl.Rectangle(0, 0, 0, 0) - self.left_column_rect = rl.Rectangle(0, 0, 0, 0) - self.right_column_rect = rl.Rectangle(0, 0, 0, 0) - - self.update_notif_rect = rl.Rectangle(0, 0, 200, HEADER_HEIGHT - 10) - self.alert_notif_rect = rl.Rectangle(0, 0, 220, HEADER_HEIGHT - 10) - - self._prime_widget = PrimeWidget() - self._setup_widget = SetupWidget() - - self._exp_mode_button = ExperimentalModeButton() - self._setup_callbacks() - - def show_event(self): - self._exp_mode_button.show_event() - self.last_refresh = time.monotonic() - self._refresh() - - def _setup_callbacks(self): - self.update_alert.set_dismiss_callback(lambda: self._set_state(HomeLayoutState.HOME)) - self.offroad_alert.set_dismiss_callback(lambda: self._set_state(HomeLayoutState.HOME)) - self._exp_mode_button.set_click_callback(lambda: self.settings_callback() if self.settings_callback else None) - - def set_settings_callback(self, callback: Callable): - self.settings_callback = callback - - def _set_state(self, state: HomeLayoutState): - # propagate show/hide events - if state != self.current_state: - if state == HomeLayoutState.HOME: - self._exp_mode_button.show_event() - - if state in self._layout_widgets: - self._layout_widgets[state].show_event() - if self.current_state in self._layout_widgets: - self._layout_widgets[self.current_state].hide_event() - - self.current_state = state - - def _render(self, rect: rl.Rectangle): - current_time = time.monotonic() - if current_time - self.last_refresh >= REFRESH_INTERVAL: - self._refresh() - self.last_refresh = current_time - - self._render_header() - - # Render content based on current state - if self.current_state == HomeLayoutState.HOME: - self._render_home_content() - elif self.current_state == HomeLayoutState.UPDATE: - self._render_update_view() - elif self.current_state == HomeLayoutState.ALERTS: - self._render_alerts_view() - - def _update_state(self): - self.header_rect = rl.Rectangle( - self._rect.x + CONTENT_MARGIN, self._rect.y + CONTENT_MARGIN, self._rect.width - 2 * CONTENT_MARGIN, HEADER_HEIGHT - ) - - content_y = self._rect.y + CONTENT_MARGIN + HEADER_HEIGHT + SPACING - content_height = self._rect.height - CONTENT_MARGIN - HEADER_HEIGHT - SPACING - CONTENT_MARGIN - - self.content_rect = rl.Rectangle( - self._rect.x + CONTENT_MARGIN, content_y, self._rect.width - 2 * CONTENT_MARGIN, content_height - ) - - left_width = self.content_rect.width - RIGHT_COLUMN_WIDTH - SPACING - - self.left_column_rect = rl.Rectangle(self.content_rect.x, self.content_rect.y, left_width, self.content_rect.height) - - self.right_column_rect = rl.Rectangle( - self.content_rect.x + left_width + SPACING, self.content_rect.y, RIGHT_COLUMN_WIDTH, self.content_rect.height - ) - - self.update_notif_rect.x = self.header_rect.x - self.update_notif_rect.y = self.header_rect.y + (self.header_rect.height - 60) // 2 - - notif_x = self.header_rect.x + (220 if self.update_available else 0) - self.alert_notif_rect.x = notif_x - self.alert_notif_rect.y = self.header_rect.y + (self.header_rect.height - 60) // 2 - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - - if self.update_available and rl.check_collision_point_rec(mouse_pos, self.update_notif_rect): - self._set_state(HomeLayoutState.UPDATE) - elif self.alert_count > 0 and rl.check_collision_point_rec(mouse_pos, self.alert_notif_rect): - self._set_state(HomeLayoutState.ALERTS) - - def _render_header(self): - font = gui_app.font(FontWeight.MEDIUM) - - version_text_width = self.header_rect.width - - # Update notification button - if self.update_available: - version_text_width -= self.update_notif_rect.width - - # Highlight if currently viewing updates - highlight_color = rl.Color(75, 95, 255, 255) if self.current_state == HomeLayoutState.UPDATE else rl.Color(54, 77, 239, 255) - rl.draw_rectangle_rounded(self.update_notif_rect, 0.3, 10, highlight_color) - - text = tr("UPDATE") - text_size = measure_text_cached(font, text, HEAD_BUTTON_FONT_SIZE) - text_x = self.update_notif_rect.x + (self.update_notif_rect.width - text_size.x) // 2 - text_y = self.update_notif_rect.y + (self.update_notif_rect.height - text_size.y) // 2 - rl.draw_text_ex(font, text, rl.Vector2(int(text_x), int(text_y)), HEAD_BUTTON_FONT_SIZE, 0, rl.WHITE) - - # Alert notification button - if self.alert_count > 0: - version_text_width -= self.alert_notif_rect.width - - # Highlight if currently viewing alerts - highlight_color = rl.Color(255, 70, 70, 255) if self.current_state == HomeLayoutState.ALERTS else rl.Color(226, 44, 44, 255) - rl.draw_rectangle_rounded(self.alert_notif_rect, 0.3, 10, highlight_color) - - alert_text = trn("{} ALERT", "{} ALERTS", self.alert_count).format(self.alert_count) - text_size = measure_text_cached(font, alert_text, HEAD_BUTTON_FONT_SIZE) - text_x = self.alert_notif_rect.x + (self.alert_notif_rect.width - text_size.x) // 2 - text_y = self.alert_notif_rect.y + (self.alert_notif_rect.height - text_size.y) // 2 - rl.draw_text_ex(font, alert_text, rl.Vector2(int(text_x), int(text_y)), HEAD_BUTTON_FONT_SIZE, 0, rl.WHITE) - - # Version text (right aligned) - if self.update_available or self.alert_count > 0: - version_text_width -= SPACING * 1.5 - - version_rect = rl.Rectangle(self.header_rect.x + self.header_rect.width - version_text_width, self.header_rect.y, - version_text_width, self.header_rect.height) - gui_label(version_rect, self._version_text, 48, rl.WHITE, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT) - - def _render_home_content(self): - self._render_left_column() - self._render_right_column() - - def _render_update_view(self): - self.update_alert.render(self.content_rect) - - def _render_alerts_view(self): - self.offroad_alert.render(self.content_rect) - - def _render_left_column(self): - self._prime_widget.render(self.left_column_rect) - - def _render_right_column(self): - exp_height = 125 - exp_rect = rl.Rectangle( - self.right_column_rect.x, self.right_column_rect.y, self.right_column_rect.width, exp_height - ) - self._exp_mode_button.render(exp_rect) - - setup_rect = rl.Rectangle( - self.right_column_rect.x, - self.right_column_rect.y + exp_height + SPACING, - self.right_column_rect.width, - self.right_column_rect.height - exp_height - SPACING, - ) - self._setup_widget.render(setup_rect) - - def _refresh(self): - self._version_text = self._get_version_text() - update_available = self.update_alert.refresh() - alert_count = self.offroad_alert.refresh() - alerts_present = alert_count > 0 - - # Show panels on transition from no alert/update to any alerts/update - if not update_available and not alerts_present: - self._set_state(HomeLayoutState.HOME) - elif update_available and ((not self._prev_update_available) or (not alerts_present and self.current_state == HomeLayoutState.ALERTS)): - self._set_state(HomeLayoutState.UPDATE) - elif alerts_present and ((not self._prev_alerts_present) or (not update_available and self.current_state == HomeLayoutState.UPDATE)): - self._set_state(HomeLayoutState.ALERTS) - - self.update_available = update_available - self.alert_count = alert_count - self._prev_update_available = update_available - self._prev_alerts_present = alerts_present - - def _get_version_text(self) -> str: - brand = "openpilot" - description = self.params.get("UpdaterCurrentDescription") - return f"{brand} {description}" if description else brand diff --git a/selfdrive/ui/layouts/main.py b/selfdrive/ui/layouts/main.py deleted file mode 100644 index 702854f98a7eb1..00000000000000 --- a/selfdrive/ui/layouts/main.py +++ /dev/null @@ -1,108 +0,0 @@ -import pyray as rl -from enum import IntEnum -import cereal.messaging as messaging -from openpilot.system.ui.lib.application import gui_app -from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH -from openpilot.selfdrive.ui.layouts.home import HomeLayout -from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType -from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView -from openpilot.selfdrive.ui.ui_state import device, ui_state -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow - - -class MainState(IntEnum): - HOME = 0 - SETTINGS = 1 - ONROAD = 2 - - -class MainLayout(Widget): - def __init__(self): - super().__init__() - - self._pm = messaging.PubMaster(['bookmarkButton']) - - self._sidebar = Sidebar() - self._current_mode = MainState.HOME - self._prev_onroad = False - - # Initialize layouts - self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()} - - self._sidebar_rect = rl.Rectangle(0, 0, 0, 0) - self._content_rect = rl.Rectangle(0, 0, 0, 0) - - # Set callbacks - self._setup_callbacks() - - # Start onboarding if terms or training not completed - self._onboarding_window = OnboardingWindow() - if not self._onboarding_window.completed: - gui_app.set_modal_overlay(self._onboarding_window) - - def _render(self, _): - self._handle_onroad_transition() - self._render_main_content() - - def _setup_callbacks(self): - self._sidebar.set_callbacks(on_settings=self._on_settings_clicked, - on_flag=self._on_bookmark_clicked, - open_settings=lambda: self.open_settings(PanelType.TOGGLES)) - self._layouts[MainState.HOME]._setup_widget.set_open_settings_callback(lambda: self.open_settings(PanelType.FIREHOSE)) - self._layouts[MainState.HOME].set_settings_callback(lambda: self.open_settings(PanelType.TOGGLES)) - self._layouts[MainState.SETTINGS].set_callbacks(on_close=self._set_mode_for_state) - self._layouts[MainState.ONROAD].set_click_callback(self._on_onroad_clicked) - device.add_interactive_timeout_callback(self._set_mode_for_state) - - def _update_layout_rects(self): - self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height) - - x_offset = SIDEBAR_WIDTH if self._sidebar.is_visible else 0 - self._content_rect = rl.Rectangle(self._rect.y + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height) - - def _handle_onroad_transition(self): - if ui_state.started != self._prev_onroad: - self._prev_onroad = ui_state.started - - self._set_mode_for_state() - - def _set_mode_for_state(self): - if ui_state.started: - # Don't hide sidebar from interactive timeout - if self._current_mode != MainState.ONROAD: - self._sidebar.set_visible(False) - self._set_current_layout(MainState.ONROAD) - else: - 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._set_current_layout(MainState.SETTINGS) - self._sidebar.set_visible(False) - - def _on_settings_clicked(self): - self.open_settings(PanelType.DEVICE) - - def _on_bookmark_clicked(self): - user_bookmark = messaging.new_message('bookmarkButton') - user_bookmark.valid = True - self._pm.send('bookmarkButton', user_bookmark) - - def _on_onroad_clicked(self): - self._sidebar.set_visible(not self._sidebar.is_visible) - - def _render_main_content(self): - # Render sidebar - if self._sidebar.is_visible: - self._sidebar.render(self._sidebar_rect) - - content_rect = self._content_rect if self._sidebar.is_visible else self._rect - self._layouts[self._current_mode].render(content_rect) diff --git a/selfdrive/ui/layouts/onboarding.py b/selfdrive/ui/layouts/onboarding.py deleted file mode 100644 index 5d61c1c95a3293..00000000000000 --- a/selfdrive/ui/layouts/onboarding.py +++ /dev/null @@ -1,213 +0,0 @@ -import os -import re -import threading -from enum import IntEnum - -import pyray as rl -from openpilot.common.basedir import BASEDIR -from openpilot.system.ui.lib.application import FontWeight, gui_app -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.label import Label -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.version import terms_version, training_version - -DEBUG = False - -STEP_RECTS = [rl.Rectangle(104, 800, 633, 175), rl.Rectangle(1835, 0, 2159, 1080), rl.Rectangle(1835, 0, 2156, 1080), - rl.Rectangle(1526, 473, 427, 472), rl.Rectangle(1643, 441, 217, 223), rl.Rectangle(1835, 0, 2155, 1080), - rl.Rectangle(1786, 591, 267, 236), rl.Rectangle(1353, 0, 804, 1080), rl.Rectangle(1458, 485, 633, 211), - rl.Rectangle(95, 794, 1158, 187), rl.Rectangle(1560, 170, 392, 397), rl.Rectangle(1835, 0, 2159, 1080), - rl.Rectangle(1351, 0, 807, 1080), rl.Rectangle(1835, 0, 2158, 1080), rl.Rectangle(1531, 82, 441, 920), - rl.Rectangle(1336, 438, 490, 393), rl.Rectangle(1835, 0, 2159, 1080), rl.Rectangle(1835, 0, 2159, 1080), - rl.Rectangle(87, 795, 1187, 186)] - -DM_RECORD_STEP = 9 -DM_RECORD_YES_RECT = rl.Rectangle(695, 794, 558, 187) - -RESTART_TRAINING_RECT = rl.Rectangle(87, 795, 472, 186) - - -class OnboardingState(IntEnum): - TERMS = 0 - ONBOARDING = 1 - DECLINE = 2 - - -class TrainingGuide(Widget): - def __init__(self, completed_callback=None): - super().__init__() - self._completed_callback = completed_callback - - self._step = 0 - self._load_image_paths() - - # Load first image now so we show something immediately - self._textures = [gui_app.texture(self._image_paths[0])] - self._image_objs = [] - - threading.Thread(target=self._preload_thread, daemon=True).start() - - def _load_image_paths(self): - paths = [fn for fn in os.listdir(os.path.join(BASEDIR, "selfdrive/assets/training")) if re.match(r'^step\d*\.png$', fn)] - paths = sorted(paths, key=lambda x: int(re.search(r'\d+', x).group())) - self._image_paths = [os.path.join(BASEDIR, "selfdrive/assets/training", fn) for fn in paths] - - def _preload_thread(self): - # PNG loading is slow in raylib, so we preload in a thread and upload to GPU in main thread - # We've already loaded the first image on init - for path in self._image_paths[1:]: - self._image_objs.append(gui_app._load_image_from_path(path)) - - def _handle_mouse_release(self, mouse_pos): - if rl.check_collision_point_rec(mouse_pos, STEP_RECTS[self._step]): - # Record DM camera? - if self._step == DM_RECORD_STEP: - yes = rl.check_collision_point_rec(mouse_pos, DM_RECORD_YES_RECT) - print(f"putting RecordFront to {yes}") - ui_state.params.put_bool("RecordFront", yes) - - # Restart training? - elif self._step == len(self._image_paths) - 1: - if rl.check_collision_point_rec(mouse_pos, RESTART_TRAINING_RECT): - self._step = -1 - - self._step += 1 - - # Finished? - if self._step >= len(self._image_paths): - self._step = 0 - if self._completed_callback: - self._completed_callback() - - def _update_state(self): - if len(self._image_objs): - self._textures.append(gui_app._load_texture_from_image(self._image_objs.pop(0))) - - def _render(self, _): - # Safeguard against fast tapping - step = min(self._step, len(self._textures) - 1) - rl.draw_texture(self._textures[step], 0, 0, rl.WHITE) - - # progress bar - if 0 < step < len(STEP_RECTS) - 1: - h = 20 - w = int((step / (len(STEP_RECTS) - 1)) * self._rect.width) - rl.draw_rectangle(int(self._rect.x), int(self._rect.y + self._rect.height - h), - w, h, rl.Color(70, 91, 234, 255)) - - if DEBUG: - rl.draw_rectangle_lines_ex(STEP_RECTS[step], 3, rl.RED) - - return -1 - - -class TermsPage(Widget): - def __init__(self, on_accept=None, on_decline=None): - super().__init__() - self._on_accept = on_accept - self._on_decline = on_decline - - self._title = Label(tr("Welcome to openpilot"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT) - self._desc = Label(tr("You must accept the Terms and Conditions to use openpilot. Read the latest terms at https://comma.ai/terms before continuing."), - font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT) - - self._decline_btn = Button(tr("Decline"), click_callback=on_decline) - self._accept_btn = Button(tr("Agree"), button_style=ButtonStyle.PRIMARY, click_callback=on_accept) - - def _render(self, _): - welcome_x = self._rect.x + 165 - welcome_y = self._rect.y + 165 - welcome_rect = rl.Rectangle(welcome_x, welcome_y, self._rect.width - welcome_x, 90) - self._title.render(welcome_rect) - - desc_x = welcome_x - # TODO: Label doesn't top align when wrapping - desc_y = welcome_y - 100 - desc_rect = rl.Rectangle(desc_x, desc_y, self._rect.width - desc_x, self._rect.height - desc_y - 250) - self._desc.render(desc_rect) - - btn_y = self._rect.y + self._rect.height - 160 - 45 - btn_width = (self._rect.width - 45 * 3) / 2 - self._decline_btn.render(rl.Rectangle(self._rect.x + 45, btn_y, btn_width, 160)) - self._accept_btn.render(rl.Rectangle(self._rect.x + 45 * 2 + btn_width, btn_y, btn_width, 160)) - - if DEBUG: - rl.draw_rectangle_lines_ex(welcome_rect, 3, rl.RED) - rl.draw_rectangle_lines_ex(desc_rect, 3, rl.RED) - - return -1 - - -class DeclinePage(Widget): - def __init__(self, back_callback=None): - super().__init__() - self._text = Label(tr("You must accept the Terms and Conditions in order to use openpilot."), - font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT) - self._back_btn = Button(tr("Back"), click_callback=back_callback) - self._uninstall_btn = Button(tr("Decline, uninstall openpilot"), button_style=ButtonStyle.DANGER, - click_callback=self._on_uninstall_clicked) - - def _on_uninstall_clicked(self): - ui_state.params.put_bool("DoUninstall", True) - gui_app.request_close() - - def _render(self, _): - btn_y = self._rect.y + self._rect.height - 160 - 45 - btn_width = (self._rect.width - 45 * 3) / 2 - self._back_btn.render(rl.Rectangle(self._rect.x + 45, btn_y, btn_width, 160)) - self._uninstall_btn.render(rl.Rectangle(self._rect.x + 45 * 2 + btn_width, btn_y, btn_width, 160)) - - # text rect in middle of top and button - text_height = btn_y - (200 + 45) - text_rect = rl.Rectangle(self._rect.x + 165, self._rect.y + (btn_y - text_height) / 2 + 10, self._rect.width - (165 * 2), text_height) - if DEBUG: - rl.draw_rectangle_lines_ex(text_rect, 3, rl.RED) - self._text.render(text_rect) - - -class OnboardingWindow(Widget): - def __init__(self): - super().__init__() - self._accepted_terms: bool = ui_state.params.get("HasAcceptedTerms") == terms_version - self._training_done: bool = ui_state.params.get("CompletedTrainingVersion") == training_version - - self._state = OnboardingState.TERMS if not self._accepted_terms else OnboardingState.ONBOARDING - - # Windows - self._terms = TermsPage(on_accept=self._on_terms_accepted, on_decline=self._on_terms_declined) - self._training_guide: TrainingGuide | None = None - self._decline_page = DeclinePage(back_callback=self._on_decline_back) - - @property - def completed(self) -> bool: - return self._accepted_terms and self._training_done - - def _on_terms_declined(self): - self._state = OnboardingState.DECLINE - - def _on_decline_back(self): - self._state = OnboardingState.TERMS - - def _on_terms_accepted(self): - ui_state.params.put("HasAcceptedTerms", terms_version) - self._state = OnboardingState.ONBOARDING - if self._training_done: - gui_app.set_modal_overlay(None) - - def _on_completed_training(self): - ui_state.params.put("CompletedTrainingVersion", training_version) - gui_app.set_modal_overlay(None) - - def _render(self, _): - if self._training_guide is None: - self._training_guide = TrainingGuide(completed_callback=self._on_completed_training) - - if self._state == OnboardingState.TERMS: - self._terms.render(self._rect) - if self._state == OnboardingState.ONBOARDING: - self._training_guide.render(self._rect) - elif self._state == OnboardingState.DECLINE: - self._decline_page.render(self._rect) - return -1 diff --git a/selfdrive/ui/layouts/settings/common.py b/selfdrive/ui/layouts/settings/common.py deleted file mode 100644 index 5e87a6447ae3c0..00000000000000 --- a/selfdrive/ui/layouts/settings/common.py +++ /dev/null @@ -1,5 +0,0 @@ -from openpilot.selfdrive.ui.ui_state import ui_state - - -def restart_needed_callback(_): - ui_state.params.put_bool("OnroadCycleRequested", True) diff --git a/selfdrive/ui/layouts/settings/developer.py b/selfdrive/ui/layouts/settings/developer.py deleted file mode 100644 index 646c817508d8c6..00000000000000 --- a/selfdrive/ui/layouts/settings/developer.py +++ /dev/null @@ -1,185 +0,0 @@ -from openpilot.common.params import Params -from openpilot.selfdrive.ui.widgets.ssh_key import ssh_key_item -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.list_view import toggle_item -from openpilot.system.ui.widgets.scroller_tici import Scroller -from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.multilang import tr, tr_noop -from openpilot.system.ui.widgets import DialogResult - -# Description constants -DESCRIPTIONS = { - 'enable_adb': tr_noop( - "ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. " + - "See https://docs.comma.ai/how-to/connect-to-comma for more info." - ), - 'ssh_key': tr_noop( - "Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username " + - "other than your own. A comma employee will NEVER ask you to add their GitHub username." - ), - 'alpha_longitudinal': tr_noop( - "WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).

    " + - "On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. " + - "Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. " + - "Changing this setting will restart openpilot if the car is powered on." - ), -} - - -class DeveloperLayout(Widget): - def __init__(self): - super().__init__() - self._params = Params() - self._is_release = self._params.get_bool("IsReleaseBranch") - - # Build items and keep references for callbacks/state updates - self._adb_toggle = toggle_item( - lambda: tr("Enable ADB"), - description=lambda: tr(DESCRIPTIONS["enable_adb"]), - initial_state=self._params.get_bool("AdbEnabled"), - callback=self._on_enable_adb, - enabled=ui_state.is_offroad, - ) - - # SSH enable toggle + SSH key management - self._ssh_toggle = toggle_item( - lambda: tr("Enable SSH"), - description="", - initial_state=self._params.get_bool("SshEnabled"), - callback=self._on_enable_ssh, - ) - self._ssh_keys = ssh_key_item(lambda: tr("SSH Keys"), description=lambda: tr(DESCRIPTIONS["ssh_key"])) - - self._joystick_toggle = toggle_item( - lambda: tr("Joystick Debug Mode"), - description="", - initial_state=self._params.get_bool("JoystickDebugMode"), - callback=self._on_joystick_debug_mode, - enabled=ui_state.is_offroad, - ) - - self._long_maneuver_toggle = toggle_item( - lambda: tr("Longitudinal Maneuver Mode"), - description="", - initial_state=self._params.get_bool("LongitudinalManeuverMode"), - callback=self._on_long_maneuver_mode, - ) - - self._alpha_long_toggle = toggle_item( - lambda: tr("openpilot Longitudinal Control (Alpha)"), - description=lambda: tr(DESCRIPTIONS["alpha_longitudinal"]), - initial_state=self._params.get_bool("AlphaLongitudinalEnabled"), - callback=self._on_alpha_long_enabled, - enabled=lambda: not ui_state.engaged, - ) - - self._ui_debug_toggle = toggle_item( - lambda: tr("UI Debug Mode"), - description="", - initial_state=self._params.get_bool("ShowDebugInfo"), - callback=self._on_enable_ui_debug, - ) - self._on_enable_ui_debug(self._params.get_bool("ShowDebugInfo")) - - self._scroller = Scroller([ - self._adb_toggle, - self._ssh_toggle, - self._ssh_keys, - self._joystick_toggle, - self._long_maneuver_toggle, - self._alpha_long_toggle, - self._ui_debug_toggle, - ], line_separator=True, spacing=0) - - # Toggles should be not available to change in onroad state - ui_state.add_offroad_transition_callback(self._update_toggles) - - def _render(self, rect): - self._scroller.render(rect) - - def show_event(self): - self._scroller.show_event() - self._update_toggles() - - def _update_toggles(self): - ui_state.update_params() - - # Hide non-release toggles on release builds - # TODO: we can do an onroad cycle, but alpha long toggle requires a deinit function to re-enable radar and not fault - for item in (self._joystick_toggle, self._long_maneuver_toggle, self._alpha_long_toggle): - item.set_visible(not self._is_release) - - # CP gating - if ui_state.CP is not None: - alpha_avail = ui_state.CP.alphaLongitudinalAvailable - if not alpha_avail or self._is_release: - self._alpha_long_toggle.set_visible(False) - self._params.remove("AlphaLongitudinalEnabled") - else: - self._alpha_long_toggle.set_visible(True) - - long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad() - self._long_maneuver_toggle.action_item.set_enabled(long_man_enabled) - if not long_man_enabled: - self._long_maneuver_toggle.action_item.set_state(False) - self._params.put_bool("LongitudinalManeuverMode", False) - else: - self._long_maneuver_toggle.action_item.set_enabled(False) - self._alpha_long_toggle.set_visible(False) - - # TODO: make a param control list item so we don't need to manage internal state as much here - # refresh toggles from params to mirror external changes - for key, item in ( - ("AdbEnabled", self._adb_toggle), - ("SshEnabled", self._ssh_toggle), - ("JoystickDebugMode", self._joystick_toggle), - ("LongitudinalManeuverMode", self._long_maneuver_toggle), - ("AlphaLongitudinalEnabled", self._alpha_long_toggle), - ("ShowDebugInfo", self._ui_debug_toggle), - ): - item.action_item.set_state(self._params.get_bool(key)) - - def _on_enable_ui_debug(self, state: bool): - self._params.put_bool("ShowDebugInfo", state) - gui_app.set_show_touches(state) - gui_app.set_show_fps(state) - - def _on_enable_adb(self, state: bool): - self._params.put_bool("AdbEnabled", state) - - def _on_enable_ssh(self, state: bool): - self._params.put_bool("SshEnabled", state) - - def _on_joystick_debug_mode(self, state: bool): - self._params.put_bool("JoystickDebugMode", state) - self._params.put_bool("LongitudinalManeuverMode", False) - self._long_maneuver_toggle.action_item.set_state(False) - - def _on_long_maneuver_mode(self, state: bool): - self._params.put_bool("LongitudinalManeuverMode", state) - self._params.put_bool("JoystickDebugMode", False) - self._joystick_toggle.action_item.set_state(False) - - def _on_alpha_long_enabled(self, state: bool): - if state: - def confirm_callback(result: int): - if result == DialogResult.CONFIRM: - self._params.put_bool("AlphaLongitudinalEnabled", True) - self._params.put_bool("OnroadCycleRequested", True) - self._update_toggles() - else: - self._alpha_long_toggle.action_item.set_state(False) - - # show confirmation dialog - content = (f"

    {self._alpha_long_toggle.title}


    " + - f"

    {self._alpha_long_toggle.description}

    ") - - dlg = ConfirmDialog(content, tr("Enable"), rich=True) - gui_app.set_modal_overlay(dlg, callback=confirm_callback) - - else: - self._params.put_bool("AlphaLongitudinalEnabled", False) - self._params.put_bool("OnroadCycleRequested", True) - self._update_toggles() diff --git a/selfdrive/ui/layouts/settings/device.py b/selfdrive/ui/layouts/settings/device.py deleted file mode 100644 index 00ae6a188ea3fc..00000000000000 --- a/selfdrive/ui/layouts/settings/device.py +++ /dev/null @@ -1,210 +0,0 @@ -import os -import math - -from cereal import messaging, log -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.ui.onroad.driver_camera_dialog import DriverCameraDialog -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.layouts.onboarding import TrainingGuide -from openpilot.selfdrive.ui.widgets.pairing_dialog import PairingDialog -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import FontWeight, gui_app -from openpilot.system.ui.lib.multilang import multilang, tr, tr_noop -from openpilot.system.ui.widgets import Widget, DialogResult -from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog, alert_dialog -from openpilot.system.ui.widgets.html_render import HtmlModal -from openpilot.system.ui.widgets.list_view import text_item, button_item, dual_button_item -from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog -from openpilot.system.ui.widgets.scroller_tici import Scroller - -# Description constants -DESCRIPTIONS = { - 'pair_device': tr_noop("Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer."), - 'driver_camera': tr_noop("Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)"), - 'reset_calibration': tr_noop("openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down."), - 'review_guide': tr_noop("Review the rules, features, and limitations of openpilot"), -} - - -class DeviceLayout(Widget): - def __init__(self): - super().__init__() - - self._params = Params() - self._select_language_dialog: MultiOptionDialog | None = None - self._driver_camera: DriverCameraDialog | None = None - self._pair_device_dialog: PairingDialog | None = None - self._fcc_dialog: HtmlModal | None = None - self._training_guide: TrainingGuide | None = None - - items = self._initialize_items() - self._scroller = Scroller(items, line_separator=True, spacing=0) - - ui_state.add_offroad_transition_callback(self._offroad_transition) - - def _initialize_items(self): - self._pair_device_btn = button_item(lambda: tr("Pair Device"), lambda: tr("PAIR"), lambda: tr(DESCRIPTIONS['pair_device']), callback=self._pair_device) - self._pair_device_btn.set_visible(lambda: not ui_state.prime_state.is_paired()) - - self._reset_calib_btn = button_item(lambda: tr("Reset Calibration"), lambda: tr("RESET"), lambda: tr(DESCRIPTIONS['reset_calibration']), - callback=self._reset_calibration_prompt) - self._reset_calib_btn.set_description_opened_callback(self._update_calib_description) - - self._power_off_btn = dual_button_item(lambda: tr("Reboot"), lambda: tr("Power Off"), - left_callback=self._reboot_prompt, right_callback=self._power_off_prompt) - - items = [ - text_item(lambda: tr("Dongle ID"), self._params.get("DongleId") or (lambda: tr("N/A"))), - text_item(lambda: tr("Serial"), self._params.get("HardwareSerial") or (lambda: tr("N/A"))), - self._pair_device_btn, - button_item(lambda: tr("Driver Camera"), lambda: tr("PREVIEW"), lambda: tr(DESCRIPTIONS['driver_camera']), - callback=self._show_driver_camera, enabled=ui_state.is_offroad), - self._reset_calib_btn, - button_item(lambda: tr("Review Training Guide"), lambda: tr("REVIEW"), lambda: tr(DESCRIPTIONS['review_guide']), - self._on_review_training_guide, enabled=ui_state.is_offroad), - regulatory_btn := button_item(lambda: tr("Regulatory"), lambda: tr("VIEW"), callback=self._on_regulatory, enabled=ui_state.is_offroad), - button_item(lambda: tr("Change Language"), lambda: tr("CHANGE"), callback=self._show_language_dialog), - self._power_off_btn, - ] - regulatory_btn.set_visible(TICI) - return items - - def _offroad_transition(self): - self._power_off_btn.action_item.right_button.set_visible(ui_state.is_offroad()) - - def show_event(self): - self._scroller.show_event() - - def _render(self, rect): - self._scroller.render(rect) - - def _show_language_dialog(self): - def handle_language_selection(result: int): - if result == 1 and self._select_language_dialog: - selected_language = multilang.languages[self._select_language_dialog.selection] - multilang.change_language(selected_language) - self._update_calib_description() - self._select_language_dialog = None - - self._select_language_dialog = MultiOptionDialog(tr("Select a language"), multilang.languages, multilang.codes[multilang.language], - option_font_weight=FontWeight.UNIFONT) - gui_app.set_modal_overlay(self._select_language_dialog, callback=handle_language_selection) - - def _show_driver_camera(self): - if not self._driver_camera: - self._driver_camera = DriverCameraDialog() - - gui_app.set_modal_overlay(self._driver_camera, callback=lambda result: setattr(self, '_driver_camera', None)) - - def _reset_calibration_prompt(self): - if ui_state.engaged: - gui_app.set_modal_overlay(alert_dialog(tr("Disengage to Reset Calibration"))) - return - - def reset_calibration(result: int): - # Check engaged again in case it changed while the dialog was open - if ui_state.engaged or result != DialogResult.CONFIRM: - return - - self._params.remove("CalibrationParams") - self._params.remove("LiveTorqueParameters") - self._params.remove("LiveParameters") - self._params.remove("LiveParametersV2") - self._params.remove("LiveDelay") - self._params.put_bool("OnroadCycleRequested", True) - self._update_calib_description() - - dialog = ConfirmDialog(tr("Are you sure you want to reset calibration?"), tr("Reset")) - gui_app.set_modal_overlay(dialog, callback=reset_calibration) - - def _update_calib_description(self): - desc = tr(DESCRIPTIONS['reset_calibration']) - - calib_bytes = self._params.get("CalibrationParams") - if calib_bytes: - try: - calib = messaging.log_from_bytes(calib_bytes, log.Event).liveCalibration - - if calib.calStatus != log.LiveCalibrationData.Status.uncalibrated: - pitch = math.degrees(calib.rpyCalib[1]) - yaw = math.degrees(calib.rpyCalib[2]) - desc += tr(" Your device is pointed {:.1f}° {} and {:.1f}° {}.").format(abs(pitch), tr("down") if pitch > 0 else tr("up"), - abs(yaw), tr("left") if yaw > 0 else tr("right")) - except Exception: - cloudlog.exception("invalid CalibrationParams") - - lag_perc = 0 - lag_bytes = self._params.get("LiveDelay") - if lag_bytes: - try: - lag_perc = messaging.log_from_bytes(lag_bytes, log.Event).liveDelay.calPerc - except Exception: - cloudlog.exception("invalid LiveDelay") - if lag_perc < 100: - desc += tr("

    Steering lag calibration is {}% complete.").format(lag_perc) - else: - desc += tr("

    Steering lag calibration is complete.") - - torque_bytes = self._params.get("LiveTorqueParameters") - if torque_bytes: - try: - torque = messaging.log_from_bytes(torque_bytes, log.Event).liveTorqueParameters - # don't add for non-torque cars - if torque.useParams: - torque_perc = torque.calPerc - if torque_perc < 100: - desc += tr(" Steering torque response calibration is {}% complete.").format(torque_perc) - else: - desc += tr(" Steering torque response calibration is complete.") - except Exception: - cloudlog.exception("invalid LiveTorqueParameters") - - desc += "

    " - desc += tr("openpilot is continuously calibrating, resetting is rarely required. " + - "Resetting calibration will restart openpilot if the car is powered on.") - - self._reset_calib_btn.set_description(desc) - - def _reboot_prompt(self): - if ui_state.engaged: - gui_app.set_modal_overlay(alert_dialog(tr("Disengage to Reboot"))) - return - - dialog = ConfirmDialog(tr("Are you sure you want to reboot?"), tr("Reboot")) - gui_app.set_modal_overlay(dialog, callback=self._perform_reboot) - - def _perform_reboot(self, result: int): - if not ui_state.engaged and result == DialogResult.CONFIRM: - self._params.put_bool_nonblocking("DoReboot", True) - - def _power_off_prompt(self): - if ui_state.engaged: - gui_app.set_modal_overlay(alert_dialog(tr("Disengage to Power Off"))) - return - - dialog = ConfirmDialog(tr("Are you sure you want to power off?"), tr("Power Off")) - gui_app.set_modal_overlay(dialog, callback=self._perform_power_off) - - def _perform_power_off(self, result: int): - if not ui_state.engaged and result == DialogResult.CONFIRM: - self._params.put_bool_nonblocking("DoShutdown", True) - - def _pair_device(self): - if not self._pair_device_dialog: - self._pair_device_dialog = PairingDialog() - gui_app.set_modal_overlay(self._pair_device_dialog, callback=lambda result: setattr(self, '_pair_device_dialog', None)) - - def _on_regulatory(self): - if not self._fcc_dialog: - self._fcc_dialog = HtmlModal(os.path.join(BASEDIR, "selfdrive/assets/offroad/fcc.html")) - gui_app.set_modal_overlay(self._fcc_dialog) - - def _on_review_training_guide(self): - if not self._training_guide: - def completed_callback(): - gui_app.set_modal_overlay(None) - - self._training_guide = TrainingGuide(completed_callback=completed_callback) - gui_app.set_modal_overlay(self._training_guide) diff --git a/selfdrive/ui/layouts/settings/firehose.py b/selfdrive/ui/layouts/settings/firehose.py deleted file mode 100644 index ea83e962e61878..00000000000000 --- a/selfdrive/ui/layouts/settings/firehose.py +++ /dev/null @@ -1,90 +0,0 @@ -import pyray as rl - -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr, trn, tr_noop -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.selfdrive.ui.mici.layouts.settings.firehose import FirehoseLayoutBase - -TITLE = tr_noop("Firehose Mode") -DESCRIPTION = tr_noop( - "openpilot learns to drive by watching humans, like you, drive.\n\n" - + "Firehose Mode allows you to maximize your training data uploads to improve " - + "openpilot's driving models. More data means bigger models, which means better Experimental Mode." -) -INSTRUCTIONS = tr_noop( - "For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.\n\n" - + "Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.\n\n\n" - + "Frequently Asked Questions\n\n" - + "Does it matter how or where I drive? Nope, just drive as you normally would.\n\n" - + "Do all of my segments get pulled in Firehose Mode? No, we selectively pull a subset of your segments.\n\n" - + "What's a good USB-C adapter? Any fast phone or laptop charger should be fine.\n\n" - + "Does it matter which software I run? Yes, only upstream openpilot (and particular forks) are able to be used for training." -) - - -class FirehoseLayout(FirehoseLayoutBase): - def __init__(self): - super().__init__() - self._scroll_panel = GuiScrollPanel() - - def _render(self, rect: rl.Rectangle): - # Calculate content dimensions - content_rect = rl.Rectangle(rect.x, rect.y, rect.width, self._content_height) - - # Handle scrolling and render with clipping - scroll_offset = self._scroll_panel.update(rect, content_rect) - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - self._content_height = self._render_content(rect, scroll_offset) - rl.end_scissor_mode() - - def _render_content(self, rect: rl.Rectangle, scroll_offset: float) -> int: - x = int(rect.x + 40) - y = int(rect.y + 40 + scroll_offset) - w = int(rect.width - 80) - - # Title (centered) - title_text = tr(TITLE) # live translate - title_font = gui_app.font(FontWeight.MEDIUM) - text_width = measure_text_cached(title_font, title_text, 100).x - title_x = rect.x + (rect.width - text_width) / 2 - rl.draw_text_ex(title_font, title_text, rl.Vector2(title_x, y), 100, 0, rl.WHITE) - y += 200 - - # Description - y = self._draw_wrapped_text(x, y, w, tr(DESCRIPTION), gui_app.font(FontWeight.NORMAL), 45, rl.WHITE) - y += 40 + 20 - - # Separator - rl.draw_rectangle(x, y, w, 2, self.GRAY) - y += 30 + 20 - - # Status - status_text, status_color = self._get_status() - y = self._draw_wrapped_text(x, y, w, status_text, gui_app.font(FontWeight.BOLD), 60, status_color) - y += 20 + 20 - - # Contribution count (if available) - if self._segment_count > 0: - contrib_text = trn("{} segment of your driving is in the training dataset so far.", - "{} segments of your driving is in the training dataset so far.", self._segment_count).format(self._segment_count) - y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 52, rl.WHITE) - y += 20 + 20 - - # Separator - rl.draw_rectangle(x, y, w, 2, self.GRAY) - y += 30 + 20 - - # Instructions - y = self._draw_wrapped_text(x, y, w, tr(INSTRUCTIONS), gui_app.font(FontWeight.NORMAL), 40, self.LIGHT_GRAY) - - # bottom margin + remove effect of scroll offset - return int(round(y - self._scroll_panel.offset + 40)) - - def _draw_wrapped_text(self, x, y, width, text, font, font_size, color): - wrapped = wrap_text(font, text, font_size, width) - for line in wrapped: - rl.draw_text_ex(font, line, rl.Vector2(x, y), font_size, 0, color) - y += font_size * FONT_SCALE - return round(y) diff --git a/selfdrive/ui/layouts/settings/settings.py b/selfdrive/ui/layouts/settings/settings.py deleted file mode 100644 index 68f45df77d0d5f..00000000000000 --- a/selfdrive/ui/layouts/settings/settings.py +++ /dev/null @@ -1,173 +0,0 @@ -import pyray as rl -from dataclasses import dataclass -from enum import IntEnum -from collections.abc import Callable -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 -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.multilang import tr, tr_noop -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 NetworkUI - -# Constants -SIDEBAR_WIDTH = 500 -CLOSE_BTN_SIZE = 200 -CLOSE_ICON_SIZE = 70 -NAV_BTN_HEIGHT = 110 -PANEL_MARGIN = 50 - -# Colors -SIDEBAR_COLOR = rl.BLACK -PANEL_COLOR = rl.Color(41, 41, 41, 255) -CLOSE_BTN_COLOR = rl.Color(41, 41, 41, 255) -CLOSE_BTN_PRESSED = rl.Color(59, 59, 59, 255) -TEXT_NORMAL = rl.Color(128, 128, 128, 255) -TEXT_SELECTED = rl.WHITE - - -class PanelType(IntEnum): - DEVICE = 0 - NETWORK = 1 - TOGGLES = 2 - SOFTWARE = 3 - FIREHOSE = 4 - DEVELOPER = 5 - - -@dataclass -class PanelInfo: - name: str - instance: Widget - button_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) - - -class SettingsLayout(Widget): - def __init__(self): - super().__init__() - self._current_panel = PanelType.DEVICE - - # Panel configuration - wifi_manager = WifiManager() - wifi_manager.set_active(False) - - self._panels = { - PanelType.DEVICE: PanelInfo(tr_noop("Device"), DeviceLayout()), - PanelType.NETWORK: PanelInfo(tr_noop("Network"), NetworkUI(wifi_manager)), - PanelType.TOGGLES: PanelInfo(tr_noop("Toggles"), TogglesLayout()), - PanelType.SOFTWARE: PanelInfo(tr_noop("Software"), SoftwareLayout()), - PanelType.FIREHOSE: PanelInfo(tr_noop("Firehose"), FirehoseLayout()), - PanelType.DEVELOPER: PanelInfo(tr_noop("Developer"), DeveloperLayout()), - } - - self._font_medium = gui_app.font(FontWeight.MEDIUM) - self._close_icon = gui_app.texture("icons/close2.png", CLOSE_ICON_SIZE, CLOSE_ICON_SIZE) - - # Callbacks - self._close_callback: Callable | None = None - - def set_callbacks(self, on_close: Callable): - self._close_callback = on_close - - def _render(self, rect: rl.Rectangle): - # Calculate layout - sidebar_rect = rl.Rectangle(rect.x, rect.y, SIDEBAR_WIDTH, rect.height) - panel_rect = rl.Rectangle(rect.x + SIDEBAR_WIDTH, rect.y, rect.width - SIDEBAR_WIDTH, rect.height) - - # Draw components - self._draw_sidebar(sidebar_rect) - self._draw_current_panel(panel_rect) - - def _draw_sidebar(self, rect: rl.Rectangle): - rl.draw_rectangle_rec(rect, SIDEBAR_COLOR) - - # Close button - close_btn_rect = rl.Rectangle( - rect.x + (rect.width - CLOSE_BTN_SIZE) / 2, rect.y + 60, CLOSE_BTN_SIZE, CLOSE_BTN_SIZE - ) - - pressed = (rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT) and - rl.check_collision_point_rec(rl.get_mouse_position(), close_btn_rect)) - close_color = CLOSE_BTN_PRESSED if pressed else CLOSE_BTN_COLOR - rl.draw_rectangle_rounded(close_btn_rect, 1.0, 20, close_color) - - icon_color = rl.Color(255, 255, 255, 255) if not pressed else rl.Color(220, 220, 220, 255) - icon_dest = rl.Rectangle( - close_btn_rect.x + (close_btn_rect.width - self._close_icon.width) / 2, - close_btn_rect.y + (close_btn_rect.height - self._close_icon.height) / 2, - self._close_icon.width, - self._close_icon.height, - ) - rl.draw_texture_pro( - self._close_icon, - rl.Rectangle(0, 0, self._close_icon.width, self._close_icon.height), - icon_dest, - rl.Vector2(0, 0), - 0, - icon_color, - ) - - # Store close button rect for click detection - self._close_btn_rect = close_btn_rect - - # Navigation buttons - y = rect.y + 300 - for panel_type, panel_info in self._panels.items(): - button_rect = rl.Rectangle(rect.x + 50, y, rect.width - 150, NAV_BTN_HEIGHT) - - # Button styling - is_selected = panel_type == self._current_panel - text_color = TEXT_SELECTED if is_selected else TEXT_NORMAL - # Draw button text (right-aligned) - panel_name = tr(panel_info.name) - text_size = measure_text_cached(self._font_medium, panel_name, 65) - text_pos = rl.Vector2( - button_rect.x + button_rect.width - text_size.x, button_rect.y + (button_rect.height - text_size.y) / 2 - ) - rl.draw_text_ex(self._font_medium, panel_name, text_pos, 65, 0, text_color) - - # Store button rect for click detection - panel_info.button_rect = button_rect - - y += NAV_BTN_HEIGHT - - def _draw_current_panel(self, rect: rl.Rectangle): - rl.draw_rectangle_rounded( - rl.Rectangle(rect.x + 10, rect.y + 10, rect.width - 20, rect.height - 20), 0.04, 30, PANEL_COLOR - ) - content_rect = rl.Rectangle(rect.x + PANEL_MARGIN, rect.y + 25, rect.width - (PANEL_MARGIN * 2), rect.height - 50) - # rl.draw_rectangle_rounded(content_rect, 0.03, 30, PANEL_COLOR) - panel = self._panels[self._current_panel] - if panel.instance: - panel.instance.render(content_rect) - - def _handle_mouse_release(self, mouse_pos: MousePos) -> None: - # Check close button - if rl.check_collision_point_rec(mouse_pos, self._close_btn_rect): - if self._close_callback: - self._close_callback() - return - - # Check navigation buttons - for panel_type, panel_info in self._panels.items(): - if rl.check_collision_point_rec(mouse_pos, panel_info.button_rect): - self.set_current_panel(panel_type) - return - - 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 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/layouts/settings/software.py b/selfdrive/ui/layouts/settings/software.py deleted file mode 100644 index e0df8f27056adb..00000000000000 --- a/selfdrive/ui/layouts/settings/software.py +++ /dev/null @@ -1,203 +0,0 @@ -import os -import time -import datetime -from openpilot.common.time_helpers import system_time_valid -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.multilang import tr, trn -from openpilot.system.ui.widgets import Widget, DialogResult -from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog -from openpilot.system.ui.widgets.list_view import button_item, text_item, ListItem -from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog -from openpilot.system.ui.widgets.scroller_tici import Scroller - -# TODO: remove this. updater fails to respond on startup if time is not correct -UPDATED_TIMEOUT = 10 # seconds to wait for updated to respond - -# Mapping updater internal states to translated display strings -STATE_TO_DISPLAY_TEXT = { - "checking...": tr("checking..."), - "downloading...": tr("downloading..."), - "finalizing update...": tr("finalizing update..."), -} - - -def time_ago(date: datetime.datetime | None) -> str: - if not date: - return tr("never") - - if not system_time_valid(): - return date.strftime("%a %b %d %Y") - - now = datetime.datetime.now(datetime.UTC) - if date.tzinfo is None: - date = date.replace(tzinfo=datetime.UTC) - - diff_seconds = int((now - date).total_seconds()) - if diff_seconds < 60: - return tr("now") - if diff_seconds < 3600: - m = diff_seconds // 60 - return trn("{} minute ago", "{} minutes ago", m).format(m) - if diff_seconds < 86400: - h = diff_seconds // 3600 - return trn("{} hour ago", "{} hours ago", h).format(h) - if diff_seconds < 604800: - d = diff_seconds // 86400 - return trn("{} day ago", "{} days ago", d).format(d) - return date.strftime("%a %b %d %Y") - - -class SoftwareLayout(Widget): - def __init__(self): - super().__init__() - - self._onroad_label = ListItem(lambda: tr("Updates are only downloaded while the car is off.")) - self._version_item = text_item(lambda: tr("Current Version"), ui_state.params.get("UpdaterCurrentDescription") or "") - self._download_btn = button_item(lambda: tr("Download"), lambda: tr("CHECK"), callback=self._on_download_update) - - # Install button is initially hidden - self._install_btn = button_item(lambda: tr("Install Update"), lambda: tr("INSTALL"), callback=self._on_install_update) - self._install_btn.set_visible(False) - - # Track waiting-for-updater transition to avoid brief re-enable while still idle - self._waiting_for_updater = False - self._waiting_start_ts: float = 0.0 - - # Branch switcher - self._branch_btn = button_item(lambda: tr("Target Branch"), lambda: tr("SELECT"), callback=self._on_select_branch) - self._branch_btn.set_visible(not ui_state.params.get_bool("IsTestedBranch")) - self._branch_btn.action_item.set_value(ui_state.params.get("UpdaterTargetBranch") or "") - self._branch_dialog: MultiOptionDialog | None = None - - self._scroller = Scroller([ - self._onroad_label, - self._version_item, - self._download_btn, - self._install_btn, - self._branch_btn, - button_item(lambda: tr("Uninstall"), lambda: tr("UNINSTALL"), callback=self._on_uninstall), - ], line_separator=True, spacing=0) - - def show_event(self): - self._scroller.show_event() - - def _render(self, rect): - self._scroller.render(rect) - - def _update_state(self): - # Show/hide onroad warning - self._onroad_label.set_visible(ui_state.is_onroad()) - - # Update current version and release notes - current_desc = ui_state.params.get("UpdaterCurrentDescription") or "" - current_release_notes = (ui_state.params.get("UpdaterCurrentReleaseNotes") or b"").decode("utf-8", "replace") - self._version_item.action_item.set_text(current_desc) - self._version_item.set_description(current_release_notes) - - # Update download button visibility and state - self._download_btn.set_visible(ui_state.is_offroad()) - - updater_state = ui_state.params.get("UpdaterState") or "idle" - failed_count = ui_state.params.get("UpdateFailedCount") or 0 - fetch_available = ui_state.params.get_bool("UpdaterFetchAvailable") - update_available = ui_state.params.get_bool("UpdateAvailable") - - if updater_state != "idle": - # Updater responded - self._waiting_for_updater = False - self._download_btn.action_item.set_enabled(False) - # Use the mapping, with a fallback to the original state string - display_text = STATE_TO_DISPLAY_TEXT.get(updater_state, updater_state) - self._download_btn.action_item.set_value(display_text) - else: - if failed_count > 0: - self._download_btn.action_item.set_value(tr("failed to check for update")) - self._download_btn.action_item.set_text(tr("CHECK")) - elif fetch_available: - self._download_btn.action_item.set_value(tr("update available")) - self._download_btn.action_item.set_text(tr("DOWNLOAD")) - else: - last_update = ui_state.params.get("LastUpdateTime") - if last_update: - formatted = time_ago(last_update) - self._download_btn.action_item.set_value(tr("up to date, last checked {}").format(formatted)) - else: - self._download_btn.action_item.set_value(tr("up to date, last checked never")) - self._download_btn.action_item.set_text(tr("CHECK")) - - # If we've been waiting too long without a state change, reset state - if self._waiting_for_updater and (time.monotonic() - self._waiting_start_ts > UPDATED_TIMEOUT): - self._waiting_for_updater = False - - # Only enable if we're not waiting for updater to flip out of idle - self._download_btn.action_item.set_enabled(not self._waiting_for_updater) - - # Update target branch button value - current_branch = ui_state.params.get("UpdaterTargetBranch") or "" - self._branch_btn.action_item.set_value(current_branch) - - # Update install button - self._install_btn.set_visible(ui_state.is_offroad() and update_available) - if update_available: - new_desc = ui_state.params.get("UpdaterNewDescription") or "" - new_release_notes = (ui_state.params.get("UpdaterNewReleaseNotes") or b"").decode("utf-8", "replace") - self._install_btn.action_item.set_text(tr("INSTALL")) - self._install_btn.action_item.set_value(new_desc) - self._install_btn.set_description(new_release_notes) - # Enable install button for testing (like Qt showEvent) - self._install_btn.action_item.set_enabled(True) - else: - self._install_btn.set_visible(False) - - def _on_download_update(self): - # Check if we should start checking or start downloading - self._download_btn.action_item.set_enabled(False) - if self._download_btn.action_item.text == tr("CHECK"): - # Start checking for updates - self._waiting_for_updater = True - self._waiting_start_ts = time.monotonic() - os.system("pkill -SIGUSR1 -f system.updated.updated") - else: - # Start downloading - self._waiting_for_updater = True - self._waiting_start_ts = time.monotonic() - os.system("pkill -SIGHUP -f system.updated.updated") - - def _on_uninstall(self): - def handle_uninstall_confirmation(result): - if result == DialogResult.CONFIRM: - ui_state.params.put_bool("DoUninstall", True) - - dialog = ConfirmDialog(tr("Are you sure you want to uninstall?"), tr("Uninstall")) - gui_app.set_modal_overlay(dialog, callback=handle_uninstall_confirmation) - - def _on_install_update(self): - # Trigger reboot to install update - self._install_btn.action_item.set_enabled(False) - ui_state.params.put_bool("DoReboot", True) - - def _on_select_branch(self): - # Get available branches and order - current_git_branch = ui_state.params.get("GitBranch") or "" - branches_str = ui_state.params.get("UpdaterAvailableBranches") or "" - branches = [b for b in branches_str.split(",") if b] - - for b in [current_git_branch, "devel-staging", "devel", "nightly", "nightly-dev", "master"]: - if b in branches: - branches.remove(b) - branches.insert(0, b) - - current_target = ui_state.params.get("UpdaterTargetBranch") or "" - self._branch_dialog = MultiOptionDialog(tr("Select a branch"), branches, current_target) - - def handle_selection(result): - # Confirmed selection - if result == DialogResult.CONFIRM and self._branch_dialog is not None and self._branch_dialog.selection: - selection = self._branch_dialog.selection - ui_state.params.put("UpdaterTargetBranch", selection) - self._branch_btn.action_item.set_value(selection) - os.system("pkill -SIGUSR1 -f system.updated.updated") - self._branch_dialog = None - - gui_app.set_modal_overlay(self._branch_dialog, callback=handle_selection) diff --git a/selfdrive/ui/layouts/settings/toggles.py b/selfdrive/ui/layouts/settings/toggles.py deleted file mode 100644 index 7fae2dfd244a88..00000000000000 --- a/selfdrive/ui/layouts/settings/toggles.py +++ /dev/null @@ -1,244 +0,0 @@ -from cereal import log -from openpilot.common.params import Params, UnknownKeyName -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.list_view import multiple_button_item, toggle_item -from openpilot.system.ui.widgets.scroller_tici import Scroller -from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.multilang import tr, tr_noop -from openpilot.system.ui.widgets import DialogResult -from openpilot.selfdrive.ui.ui_state import ui_state - -PERSONALITY_TO_INT = log.LongitudinalPersonality.schema.enumerants - -# Description constants -DESCRIPTIONS = { - "OpenpilotEnabledToggle": tr_noop( - "Use the openpilot system for adaptive cruise control and lane keep driver assistance. " + - "Your attention is required at all times to use this feature." - ), - "DisengageOnAccelerator": tr_noop("When enabled, pressing the accelerator pedal will disengage openpilot."), - "LongitudinalPersonality": tr_noop( - "Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. " + - "In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " + - "your steering wheel distance button." - ), - "IsLdwEnabled": tr_noop( - "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " + - "without a turn signal activated while driving over 31 mph (50 km/h)." - ), - "AlwaysOnDM": tr_noop("Enable driver monitoring even when openpilot is not engaged."), - 'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), - "IsMetric": tr_noop("Display speed in km/h instead of mph."), - "RecordAudio": tr_noop("Record and store microphone audio while driving. The audio will be included in the dashcam video in comma connect."), -} - - -class TogglesLayout(Widget): - def __init__(self): - super().__init__() - self._params = Params() - self._is_release = self._params.get_bool("IsReleaseBranch") - - # param, title, desc, icon, needs_restart - self._toggle_defs = { - "OpenpilotEnabledToggle": ( - lambda: tr("Enable openpilot"), - DESCRIPTIONS["OpenpilotEnabledToggle"], - "chffr_wheel.png", - True, - ), - "ExperimentalMode": ( - lambda: tr("Experimental Mode"), - "", - "experimental_white.png", - False, - ), - "DisengageOnAccelerator": ( - lambda: tr("Disengage on Accelerator Pedal"), - DESCRIPTIONS["DisengageOnAccelerator"], - "disengage_on_accelerator.png", - False, - ), - "IsLdwEnabled": ( - lambda: tr("Enable Lane Departure Warnings"), - DESCRIPTIONS["IsLdwEnabled"], - "warning.png", - False, - ), - "AlwaysOnDM": ( - lambda: tr("Always-On Driver Monitoring"), - DESCRIPTIONS["AlwaysOnDM"], - "monitoring.png", - False, - ), - "RecordFront": ( - lambda: tr("Record and Upload Driver Camera"), - DESCRIPTIONS["RecordFront"], - "monitoring.png", - True, - ), - "RecordAudio": ( - lambda: tr("Record and Upload Microphone Audio"), - DESCRIPTIONS["RecordAudio"], - "microphone.png", - True, - ), - "IsMetric": ( - lambda: tr("Use Metric System"), - DESCRIPTIONS["IsMetric"], - "metric.png", - False, - ), - } - - self._long_personality_setting = multiple_button_item( - lambda: tr("Driving Personality"), - lambda: tr(DESCRIPTIONS["LongitudinalPersonality"]), - buttons=[lambda: tr("Aggressive"), lambda: tr("Standard"), lambda: tr("Relaxed")], - button_width=255, - callback=self._set_longitudinal_personality, - selected_index=self._params.get("LongitudinalPersonality", return_default=True), - icon="speed_limit.png" - ) - - self._toggles = {} - self._locked_toggles = set() - for param, (title, desc, icon, needs_restart) in self._toggle_defs.items(): - toggle = toggle_item( - title, - desc, - self._params.get_bool(param), - callback=lambda state, p=param: self._toggle_callback(state, p), - icon=icon, - ) - - try: - locked = self._params.get_bool(param + "Lock") - except UnknownKeyName: - locked = False - toggle.action_item.set_enabled(not locked) - - # Make description callable for live translation - additional_desc = "" - if needs_restart and not locked: - additional_desc = tr("Changing this setting will restart openpilot if the car is powered on.") - toggle.set_description(lambda og_desc=toggle.description, add_desc=additional_desc: tr(og_desc) + (" " + tr(add_desc) if add_desc else "")) - - # track for engaged state updates - if locked: - self._locked_toggles.add(param) - - self._toggles[param] = toggle - - # insert longitudinal personality after NDOG toggle - if param == "DisengageOnAccelerator": - self._toggles["LongitudinalPersonality"] = self._long_personality_setting - - self._update_experimental_mode_icon() - self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0) - - ui_state.add_engaged_transition_callback(self._update_toggles) - - def _update_state(self): - if ui_state.sm.updated["selfdriveState"]: - personality = PERSONALITY_TO_INT[ui_state.sm["selfdriveState"].personality] - if personality != ui_state.personality and ui_state.started: - self._long_personality_setting.action_item.set_selected_button(personality) - ui_state.personality = personality - - def show_event(self): - self._scroller.show_event() - self._update_toggles() - - def _update_toggles(self): - ui_state.update_params() - - e2e_description = tr( - "openpilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " + - "Experimental features are listed below:
    " + - "

    End-to-End Longitudinal Control


    " + - "Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. " + - "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; " + - "mistakes should be expected.
    " + - "

    New Driving Visualization


    " + - "The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. " + - "The Experimental mode logo will also be shown in the top right corner." - ) - - if ui_state.CP is not None: - if ui_state.has_longitudinal_control: - self._toggles["ExperimentalMode"].action_item.set_enabled(True) - self._toggles["ExperimentalMode"].set_description(e2e_description) - self._long_personality_setting.action_item.set_enabled(True) - else: - # no long for now - self._toggles["ExperimentalMode"].action_item.set_enabled(False) - self._toggles["ExperimentalMode"].action_item.set_state(False) - self._long_personality_setting.action_item.set_enabled(False) - self._params.remove("ExperimentalMode") - - unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.") - - long_desc = unavailable + " " + tr("openpilot longitudinal control may come in a future update.") - if ui_state.CP.alphaLongitudinalAvailable: - if self._is_release: - long_desc = unavailable + " " + tr("An alpha version of openpilot longitudinal control can be tested, along with " + - "Experimental mode, on non-release branches.") - else: - long_desc = tr("Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.") - - self._toggles["ExperimentalMode"].set_description("" + long_desc + "

    " + e2e_description) - else: - self._toggles["ExperimentalMode"].set_description(e2e_description) - - self._update_experimental_mode_icon() - - # TODO: make a param control list item so we don't need to manage internal state as much here - # refresh toggles from params to mirror external changes - for param in self._toggle_defs: - self._toggles[param].action_item.set_state(self._params.get_bool(param)) - - # these toggles need restart, block while engaged - for toggle_def in self._toggle_defs: - if self._toggle_defs[toggle_def][3] and toggle_def not in self._locked_toggles: - self._toggles[toggle_def].action_item.set_enabled(not ui_state.engaged) - - def _render(self, rect): - self._scroller.render(rect) - - def _update_experimental_mode_icon(self): - icon = "experimental.png" if self._toggles["ExperimentalMode"].action_item.get_state() else "experimental_white.png" - self._toggles["ExperimentalMode"].set_icon(icon) - - def _handle_experimental_mode_toggle(self, state: bool): - confirmed = self._params.get_bool("ExperimentalModeConfirmed") - if state and not confirmed: - def confirm_callback(result: int): - if result == DialogResult.CONFIRM: - self._params.put_bool("ExperimentalMode", True) - self._params.put_bool("ExperimentalModeConfirmed", True) - else: - self._toggles["ExperimentalMode"].action_item.set_state(False) - self._update_experimental_mode_icon() - - # show confirmation dialog - content = (f"

    {self._toggles['ExperimentalMode'].title}


    " + - f"

    {self._toggles['ExperimentalMode'].description}

    ") - dlg = ConfirmDialog(content, tr("Enable"), rich=True) - gui_app.set_modal_overlay(dlg, callback=confirm_callback) - else: - self._update_experimental_mode_icon() - self._params.put_bool("ExperimentalMode", state) - - def _toggle_callback(self, state: bool, param: str): - if param == "ExperimentalMode": - self._handle_experimental_mode_toggle(state) - return - - self._params.put_bool(param, state) - if self._toggle_defs[param][3]: - self._params.put_bool("OnroadCycleRequested", True) - - def _set_longitudinal_personality(self, button_index: int): - self._params.put("LongitudinalPersonality", button_index) diff --git a/selfdrive/ui/layouts/sidebar.py b/selfdrive/ui/layouts/sidebar.py deleted file mode 100644 index 050cd795bffba7..00000000000000 --- a/selfdrive/ui/layouts/sidebar.py +++ /dev/null @@ -1,229 +0,0 @@ -import pyray as rl -import time -from dataclasses import dataclass -from collections.abc import Callable -from cereal import log -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr, tr_noop -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget - -SIDEBAR_WIDTH = 300 -METRIC_HEIGHT = 126 -METRIC_WIDTH = 240 -METRIC_MARGIN = 30 -FONT_SIZE = 35 - -SETTINGS_BTN = rl.Rectangle(50, 35, 200, 117) -HOME_BTN = rl.Rectangle(60, 860, 180, 180) - -ThermalStatus = log.DeviceState.ThermalStatus -NetworkType = log.DeviceState.NetworkType - - -# Color scheme -class Colors: - WHITE = rl.WHITE - WHITE_DIM = rl.Color(255, 255, 255, 85) - GRAY = rl.Color(84, 84, 84, 255) - - # Status colors - GOOD = rl.WHITE - WARNING = rl.Color(218, 202, 37, 255) - DANGER = rl.Color(201, 34, 49, 255) - - # UI elements - METRIC_BORDER = rl.Color(255, 255, 255, 85) - BUTTON_NORMAL = rl.WHITE - BUTTON_PRESSED = rl.Color(255, 255, 255, 166) - - -NETWORK_TYPES = { - NetworkType.none: tr_noop("--"), - NetworkType.wifi: tr_noop("Wi-Fi"), - NetworkType.ethernet: tr_noop("ETH"), - NetworkType.cell2G: tr_noop("2G"), - NetworkType.cell3G: tr_noop("3G"), - NetworkType.cell4G: tr_noop("LTE"), - NetworkType.cell5G: tr_noop("5G"), -} - - -@dataclass(slots=True) -class MetricData: - label: str - value: str - color: rl.Color - - def update(self, label: str, value: str, color: rl.Color): - self.label = label - self.value = value - self.color = color - - -class Sidebar(Widget): - def __init__(self): - super().__init__() - self._net_type = NETWORK_TYPES.get(NetworkType.none) - self._net_strength = 0 - - self._temp_status = MetricData(tr_noop("TEMP"), tr_noop("GOOD"), Colors.GOOD) - self._panda_status = MetricData(tr_noop("VEHICLE"), tr_noop("ONLINE"), Colors.GOOD) - self._connect_status = MetricData(tr_noop("CONNECT"), tr_noop("OFFLINE"), Colors.WARNING) - self._recording_audio = False - - self._home_img = gui_app.texture("images/button_home.png", HOME_BTN.width, HOME_BTN.height) - self._flag_img = gui_app.texture("images/button_flag.png", HOME_BTN.width, HOME_BTN.height) - self._settings_img = gui_app.texture("images/button_settings.png", SETTINGS_BTN.width, SETTINGS_BTN.height) - self._mic_img = gui_app.texture("icons/microphone.png", 30, 30) - self._mic_indicator_rect = rl.Rectangle(0, 0, 0, 0) - self._font_regular = gui_app.font(FontWeight.NORMAL) - self._font_bold = gui_app.font(FontWeight.SEMI_BOLD) - - # Callbacks - self._on_settings_click: Callable | None = None - self._on_flag_click: Callable | None = None - self._open_settings_callback: Callable | None = None - - def set_callbacks(self, on_settings: Callable | None = None, on_flag: Callable | None = None, - open_settings: Callable | None = None): - self._on_settings_click = on_settings - self._on_flag_click = on_flag - self._open_settings_callback = open_settings - - def _render(self, rect: rl.Rectangle): - # Background - rl.draw_rectangle_rec(rect, rl.BLACK) - - self._draw_buttons(rect) - self._draw_network_indicator(rect) - self._draw_metrics(rect) - - def _update_state(self): - sm = ui_state.sm - if not sm.updated['deviceState']: - return - - device_state = sm['deviceState'] - - self._recording_audio = ui_state.recording_audio - self._update_network_status(device_state) - self._update_temperature_status(device_state) - self._update_connection_status(device_state) - self._update_panda_status() - - def _update_network_status(self, device_state): - self._net_type = NETWORK_TYPES.get(device_state.networkType.raw, tr_noop("Unknown")) - strength = device_state.networkStrength - self._net_strength = max(0, min(5, strength.raw + 1)) if strength.raw > 0 else 0 - - def _update_temperature_status(self, device_state): - thermal_status = device_state.thermalStatus - - if thermal_status == ThermalStatus.green: - self._temp_status.update(tr_noop("TEMP"), tr_noop("GOOD"), Colors.GOOD) - elif thermal_status == ThermalStatus.yellow: - self._temp_status.update(tr_noop("TEMP"), tr_noop("OK"), Colors.WARNING) - else: - self._temp_status.update(tr_noop("TEMP"), tr_noop("HIGH"), Colors.DANGER) - - def _update_connection_status(self, device_state): - last_ping = device_state.lastAthenaPingTime - if last_ping == 0: - self._connect_status.update(tr_noop("CONNECT"), tr_noop("OFFLINE"), Colors.WARNING) - elif time.monotonic_ns() - last_ping < 80_000_000_000: # 80 seconds in nanoseconds - self._connect_status.update(tr_noop("CONNECT"), tr_noop("ONLINE"), Colors.GOOD) - else: - self._connect_status.update(tr_noop("CONNECT"), tr_noop("ERROR"), Colors.DANGER) - - def _update_panda_status(self): - if ui_state.panda_type == log.PandaState.PandaType.unknown: - self._panda_status.update(tr_noop("NO"), tr_noop("PANDA"), Colors.DANGER) - else: - self._panda_status.update(tr_noop("VEHICLE"), tr_noop("ONLINE"), Colors.GOOD) - - def _handle_mouse_release(self, mouse_pos: MousePos): - if rl.check_collision_point_rec(mouse_pos, SETTINGS_BTN): - if self._on_settings_click: - self._on_settings_click() - elif rl.check_collision_point_rec(mouse_pos, HOME_BTN) and ui_state.started: - if self._on_flag_click: - self._on_flag_click() - elif self._recording_audio and rl.check_collision_point_rec(mouse_pos, self._mic_indicator_rect): - if self._open_settings_callback: - self._open_settings_callback() - - def _draw_buttons(self, rect: rl.Rectangle): - mouse_pos = rl.get_mouse_position() - mouse_down = self.is_pressed and rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT) - - # Settings button - settings_down = mouse_down and rl.check_collision_point_rec(mouse_pos, SETTINGS_BTN) - tint = Colors.BUTTON_PRESSED if settings_down else Colors.BUTTON_NORMAL - rl.draw_texture(self._settings_img, int(SETTINGS_BTN.x), int(SETTINGS_BTN.y), tint) - - # Home/Flag button - flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN) - button_img = self._flag_img if ui_state.started else self._home_img - - tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL - rl.draw_texture(button_img, int(HOME_BTN.x), int(HOME_BTN.y), tint) - - # Microphone button - if self._recording_audio: - self._mic_indicator_rect = rl.Rectangle(rect.x + rect.width - 130, rect.y + 245, 75, 40) - - mic_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, self._mic_indicator_rect) - bg_color = rl.Color(Colors.DANGER.r, Colors.DANGER.g, Colors.DANGER.b, int(255 * 0.65)) if mic_pressed else Colors.DANGER - - rl.draw_rectangle_rounded(self._mic_indicator_rect, 1, 10, bg_color) - rl.draw_texture(self._mic_img, int(self._mic_indicator_rect.x + (self._mic_indicator_rect.width - self._mic_img.width) / 2), - int(self._mic_indicator_rect.y + (self._mic_indicator_rect.height - self._mic_img.height) / 2), Colors.WHITE) - - def _draw_network_indicator(self, rect: rl.Rectangle): - # Signal strength dots - x_start = rect.x + 58 - y_pos = rect.y + 196 - dot_size = 27 - dot_spacing = 37 - - for i in range(5): - color = Colors.WHITE if i < self._net_strength else Colors.GRAY - x = int(x_start + i * dot_spacing + dot_size // 2) - y = int(y_pos + dot_size // 2) - rl.draw_circle(x, y, dot_size // 2, color) - - # Network type text - text_y = rect.y + 247 - text_pos = rl.Vector2(rect.x + 58, text_y) - rl.draw_text_ex(self._font_regular, tr(self._net_type), text_pos, FONT_SIZE, 0, Colors.WHITE) - - def _draw_metrics(self, rect: rl.Rectangle): - metrics = [(self._temp_status, 338), (self._panda_status, 496), (self._connect_status, 654)] - - for metric, y_offset in metrics: - self._draw_metric(rect, metric, rect.y + y_offset) - - def _draw_metric(self, rect: rl.Rectangle, metric: MetricData, y: float): - metric_rect = rl.Rectangle(rect.x + METRIC_MARGIN, y, METRIC_WIDTH, METRIC_HEIGHT) - # Draw colored left edge (clipped rounded rectangle) - edge_rect = rl.Rectangle(metric_rect.x + 4, metric_rect.y + 4, 100, 118) - rl.begin_scissor_mode(int(metric_rect.x + 4), int(metric_rect.y), 18, int(metric_rect.height)) - rl.draw_rectangle_rounded(edge_rect, 0.3, 10, metric.color) - rl.end_scissor_mode() - - # Draw border - rl.draw_rectangle_rounded_lines_ex(metric_rect, 0.3, 10, 2, Colors.METRIC_BORDER) - - # Draw label and value - labels = [tr(metric.label), tr(metric.value)] - text_y = metric_rect.y + (metric_rect.height / 2 - len(labels) * FONT_SIZE * FONT_SCALE) - for text in labels: - text_size = measure_text_cached(self._font_bold, text, FONT_SIZE) - text_y += text_size.y - text_pos = rl.Vector2( - metric_rect.x + 22 + (metric_rect.width - 22 - text_size.x) / 2, - text_y - ) - rl.draw_text_ex(self._font_bold, text, text_pos, FONT_SIZE, 0, Colors.WHITE) diff --git a/selfdrive/ui/lib/api_helpers.py b/selfdrive/ui/lib/api_helpers.py deleted file mode 100644 index 8ed1c22a6353ac..00000000000000 --- a/selfdrive/ui/lib/api_helpers.py +++ /dev/null @@ -1,18 +0,0 @@ -import time -from functools import lru_cache -from openpilot.common.api import Api -from openpilot.common.time_helpers import system_time_valid - -TOKEN_EXPIRY_HOURS = 2 - - -@lru_cache(maxsize=1) -def _get_token(dongle_id: str, t: int): - if not system_time_valid(): - raise RuntimeError("System time is not valid, cannot generate token") - - 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 deleted file mode 100644 index 1aed949bee922b..00000000000000 --- a/selfdrive/ui/lib/prime_state.py +++ /dev/null @@ -1,107 +0,0 @@ -from enum import IntEnum -import os -import requests -import threading -import time - -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 -from openpilot.selfdrive.ui.lib.api_helpers import get_token - - -class PrimeType(IntEnum): - UNKNOWN = -2 - UNPAIRED = -1 - NONE = 0 - MAGENTA = 1 - LITE = 2 - BLUE = 3 - MAGENTA_NEW = 4 - PURPLE = 5 - - -class PrimeState: - FETCH_INTERVAL = 5.0 # seconds between API calls - API_TIMEOUT = 10.0 # seconds for API requests - SLEEP_INTERVAL = 0.5 # seconds to sleep between checks in the worker thread - - def __init__(self): - self._params = Params() - self._lock = threading.Lock() - self._session = requests.Session() # reuse session to reduce SSL handshake overhead - self.prime_type: PrimeType = self._load_initial_state() - - self._running = False - self._thread = None - - def _load_initial_state(self) -> PrimeType: - prime_type_str = os.getenv("PRIME_TYPE") or self._params.get("PrimeType") - try: - if prime_type_str is not None: - return PrimeType(int(prime_type_str)) - except (ValueError, TypeError): - pass - return PrimeType.UNKNOWN - - def _fetch_prime_status(self) -> None: - dongle_id = self._params.get("DongleId") - if not dongle_id or dongle_id == UNREGISTERED_DONGLE_ID: - return - - try: - identity_token = get_token(dongle_id) - response = api_get(f"v1.1/devices/{dongle_id}", timeout=self.API_TIMEOUT, access_token=identity_token, session=self._session) - 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) - except Exception as e: - cloudlog.error(f"Failed to fetch prime status: {e}") - - def set_type(self, prime_type: PrimeType) -> None: - with self._lock: - if prime_type != self.prime_type: - self.prime_type = prime_type - self._params.put("PrimeType", int(prime_type)) - cloudlog.info(f"Prime type updated to {prime_type}") - - def _worker_thread(self) -> None: - from openpilot.selfdrive.ui.ui_state import ui_state, device - while self._running: - if not ui_state.started and device._awake: - self._fetch_prime_status() - - for _ in range(int(self.FETCH_INTERVAL / self.SLEEP_INTERVAL)): - if not self._running: - break - time.sleep(self.SLEEP_INTERVAL) - - def start(self) -> None: - if self._thread and self._thread.is_alive(): - return - self._running = True - self._thread = threading.Thread(target=self._worker_thread, daemon=True) - self._thread.start() - - def stop(self) -> None: - self._running = False - if self._thread and self._thread.is_alive(): - self._thread.join(timeout=1.0) - - def get_type(self) -> PrimeType: - with self._lock: - return self.prime_type - - def is_prime(self) -> bool: - with self._lock: - return bool(self.prime_type > PrimeType.NONE) - - def is_paired(self) -> bool: - with self._lock: - return self.prime_type > PrimeType.UNPAIRED - - def __del__(self): - self.stop() diff --git a/selfdrive/ui/main.cc b/selfdrive/ui/main.cc new file mode 100644 index 00000000000000..ed54d5aa19206f --- /dev/null +++ b/selfdrive/ui/main.cc @@ -0,0 +1,30 @@ +#include + +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/window.h" + +int main(int argc, char *argv[]) { + setpriority(PRIO_PROCESS, 0, -20); + + qInstallMessageHandler(swagLogMessageHandler); + initApp(argc, argv); + + QTranslator translator; + QString translation_file = QString::fromStdString(Params().get("LanguageSetting")); + if (!translator.load(translation_file, "translations") && translation_file.length()) { + qCritical() << "Failed to load translation file:" << translation_file; + } + + QApplication a(argc, argv); + a.installTranslator(&translator); + + MainWindow w; + setMainWindow(&w); + a.installEventFilter(&w); + return a.exec(); +} diff --git a/selfdrive/ui/mici/layouts/home.py b/selfdrive/ui/mici/layouts/home.py deleted file mode 100644 index f5dab7249a4042..00000000000000 --- a/selfdrive/ui/mici/layouts/home.py +++ /dev/null @@ -1,265 +0,0 @@ -import time - -from cereal import log -import pyray as rl -from collections.abc import Callable -from openpilot.system.ui.widgets.label import gui_label, MiciLabel, UnifiedLabel -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.lib.application import gui_app, FontWeight, DEFAULT_TEXT_COLOR, MousePos -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.text import wrap_text -from openpilot.system.version import training_version, RELEASE_BRANCHES - -HEAD_BUTTON_FONT_SIZE = 40 -HOME_PADDING = 8 - -NetworkType = log.DeviceState.NetworkType - -NETWORK_TYPES = { - NetworkType.none: "Offline", - NetworkType.wifi: "WiFi", - NetworkType.cell2G: "2G", - NetworkType.cell3G: "3G", - NetworkType.cell4G: "LTE", - NetworkType.cell5G: "5G", - NetworkType.ethernet: "Ethernet", -} - - -class DeviceStatus(Widget): - def __init__(self): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, 300, 175)) - self._update_state() - self._version_text = self._get_version_text() - - self._do_welcome() - - def _do_welcome(self): - ui_state.params.put("CompletedTrainingVersion", training_version) - - def refresh(self): - self._update_state() - self._version_text = self._get_version_text() - - def _get_version_text(self) -> str: - brand = "openpilot" - description = ui_state.params.get("UpdaterCurrentDescription") - return f"{brand} {description}" if description else brand - - def _update_state(self): - # TODO: refresh function that can be called periodically, not at 60 fps, so we can update version - # update system status - self._system_status = "SYSTEM READY ✓" if ui_state.panda_type != log.PandaState.PandaType.unknown else "BOOTING UP..." - - # update network status - strength = ui_state.sm['deviceState'].networkStrength.raw - strength_text = "● " * strength + "○ " * (4 - strength) # ◌ also works - network_type = NETWORK_TYPES[ui_state.sm['deviceState'].networkType.raw] - self._network_status = f"{network_type} {strength_text}" - - def _render(self, _): - # draw status - status_rect = rl.Rectangle(self._rect.x, self._rect.y, self._rect.width, 40) - gui_label(status_rect, self._system_status, font_size=HEAD_BUTTON_FONT_SIZE, color=DEFAULT_TEXT_COLOR, - font_weight=FontWeight.BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - - # draw network status - network_rect = rl.Rectangle(self._rect.x, self._rect.y + 60, self._rect.width, 40) - gui_label(network_rect, self._network_status, font_size=40, color=DEFAULT_TEXT_COLOR, - font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - - # draw version - version_font_size = 30 - version_rect = rl.Rectangle(self._rect.x, self._rect.y + 140, self._rect.width + 20, 40) - wrapped_text = '\n'.join(wrap_text(self._version_text, version_font_size, version_rect.width)) - gui_label(version_rect, wrapped_text, font_size=version_font_size, color=DEFAULT_TEXT_COLOR, - font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT) - - -class MiciHomeLayout(Widget): - def __init__(self): - super().__init__() - self._on_settings_click: Callable | None = None - - self._last_refresh = 0 - self._mouse_down_t: None | float = None - self._did_long_press = False - self._is_pressed_prev = False - - self._version_text = None - self._experimental_mode = False - - self._settings_txt = gui_app.texture("icons_mici/settings.png", 48, 48) - self._experimental_txt = gui_app.texture("icons_mici/experimental_mode.png", 48, 48) - self._mic_txt = gui_app.texture("icons_mici/microphone.png", 32, 46) - - self._net_type = NETWORK_TYPES.get(NetworkType.none) - self._net_strength = 0 - - self._wifi_slash_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_slash.png", 50, 44) - self._wifi_none_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_none.png", 50, 37) - self._wifi_low_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_low.png", 50, 37) - self._wifi_medium_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_medium.png", 50, 37) - self._wifi_full_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 50, 37) - - self._cell_none_txt = gui_app.texture("icons_mici/settings/network/cell_strength_none.png", 54, 36) - self._cell_low_txt = gui_app.texture("icons_mici/settings/network/cell_strength_low.png", 54, 36) - self._cell_medium_txt = gui_app.texture("icons_mici/settings/network/cell_strength_medium.png", 54, 36) - self._cell_high_txt = gui_app.texture("icons_mici/settings/network/cell_strength_high.png", 54, 36) - self._cell_full_txt = gui_app.texture("icons_mici/settings/network/cell_strength_full.png", 54, 36) - - self._openpilot_label = MiciLabel("openpilot", font_size=96, color=rl.Color(255, 255, 255, int(255 * 0.9)), font_weight=FontWeight.DISPLAY) - self._version_label = MiciLabel("", font_size=36, font_weight=FontWeight.ROMAN) - self._large_version_label = MiciLabel("", font_size=64, color=rl.GRAY, font_weight=FontWeight.ROMAN) - self._date_label = MiciLabel("", font_size=36, color=rl.GRAY, font_weight=FontWeight.ROMAN) - self._branch_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, scroll=True) - self._version_commit_label = MiciLabel("", font_size=36, color=rl.GRAY, font_weight=FontWeight.ROMAN) - - def show_event(self): - self._version_text = self._get_version_text() - self._update_network_status(ui_state.sm['deviceState']) - self._update_params() - - def _update_params(self): - self._experimental_mode = ui_state.params.get_bool("ExperimentalMode") - - def _update_state(self): - if self.is_pressed and not self._is_pressed_prev: - self._mouse_down_t = time.monotonic() - elif not self.is_pressed and self._is_pressed_prev: - self._mouse_down_t = None - self._did_long_press = False - self._is_pressed_prev = self.is_pressed - - if self._mouse_down_t is not None: - if time.monotonic() - self._mouse_down_t > 0.5: - # long gating for experimental mode - only allow toggle if longitudinal control is available - if ui_state.has_longitudinal_control: - self._experimental_mode = not self._experimental_mode - ui_state.params.put("ExperimentalMode", self._experimental_mode) - self._mouse_down_t = None - self._did_long_press = True - - if rl.get_time() - self._last_refresh > 5.0: - device_state = ui_state.sm['deviceState'] - self._update_network_status(device_state) - - # Update version text - self._version_text = self._get_version_text() - self._last_refresh = rl.get_time() - self._update_params() - - def _update_network_status(self, device_state): - self._net_type = device_state.networkType - strength = device_state.networkStrength - self._net_strength = max(0, min(5, strength.raw + 1)) if strength.raw > 0 else 0 - - def set_callbacks(self, on_settings: Callable | None = None): - self._on_settings_click = on_settings - - def _handle_mouse_release(self, mouse_pos: MousePos): - if not self._did_long_press: - if self._on_settings_click: - self._on_settings_click() - self._did_long_press = False - - def _get_version_text(self) -> tuple[str, str, str, str] | None: - description = ui_state.params.get("UpdaterCurrentDescription") - - if description is not None and len(description) > 0: - # Expect "version / branch / commit / date"; be tolerant of other formats - try: - version, branch, commit, date = description.split(" / ") - return version, branch, commit, date - except Exception: - return None - - return None - - def _render(self, _): - # TODO: why is there extra space here to get it to be flush? - text_pos = rl.Vector2(self.rect.x - 2 + HOME_PADDING, self.rect.y - 16) - self._openpilot_label.set_position(text_pos.x, text_pos.y) - self._openpilot_label.render() - - if self._version_text is not None: - # release branch - release_branch = self._version_text[1] in RELEASE_BRANCHES - version_pos = rl.Rectangle(text_pos.x, text_pos.y + self._openpilot_label.font_size + 16, 100, 44) - self._version_label.set_text(self._version_text[0]) - self._version_label.set_position(version_pos.x, version_pos.y) - self._version_label.render() - - self._date_label.set_text(" " + self._version_text[3]) - self._date_label.set_position(version_pos.x + self._version_label.rect.width + 10, version_pos.y) - self._date_label.render() - - self._branch_label.set_max_width(gui_app.width - self._version_label.rect.width - self._date_label.rect.width - 32) - self._branch_label.set_text(" " + ("release" if release_branch else self._version_text[1])) - self._branch_label.set_position(version_pos.x + self._version_label.rect.width + self._date_label.rect.width + 20, version_pos.y) - self._branch_label.render() - - if not release_branch: - # 2nd line - self._version_commit_label.set_text(self._version_text[2]) - self._version_commit_label.set_position(version_pos.x, version_pos.y + self._date_label.font_size + 7) - self._version_commit_label.render() - - self._render_bottom_status_bar() - - def _render_bottom_status_bar(self): - # ***** Center-aligned bottom section icons ***** - - # TODO: refactor repeated icon drawing into a small loop - ITEM_SPACING = 18 - Y_CENTER = 24 - - last_x = self.rect.x + HOME_PADDING - - # Draw settings icon in bottom left corner - rl.draw_texture(self._settings_txt, int(last_x), int(self._rect.y + self.rect.height - self._settings_txt.height / 2 - Y_CENTER), - rl.Color(255, 255, 255, int(255 * 0.9))) - last_x = last_x + self._settings_txt.width + ITEM_SPACING - - # draw network - if self._net_type == NetworkType.wifi: - # There is no 1 - draw_net_txt = {0: self._wifi_none_txt, - 2: self._wifi_low_txt, - 3: self._wifi_medium_txt, - 4: self._wifi_full_txt, - 5: self._wifi_full_txt}.get(self._net_strength, self._wifi_low_txt) - rl.draw_texture(draw_net_txt, int(last_x), - int(self._rect.y + self.rect.height - draw_net_txt.height / 2 - Y_CENTER), rl.Color(255, 255, 255, int(255 * 0.9))) - last_x += draw_net_txt.width + ITEM_SPACING - - elif self._net_type in (NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G): - draw_net_txt = {0: self._cell_none_txt, - 2: self._cell_low_txt, - 3: self._cell_medium_txt, - 4: self._cell_high_txt, - 5: self._cell_full_txt}.get(self._net_strength, self._cell_none_txt) - rl.draw_texture(draw_net_txt, int(last_x), - int(self._rect.y + self.rect.height - draw_net_txt.height / 2 - Y_CENTER), rl.Color(255, 255, 255, int(255 * 0.9))) - last_x += draw_net_txt.width + ITEM_SPACING - - else: - # No network - # Offset by difference in height between slashless and slash icons to make center align match - rl.draw_texture(self._wifi_slash_txt, int(last_x), int(self._rect.y + self.rect.height - self._wifi_slash_txt.height / 2 - - (self._wifi_slash_txt.height - self._wifi_none_txt.height) / 2 - Y_CENTER), - rl.Color(255, 255, 255, 255)) - last_x += self._wifi_slash_txt.width + ITEM_SPACING - - # draw experimental icon - if self._experimental_mode: - rl.draw_texture(self._experimental_txt, int(last_x), - int(self._rect.y + self.rect.height - self._experimental_txt.height / 2 - Y_CENTER), rl.Color(255, 255, 255, 255)) - last_x += self._experimental_txt.width + ITEM_SPACING - - # draw microphone icon when recording audio is enabled - if ui_state.recording_audio: - rl.draw_texture(self._mic_txt, int(last_x), - int(self._rect.y + self.rect.height - self._mic_txt.height / 2 - Y_CENTER), rl.Color(255, 255, 255, 255)) - last_x += self._mic_txt.width + ITEM_SPACING diff --git a/selfdrive/ui/mici/layouts/main.py b/selfdrive/ui/mici/layouts/main.py deleted file mode 100644 index b52f9ed39a06f9..00000000000000 --- a/selfdrive/ui/mici/layouts/main.py +++ /dev/null @@ -1,149 +0,0 @@ -import pyray as rl -from enum import IntEnum -import cereal.messaging as messaging -from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout -from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout -from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts -from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView -from openpilot.selfdrive.ui.ui_state import device, ui_state -from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.system.ui.lib.application import gui_app - - -ONROAD_DELAY = 2.5 # seconds - - -class MainState(IntEnum): - MAIN = 0 - SETTINGS = 1 - - -class MiciMainLayout(Widget): - def __init__(self): - super().__init__() - - self._pm = messaging.PubMaster(['bookmarkButton']) - - self._current_mode: MainState | None = None - self._prev_onroad = False - self._prev_standstill = False - self._onroad_time_delay: float | None = None - self._setup = False - - # Initialize widgets - self._home_layout = MiciHomeLayout() - self._alerts_layout = MiciOffroadAlerts() - self._settings_layout = SettingsLayout() - self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked) - - # Initialize widget rects - for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout): - # TODO: set parent rect and use it if never passed rect from render (like in Scroller) - widget.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - - self._scroller = Scroller([ - self._alerts_layout, - self._home_layout, - self._onroad_layout, - ], spacing=0, pad_start=0, pad_end=0) - self._scroller.set_reset_scroll_at_show(False) - - # Disable scrolling when onroad is interacting with bookmark - self._scroller.set_scrolling_enabled(lambda: not self._onroad_layout.is_swiping_left()) - - self._layouts = { - MainState.MAIN: self._scroller, - MainState.SETTINGS: self._settings_layout, - } - - # Set callbacks - self._setup_callbacks() - - # Start onboarding if terms or training not completed - self._onboarding_window = OnboardingWindow() - if not self._onboarding_window.completed: - gui_app.set_modal_overlay(self._onboarding_window) - - def _setup_callbacks(self): - self._home_layout.set_callbacks(on_settings=self._on_settings_clicked) - self._settings_layout.set_callbacks(on_close=self._on_settings_closed) - self._onroad_layout.set_click_callback(lambda: self._scroll_to(self._home_layout)) - device.add_interactive_timeout_callback(self._set_mode_for_started) - - def _scroll_to(self, layout: Widget): - layout_x = int(layout.rect.x) - self._scroller.scroll_to(layout_x, smooth=True) - - def _render(self, _): - # Initial show event - if self._current_mode is None: - self._set_mode(MainState.MAIN) - - if not self._setup: - if self._alerts_layout.active_alerts() > 0: - self._scroller.scroll_to(self._alerts_layout.rect.x) - else: - self._scroller.scroll_to(self._rect.width) - self._setup = True - - # Render - if self._current_mode == MainState.MAIN: - self._scroller.render(self._rect) - - elif self._current_mode == MainState.SETTINGS: - self._settings_layout.render(self._rect) - - self._handle_transitions() - - def _set_mode(self, mode: MainState): - if mode != self._current_mode: - if self._current_mode is not None: - self._layouts[self._current_mode].hide_event() - self._layouts[mode].show_event() - self._current_mode = mode - - def _handle_transitions(self): - if ui_state.started != self._prev_onroad: - self._prev_onroad = ui_state.started - - if ui_state.started: - self._onroad_time_delay = rl.get_time() - else: - self._set_mode_for_started(True) - - # delay so we show home for a bit after starting - if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY: - self._set_mode_for_started(True) - self._onroad_time_delay = None - - CS = ui_state.sm["carState"] - if not CS.standstill and self._prev_standstill: - self._set_mode(MainState.MAIN) - self._scroll_to(self._onroad_layout) - self._prev_standstill = CS.standstill - - def _set_mode_for_started(self, onroad_transition: bool = False): - if ui_state.started: - CS = ui_state.sm["carState"] - # Only go onroad if car starts or is not at a standstill - if not CS.standstill or onroad_transition: - self._set_mode(MainState.MAIN) - self._scroll_to(self._onroad_layout) - else: - # Stay in settings if car turns off while in settings - if not onroad_transition or self._current_mode != MainState.SETTINGS: - self._set_mode(MainState.MAIN) - self._scroll_to(self._home_layout) - - def _on_settings_clicked(self): - self._set_mode(MainState.SETTINGS) - - def _on_settings_closed(self): - self._set_mode(MainState.MAIN) - - def _on_bookmark_clicked(self): - user_bookmark = messaging.new_message('bookmarkButton') - user_bookmark.valid = True - self._pm.send('bookmarkButton', user_bookmark) diff --git a/selfdrive/ui/mici/layouts/offroad_alerts.py b/selfdrive/ui/mici/layouts/offroad_alerts.py deleted file mode 100644 index 60f64b31b064ce..00000000000000 --- a/selfdrive/ui/mici/layouts/offroad_alerts.py +++ /dev/null @@ -1,309 +0,0 @@ -import pyray as rl -import re -import time -from dataclasses import dataclass -from enum import IntEnum -from openpilot.common.params import Params -from openpilot.selfdrive.selfdrived.alertmanager import OFFROAD_ALERTS -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr - -REFRESH_INTERVAL = 5.0 # seconds - - -class AlertSize(IntEnum): - SMALL = 0 - MEDIUM = 1 - BIG = 2 - - -@dataclass -class AlertData: - key: str - text: str - severity: int - visible: bool = False - - -class AlertItem(Widget): - # TODO: click should always go somewhere: home or specific settings pane - """Individual alert item widget with background image and text.""" - ALERT_WIDTH = 520 - ALERT_HEIGHT_SMALL = 212 - ALERT_HEIGHT_MED = 240 - ALERT_HEIGHT_BIG = 324 - ALERT_PADDING = 28 - ICON_SIZE = 64 - ICON_MARGIN = 12 - TEXT_COLOR = rl.Color(255, 255, 255, int(255 * 0.9)) - TITLE_BODY_SPACING = 24 - - def __init__(self, alert_data: AlertData): - super().__init__() - self.alert_data = alert_data - - # Load background textures - self._bg_small = gui_app.texture("icons_mici/offroad_alerts/small_alert.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_SMALL) - self._bg_small_pressed = gui_app.texture("icons_mici/offroad_alerts/small_alert_pressed.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_SMALL) - self._bg_medium = gui_app.texture("icons_mici/offroad_alerts/medium_alert.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_MED) - self._bg_medium_pressed = gui_app.texture("icons_mici/offroad_alerts/medium_alert_pressed.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_MED) - self._bg_big = gui_app.texture("icons_mici/offroad_alerts/big_alert.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_BIG) - self._bg_big_pressed = gui_app.texture("icons_mici/offroad_alerts/big_alert_pressed.png", self.ALERT_WIDTH, self.ALERT_HEIGHT_BIG) - - # Load warning icons - self._icon_orange = gui_app.texture("icons_mici/offroad_alerts/orange_warning.png", self.ICON_SIZE, self.ICON_SIZE) - self._icon_red = gui_app.texture("icons_mici/offroad_alerts/red_warning.png", self.ICON_SIZE, self.ICON_SIZE) - self._icon_green = gui_app.texture("icons_mici/offroad_alerts/green_wheel.png", self.ICON_SIZE, self.ICON_SIZE) - - self._title_label = UnifiedLabel(text="", font_size=32, font_weight=FontWeight.SEMI_BOLD, text_color=self.TEXT_COLOR, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, line_height=0.95) - - self._body_label = UnifiedLabel(text="", font_size=28, font_weight=FontWeight.ROMAN, text_color=self.TEXT_COLOR, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, line_height=0.95) - - self._title_text = "" - self._body_text = "" - self._alert_size = AlertSize.SMALL - - self._update_content() - - def _split_text(self, text: str) -> tuple[str, str]: - """Split text into title (first sentence) and body (remaining text).""" - # Find the end of the first sentence (period, exclamation, or question mark followed by space or end) - match = re.search(r'[.!?](?:\s+|$)', text) - if match: - # Found a sentence boundary - split at the end of the sentence - title = text[:match.start()].strip() - body = text[match.end():].strip() - return title, body - else: - # No sentence boundary found, return full text as title - return "", text - - def _update_content(self): - """Update text and calculate height.""" - if not self.alert_data.visible or not self.alert_data.text: - self.set_visible(False) - return - - self.set_visible(True) - - # Split text into title and body - self._title_text, self._body_text = self._split_text(self.alert_data.text) - - # Calculate text width (alert width minus padding and icon space on right) - title_width = self.ALERT_WIDTH - (self.ALERT_PADDING * 2) - self.ICON_SIZE - self.ICON_MARGIN - body_width = self.ALERT_WIDTH - (self.ALERT_PADDING * 2) - - # Update labels - self._title_label.set_text(self._title_text) - self._body_label.set_text(self._body_text) - - # Calculate content height - title_height = self._title_label.get_content_height(title_width) if self._title_text else 0 - body_height = self._body_label.get_content_height(body_width) if self._body_text else 0 - spacing = self.TITLE_BODY_SPACING if (self._title_text and self._body_text) else 0 - total_text_height = title_height + spacing + body_height - - # Determine which background size to use based on content height - min_height_with_padding = total_text_height + (self.ALERT_PADDING * 2) - if min_height_with_padding > self.ALERT_HEIGHT_MED: - self._alert_size = AlertSize.BIG - height = self.ALERT_HEIGHT_BIG - elif min_height_with_padding > self.ALERT_HEIGHT_SMALL: - self._alert_size = AlertSize.MEDIUM - height = self.ALERT_HEIGHT_MED - else: - self._alert_size = AlertSize.SMALL - height = self.ALERT_HEIGHT_SMALL - - # Set rect size - self.set_rect(rl.Rectangle(0, 0, self.ALERT_WIDTH, height)) - - def update_alert_data(self, alert_data: AlertData): - """Update alert data and refresh display.""" - self.alert_data = alert_data - self._update_content() - - def _render(self, _): - if not self.alert_data.visible or not self.alert_data.text: - return - - # Choose background based on size - if self._alert_size == AlertSize.BIG: - bg_texture = self._bg_big_pressed if self.is_pressed else self._bg_big - elif self._alert_size == AlertSize.MEDIUM: - bg_texture = self._bg_medium_pressed if self.is_pressed else self._bg_medium - else: # AlertSize.SMALL - bg_texture = self._bg_small_pressed if self.is_pressed else self._bg_small - - # Draw background - rl.draw_texture(bg_texture, int(self._rect.x), int(self._rect.y), rl.WHITE) - - # Calculate text area (left side, avoiding icon on right) - title_width = self.ALERT_WIDTH - (self.ALERT_PADDING * 2) - self.ICON_SIZE - self.ICON_MARGIN - body_width = self.ALERT_WIDTH - (self.ALERT_PADDING * 2) - text_x = self._rect.x + self.ALERT_PADDING - text_y = self._rect.y + self.ALERT_PADDING - - # Draw title label - if self._title_text: - title_rect = rl.Rectangle( - text_x, - text_y, - title_width, - self._title_label.get_content_height(title_width), - ) - self._title_label.render(title_rect) - text_y += title_rect.height + self.TITLE_BODY_SPACING - - # Draw body label - if self._body_text: - body_rect = rl.Rectangle( - text_x, - text_y, - body_width, - self._rect.height - text_y + self._rect.y - self.ALERT_PADDING, - ) - self._body_label.render(body_rect) - - # Draw warning icon on the right side - # Use green icon for update alerts (severity = -1), red for high severity, orange for low severity - if self.alert_data.severity == -1: - icon_texture = self._icon_green - elif self.alert_data.severity > 0: - icon_texture = self._icon_red - else: - icon_texture = self._icon_orange - icon_x = self._rect.x + self.ALERT_WIDTH - self.ALERT_PADDING - self.ICON_SIZE - icon_y = self._rect.y + self.ALERT_PADDING - rl.draw_texture(icon_texture, int(icon_x), int(icon_y), rl.WHITE) - - -class MiciOffroadAlerts(Widget): - """Offroad alerts layout with vertical scrolling.""" - - def __init__(self): - super().__init__() - self.params = Params() - self.sorted_alerts: list[AlertData] = [] - self.alert_items: list[AlertItem] = [] - self._last_refresh = 0.0 - - # Create vertical scroller - self._scroller = Scroller([], horizontal=False, spacing=12, pad_start=0, pad_end=0, snap_items=False) - - # Create empty state label - self._empty_label = UnifiedLabel(tr("no alerts"), 65, FontWeight.DISPLAY, rl.WHITE, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - # Build initial alert list - self._build_alerts() - - def active_alerts(self) -> int: - return sum(alert.visible for alert in self.sorted_alerts) - - def scrolling(self): - return self._scroller.scroll_panel.is_touch_valid() - - def _build_alerts(self): - """Build sorted list of alerts from OFFROAD_ALERTS.""" - self.sorted_alerts = [] - - # Add UpdateAvailable alert at the top (severity = -1 to indicate special handling) - update_alert_data = AlertData(key="UpdateAvailable", text="", severity=-1) - self.sorted_alerts.append(update_alert_data) - update_alert_item = AlertItem(update_alert_data) - update_alert_item.set_click_callback(lambda: HARDWARE.reboot()) - self.alert_items.append(update_alert_item) - self._scroller.add_widget(update_alert_item) - - # Add regular alerts sorted by severity - for key, config in sorted(OFFROAD_ALERTS.items(), key=lambda x: x[1].get("severity", 0), reverse=True): - severity = config.get("severity", 0) - alert_data = AlertData(key=key, text="", severity=severity) - self.sorted_alerts.append(alert_data) - - # Create alert item widget - alert_item = AlertItem(alert_data) - self.alert_items.append(alert_item) - self._scroller.add_widget(alert_item) - - def refresh(self) -> int: - """Refresh alerts from params and return active count.""" - active_count = 0 - - # Handle UpdateAvailable alert specially - update_available = self.params.get_bool("UpdateAvailable") - update_alert_data = next((alert_data for alert_data in self.sorted_alerts if alert_data.key == "UpdateAvailable"), None) - - if update_alert_data: - if update_available: - version_string = "" - - # Get new version description and parse version and date - new_desc = self.params.get("UpdaterNewDescription") or "" - if new_desc: - # format: "version / branch / commit / date" - parts = new_desc.split(" / ") - if len(parts) > 3: - version, date = parts[0], parts[3] - version_string = f"\nopenpilot {version}, {date}\n" - - update_alert_data.text = f"Update available {version_string}. Click to update. Read the release notes at blog.comma.ai." - update_alert_data.visible = True - active_count += 1 - else: - update_alert_data.text = "" - update_alert_data.visible = False - - # Handle regular alerts - for alert_data in self.sorted_alerts: - if alert_data.key == "UpdateAvailable": - continue # Skip, already handled above - - text = "" - alert_json = self.params.get(alert_data.key) - - if alert_json: - text = alert_json.get("text", "").replace("%1", alert_json.get("extra", "")) - - alert_data.text = text - alert_data.visible = bool(text) - - if alert_data.visible: - active_count += 1 - - # Update alert items (they reference the same alert_data objects) - for alert_item in self.alert_items: - alert_item.update_alert_data(alert_item.alert_data) - - return active_count - - def show_event(self): - """Reset scroll position when shown and refresh alerts.""" - self._scroller.show_event() - self._last_refresh = time.monotonic() - self.refresh() - - def _update_state(self): - """Periodically refresh alerts.""" - # Refresh alerts periodically, not every frame - current_time = time.monotonic() - if current_time - self._last_refresh >= REFRESH_INTERVAL: - self.refresh() - self._last_refresh = current_time - - def _render(self, rect: rl.Rectangle): - """Render the alerts scroller or empty state.""" - if self.active_alerts() == 0: - self._empty_label.render(rect) - else: - self._scroller.render(rect) diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py deleted file mode 100644 index 4248fef2ecc957..00000000000000 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ /dev/null @@ -1,490 +0,0 @@ -from enum import IntEnum - -import weakref -import math -import numpy as np -import pyray as rl -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.lib.application import FontWeight, gui_app -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import SmallButton, SmallCircleIconButton -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.system.ui.widgets.slider import SmallSlider -from openpilot.system.ui.mici_setup import TermsHeader, TermsPage as SetupTermsPage -from openpilot.selfdrive.ui.ui_state import ui_state, device -from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer -from openpilot.selfdrive.ui.mici.onroad.driver_camera_dialog import DriverCameraDialog -from openpilot.system.ui.widgets.label import gui_label -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.version import terms_version, training_version - - -class OnboardingState(IntEnum): - TERMS = 0 - ONBOARDING = 1 - DECLINE = 2 - - -class DriverCameraSetupDialog(DriverCameraDialog): - def __init__(self): - super().__init__(no_escape=True) - self.driver_state_renderer = DriverStateRenderer(inset=True) - self.driver_state_renderer.set_rect(rl.Rectangle(0, 0, 120, 120)) - self.driver_state_renderer.load_icons() - self.driver_state_renderer.set_force_active(True) - - def _render(self, rect): - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - self._camera_view._render(rect) - - if not self._camera_view.frame: - gui_label(rect, tr("camera starting"), font_size=64, font_weight=FontWeight.BOLD, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - rl.end_scissor_mode() - return -1 - - # Position dmoji on opposite side from driver - is_rhd = self.driver_state_renderer.is_rhd - self.driver_state_renderer.set_position( - rect.x + 8 if is_rhd else rect.x + rect.width - self.driver_state_renderer.rect.width - 8, - rect.y + 8, - ) - self.driver_state_renderer.render() - - self._draw_face_detection(rect) - - rl.end_scissor_mode() - return -1 - - -class TrainingGuidePreDMTutorial(SetupTermsPage): - def __init__(self, continue_callback): - super().__init__(continue_callback, continue_text="continue") - self._title_header = TermsHeader("driver monitoring setup", gui_app.texture("icons_mici/setup/green_dm.png", 60, 60)) - - self._dm_label = UnifiedLabel("Next, we'll ensure comma four is mounted properly.\n\nIf it does not have a clear view of the driver, " + - "unplug and remount before continuing.", 42, - FontWeight.ROMAN) - - def show_event(self): - super().show_event() - # Get driver monitoring model ready for next step - ui_state.params.put_bool("IsDriverViewEnabled", True) - - @property - def _content_height(self): - return self._dm_label.rect.y + self._dm_label.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16 + scroll_offset, - self._title_header.rect.width, - self._title_header.rect.height, - )) - - self._dm_label.render(rl.Rectangle( - self._rect.x + 16, - self._title_header.rect.y + self._title_header.rect.height + 16, - self._rect.width - 32, - self._dm_label.get_content_height(int(self._rect.width - 32)), - )) - - -class DMBadFaceDetected(SetupTermsPage): - def __init__(self, continue_callback, back_callback): - super().__init__(continue_callback, back_callback, continue_text="power off") - self._title_header = TermsHeader("make sure comma four can see your face", gui_app.texture("icons_mici/setup/orange_dm.png", 60, 60)) - self._dm_label = UnifiedLabel("Re-mount if your face is occluded or driver monitoring has difficulty tracking your face.", 42, FontWeight.ROMAN) - - @property - def _content_height(self): - return self._dm_label.rect.y + self._dm_label.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16 + scroll_offset, - self._title_header.rect.width, - self._title_header.rect.height, - )) - - self._dm_label.render(rl.Rectangle( - self._rect.x + 16, - self._title_header.rect.y + self._title_header.rect.height + 16, - self._rect.width - 32, - self._dm_label.get_content_height(int(self._rect.width - 32)), - )) - - -class TrainingGuideDMTutorial(Widget): - PROGRESS_DURATION = 4 - LOOKING_THRESHOLD_DEG = 30.0 - - def __init__(self, continue_callback): - super().__init__() - self._back_button = SmallCircleIconButton(gui_app.texture("icons_mici/setup/driver_monitoring/dm_question.png", 28, 48)) - self._back_button.set_click_callback(self._show_bad_face_page) - self._good_button = SmallCircleIconButton(gui_app.texture("icons_mici/setup/driver_monitoring/dm_check.png", 42, 42)) - - # Wrap the continue callback to restore settings - def wrapped_continue_callback(): - device.set_offroad_brightness(None) - continue_callback() - - self._good_button.set_click_callback(wrapped_continue_callback) - self._good_button.set_enabled(False) - - self._progress = FirstOrderFilter(0.0, 0.5, 1 / gui_app.target_fps) - self._dialog = DriverCameraSetupDialog() - self._bad_face_page = DMBadFaceDetected(HARDWARE.shutdown, self._hide_bad_face_page) - self._should_show_bad_face_page = False - - # Disable driver monitoring model when device times out for inactivity - def inactivity_callback(): - ui_state.params.put_bool("IsDriverViewEnabled", False) - - device.add_interactive_timeout_callback(inactivity_callback) - - def _show_bad_face_page(self): - self._bad_face_page.show_event() - self.hide_event() - self._should_show_bad_face_page = True - - def _hide_bad_face_page(self): - self._bad_face_page.hide_event() - self.show_event() - self._should_show_bad_face_page = False - - def show_event(self): - super().show_event() - self._dialog.show_event() - self._progress.x = 0.0 - - device.set_offroad_brightness(100) - - def _update_state(self): - super()._update_state() - if device.awake and not ui_state.params.get_bool("IsDriverViewEnabled"): - ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", True) - - sm = ui_state.sm - if sm.recv_frame.get("driverMonitoringState", 0) == 0: - return - - dm_state = sm["driverMonitoringState"] - driver_data = self._dialog.driver_state_renderer.get_driver_data() - - if len(driver_data.faceOrientation) == 3: - pitch, yaw, _ = driver_data.faceOrientation - looking_center = abs(math.degrees(pitch)) < self.LOOKING_THRESHOLD_DEG and abs(math.degrees(yaw)) < self.LOOKING_THRESHOLD_DEG - else: - looking_center = False - - # stay at 100% once reached - if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99: - slow = self._progress.x < 0.25 - duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION - self._progress.x += 1.0 / (duration * gui_app.target_fps) - self._progress.x = min(1.0, self._progress.x) - else: - self._progress.update(0.0) - - self._good_button.set_enabled(self._progress.x >= 0.999) - - def _render(self, _): - if self._should_show_bad_face_page: - return self._bad_face_page.render(self._rect) - - self._dialog.render(self._rect) - - rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y + self._rect.height - 80), - int(self._rect.width), 80, rl.BLANK, rl.BLACK) - - # draw white ring around dm icon to indicate progress - ring_thickness = 8 - - # DM icon is 120x120, positioned on opposite side from driver - dm_size = 120 - is_rhd = self._dialog.driver_state_renderer._is_rhd - dm_center_x = (self._rect.x + dm_size / 2 + 8) if is_rhd else (self._rect.x + self._rect.width - dm_size / 2 - 8) - dm_center_y = self._rect.y + dm_size / 2 + 8 - icon_edge_radius = dm_size / 2 - outer_radius = icon_edge_radius + 1 # 2px outward from icon edge - inner_radius = outer_radius - ring_thickness # Inset by ring_thickness - start_angle = 90.0 # Start from bottom - end_angle = start_angle + self._progress.x * 360.0 # Clockwise - - # Fade in alpha - current_angle = end_angle - start_angle - alpha = int(np.interp(current_angle, [0.0, 45.0], [0, 255])) - - # White to green - color_t = np.clip(np.interp(current_angle, [45.0, 360.0], [0.0, 1.0]), 0.0, 1.0) - r = int(np.interp(color_t, [0.0, 1.0], [255, 0])) - g = int(np.interp(color_t, [0.0, 1.0], [255, 255])) - b = int(np.interp(color_t, [0.0, 1.0], [255, 64])) - ring_color = rl.Color(r, g, b, alpha) - - rl.draw_ring( - rl.Vector2(dm_center_x, dm_center_y), - inner_radius, - outer_radius, - start_angle, - end_angle, - 36, - ring_color, - ) - - if self._dialog._camera_view.frame: - self._back_button.render(rl.Rectangle( - self._rect.x + 8, - self._rect.y + self._rect.height - self._back_button.rect.height, - self._back_button.rect.width, - self._back_button.rect.height, - )) - - self._good_button.render(rl.Rectangle( - self._rect.x + self._rect.width - self._good_button.rect.width - 8, - self._rect.y + self._rect.height - self._good_button.rect.height, - self._good_button.rect.width, - self._good_button.rect.height, - )) - - # rounded border - rl.draw_rectangle_rounded_lines_ex(self._rect, 0.2 * 1.02, 10, 50, rl.BLACK) - - -class TrainingGuideRecordFront(SetupTermsPage): - def __init__(self, continue_callback): - def on_back(): - ui_state.params.put_bool("RecordFront", False) - continue_callback() - - def on_continue(): - ui_state.params.put_bool("RecordFront", True) - continue_callback() - - super().__init__(on_continue, back_callback=on_back, back_text="no", continue_text="yes") - self._title_header = TermsHeader("improve driver monitoring", gui_app.texture("icons_mici/setup/green_dm.png", 60, 60)) - - self._dm_label = UnifiedLabel("Do you want to upload driver camera data?", 42, - FontWeight.ROMAN) - - def show_event(self): - super().show_event() - # Disable driver monitoring model after last step - ui_state.params.put_bool("IsDriverViewEnabled", False) - - @property - def _content_height(self): - return self._dm_label.rect.y + self._dm_label.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16 + scroll_offset, - self._title_header.rect.width, - self._title_header.rect.height, - )) - - self._dm_label.render(rl.Rectangle( - self._rect.x + 16, - self._title_header.rect.y + self._title_header.rect.height + 16, - self._rect.width - 32, - self._dm_label.get_content_height(int(self._rect.width - 32)), - )) - - -class TrainingGuideAttentionNotice(SetupTermsPage): - def __init__(self, continue_callback): - super().__init__(continue_callback, continue_text="continue") - self._title_header = TermsHeader("driver assistance", gui_app.texture("icons_mici/setup/warning.png", 60, 60)) - self._warning_label = UnifiedLabel("1. openpilot is a driver assistance system.\n\n" + - "2. You must pay attention at all times.\n\n" + - "3. You must be ready to take over at any time.\n\n" + - "4. You are fully responsible for driving the car.", 42, - FontWeight.ROMAN) - - @property - def _content_height(self): - return self._warning_label.rect.y + self._warning_label.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16 + scroll_offset, - self._title_header.rect.width, - self._title_header.rect.height, - )) - - self._warning_label.render(rl.Rectangle( - self._rect.x + 16, - self._title_header.rect.y + self._title_header.rect.height + 16, - self._rect.width - 32, - self._warning_label.get_content_height(int(self._rect.width - 32)), - )) - - -class TrainingGuide(Widget): - def __init__(self, completed_callback=None): - super().__init__() - self._completed_callback = completed_callback - self._step = 0 - - self_ref = weakref.ref(self) - - def on_continue(): - if obj := self_ref(): - obj._advance_step() - - self._steps = [ - TrainingGuideAttentionNotice(continue_callback=on_continue), - TrainingGuidePreDMTutorial(continue_callback=on_continue), - TrainingGuideDMTutorial(continue_callback=on_continue), - TrainingGuideRecordFront(continue_callback=on_continue), - ] - - def show_event(self): - super().show_event() - device.set_override_interactive_timeout(300) - - def hide_event(self): - super().hide_event() - device.set_override_interactive_timeout(None) - - def _advance_step(self): - if self._step < len(self._steps) - 1: - self._step += 1 - self._steps[self._step].show_event() - else: - self._step = 0 - if self._completed_callback: - self._completed_callback() - - def _render(self, _): - if self._step < len(self._steps): - self._steps[self._step].render(self._rect) - return -1 - - -class DeclinePage(Widget): - def __init__(self, back_callback=None): - super().__init__() - self._uninstall_slider = SmallSlider("uninstall openpilot", self._on_uninstall) - - self._back_button = SmallButton("back") - self._back_button.set_click_callback(back_callback) - - self._warning_header = TermsHeader("you must accept the\nterms to use openpilot", - gui_app.texture("icons_mici/setup/red_warning.png", 66, 60)) - - def _on_uninstall(self): - ui_state.params.put_bool("DoUninstall", True) - gui_app.request_close() - - def _render(self, _): - self._warning_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16, - self._warning_header.rect.width, - self._warning_header.rect.height, - )) - - self._back_button.set_opacity(1 - self._uninstall_slider.slider_percentage) - self._back_button.render(rl.Rectangle( - self._rect.x + 8, - self._rect.y + self._rect.height - self._back_button.rect.height, - self._back_button.rect.width, - self._back_button.rect.height, - )) - - self._uninstall_slider.render(rl.Rectangle( - self._rect.x + self._rect.width - self._uninstall_slider.rect.width, - self._rect.y + self._rect.height - self._uninstall_slider.rect.height, - self._uninstall_slider.rect.width, - self._uninstall_slider.rect.height, - )) - - -class TermsPage(SetupTermsPage): - def __init__(self, on_accept=None, on_decline=None): - super().__init__(on_accept, on_decline, "decline") - - info_txt = gui_app.texture("icons_mici/setup/green_info.png", 60, 60) - self._title_header = TermsHeader("terms & conditions", info_txt) - - self._terms_label = UnifiedLabel("You must accept the Terms and Conditions to use openpilot. " + - "Read the latest terms at https://comma.ai/terms before continuing.", 36, - FontWeight.ROMAN) - - @property - def _content_height(self): - return self._terms_label.rect.y + self._terms_label.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.set_position(self._rect.x + 16, self._rect.y + 12 + scroll_offset) - self._title_header.render() - - self._terms_label.render(rl.Rectangle( - self._rect.x + 16, - self._title_header.rect.y + self._title_header.rect.height + self.ITEM_SPACING, - self._rect.width - 100, - self._terms_label.get_content_height(int(self._rect.width - 100)), - )) - - -class OnboardingWindow(Widget): - def __init__(self): - super().__init__() - self._accepted_terms: bool = ui_state.params.get("HasAcceptedTerms") == terms_version - self._training_done: bool = ui_state.params.get("CompletedTrainingVersion") == training_version - - self._state = OnboardingState.TERMS if not self._accepted_terms else OnboardingState.ONBOARDING - - self.set_rect(rl.Rectangle(0, 0, 458, gui_app.height)) - - # Windows - self._terms = TermsPage(on_accept=self._on_terms_accepted, on_decline=self._on_terms_declined) - self._training_guide = TrainingGuide(completed_callback=self._on_completed_training) - self._decline_page = DeclinePage(back_callback=self._on_decline_back) - - def show_event(self): - super().show_event() - device.set_override_interactive_timeout(300) - - def hide_event(self): - super().hide_event() - device.set_override_interactive_timeout(None) - - @property - def completed(self) -> bool: - return self._accepted_terms and self._training_done - - def _on_terms_declined(self): - self._state = OnboardingState.DECLINE - - def _on_decline_back(self): - self._state = OnboardingState.TERMS - - def close(self): - ui_state.params.put_bool("IsDriverViewEnabled", False) - gui_app.set_modal_overlay(None) - - def _on_terms_accepted(self): - ui_state.params.put("HasAcceptedTerms", terms_version) - self._state = OnboardingState.ONBOARDING - - def _on_completed_training(self): - ui_state.params.put("CompletedTrainingVersion", training_version) - self.close() - - def _render(self, _): - if self._state == OnboardingState.TERMS: - self._terms.render(self._rect) - elif self._state == OnboardingState.ONBOARDING: - self._training_guide.render(self._rect) - elif self._state == OnboardingState.DECLINE: - self._decline_page.render(self._rect) - return -1 diff --git a/selfdrive/ui/mici/layouts/settings/developer.py b/selfdrive/ui/mici/layouts/settings/developer.py deleted file mode 100644 index b6145e042eb3b1..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/developer.py +++ /dev/null @@ -1,151 +0,0 @@ -import pyray as rl -from collections.abc import Callable - -from openpilot.common.time_helpers import system_time_valid -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle, BigParamControl, BigCircleParamControl -from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigInputDialog -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import NavWidget -from openpilot.selfdrive.ui.layouts.settings.common import restart_needed_callback -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.widgets.ssh_key import SshKeyAction - - -class DeveloperLayoutMici(NavWidget): - def __init__(self, back_callback: Callable): - super().__init__() - self.set_back_callback(back_callback) - - def github_username_callback(username: str): - if username: - ssh_keys = SshKeyAction() - ssh_keys._fetch_ssh_key(username) - if not ssh_keys._error_message: - self._ssh_keys_btn.set_value(username) - else: - dlg = BigDialog("", ssh_keys._error_message) - gui_app.set_modal_overlay(dlg) - - def ssh_keys_callback(): - github_username = ui_state.params.get("GithubUsername") or "" - dlg = BigInputDialog("enter GitHub username", github_username, confirm_callback=github_username_callback) - if not system_time_valid(): - dlg = BigDialog("Please connect to Wi-Fi to fetch your key", "") - gui_app.set_modal_overlay(dlg) - return - gui_app.set_modal_overlay(dlg) - - txt_ssh = gui_app.texture("icons_mici/settings/developer/ssh.png", 56, 64) - github_username = ui_state.params.get("GithubUsername") or "" - self._ssh_keys_btn = BigButton("SSH keys", "Not set" if not github_username else github_username, icon=txt_ssh) - self._ssh_keys_btn.set_click_callback(ssh_keys_callback) - - # adb, ssh, ssh keys, debug mode, joystick debug mode, longitudinal maneuver mode, ip address - # ******** Main Scroller ******** - self._adb_toggle = BigCircleParamControl("icons_mici/adb_short.png", "AdbEnabled", icon_size=(82, 82), icon_offset=(0, 12)) - self._ssh_toggle = BigCircleParamControl("icons_mici/ssh_short.png", "SshEnabled", icon_size=(82, 82), icon_offset=(0, 12)) - self._joystick_toggle = BigToggle("joystick debug mode", - initial_state=ui_state.params.get_bool("JoystickDebugMode"), - toggle_callback=self._on_joystick_debug_mode) - self._long_maneuver_toggle = BigToggle("longitudinal maneuver mode", - initial_state=ui_state.params.get_bool("LongitudinalManeuverMode"), - toggle_callback=self._on_long_maneuver_mode) - self._alpha_long_toggle = BigToggle("alpha longitudinal", - initial_state=ui_state.params.get_bool("AlphaLongitudinalEnabled"), - toggle_callback=self._on_alpha_long_enabled) - self._debug_mode_toggle = BigParamControl("ui debug mode", "ShowDebugInfo", - toggle_callback=lambda checked: (gui_app.set_show_touches(checked), - gui_app.set_show_fps(checked))) - - self._scroller = Scroller([ - self._adb_toggle, - self._ssh_toggle, - self._ssh_keys_btn, - self._joystick_toggle, - self._long_maneuver_toggle, - self._alpha_long_toggle, - self._debug_mode_toggle, - ], snap_items=False) - - # Toggle lists - self._refresh_toggles = ( - ("AdbEnabled", self._adb_toggle), - ("SshEnabled", self._ssh_toggle), - ("JoystickDebugMode", self._joystick_toggle), - ("LongitudinalManeuverMode", self._long_maneuver_toggle), - ("AlphaLongitudinalEnabled", self._alpha_long_toggle), - ("ShowDebugInfo", self._debug_mode_toggle), - ) - onroad_blocked_toggles = (self._adb_toggle, self._joystick_toggle) - release_blocked_toggles = (self._joystick_toggle, self._long_maneuver_toggle, self._alpha_long_toggle) - engaged_blocked_toggles = (self._long_maneuver_toggle, self._alpha_long_toggle) - - # Hide non-release toggles on release builds - for item in release_blocked_toggles: - item.set_visible(not ui_state.is_release) - - # Disable toggles that require offroad - for item in onroad_blocked_toggles: - item.set_enabled(lambda: ui_state.is_offroad()) - - # Disable toggles that require not engaged - for item in engaged_blocked_toggles: - item.set_enabled(lambda: not ui_state.engaged) - - # Set initial state - if ui_state.params.get_bool("ShowDebugInfo"): - gui_app.set_show_touches(True) - gui_app.set_show_fps(True) - - ui_state.add_offroad_transition_callback(self._update_toggles) - - def show_event(self): - super().show_event() - self._scroller.show_event() - self._update_toggles() - - def _render(self, rect: rl.Rectangle): - self._scroller.render(rect) - - def _update_toggles(self): - ui_state.update_params() - - # CP gating - if ui_state.CP is not None: - alpha_avail = ui_state.CP.alphaLongitudinalAvailable - if not alpha_avail or ui_state.is_release: - self._alpha_long_toggle.set_visible(False) - ui_state.params.remove("AlphaLongitudinalEnabled") - else: - self._alpha_long_toggle.set_visible(True) - - long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad() - self._long_maneuver_toggle.set_enabled(long_man_enabled) - if not long_man_enabled: - self._long_maneuver_toggle.set_checked(False) - ui_state.params.put_bool("LongitudinalManeuverMode", False) - else: - self._long_maneuver_toggle.set_enabled(False) - self._alpha_long_toggle.set_visible(False) - - # Refresh toggles from params to mirror external changes - for key, item in self._refresh_toggles: - item.set_checked(ui_state.params.get_bool(key)) - - def _on_joystick_debug_mode(self, state: bool): - ui_state.params.put_bool("JoystickDebugMode", state) - ui_state.params.put_bool("LongitudinalManeuverMode", False) - self._long_maneuver_toggle.set_checked(False) - - def _on_long_maneuver_mode(self, state: bool): - ui_state.params.put_bool("LongitudinalManeuverMode", state) - ui_state.params.put_bool("JoystickDebugMode", False) - self._joystick_toggle.set_checked(False) - restart_needed_callback(state) - - def _on_alpha_long_enabled(self, state: bool): - # TODO: show confirmation dialog before enabling - ui_state.params.put_bool("AlphaLongitudinalEnabled", state) - restart_needed_callback(state) - self._update_toggles() diff --git a/selfdrive/ui/mici/layouts/settings/device.py b/selfdrive/ui/mici/layouts/settings/device.py deleted file mode 100644 index c12a92482c67c7..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/device.py +++ /dev/null @@ -1,383 +0,0 @@ -import os -import threading -import json -import pyray as rl -from enum import IntEnum -from collections.abc import Callable - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.time_helpers import system_time_valid -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2 -from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigCircleButton -from openpilot.selfdrive.ui.mici.widgets.dialog import BigMultiOptionDialog, BigDialog, BigConfirmationDialogV2 -from openpilot.selfdrive.ui.mici.widgets.pairing_dialog import PairingDialog -from openpilot.selfdrive.ui.mici.onroad.driver_camera_dialog import DriverCameraDialog -from openpilot.selfdrive.ui.mici.layouts.onboarding import TrainingGuide -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import Widget, NavWidget -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.widgets.label import MiciLabel -from openpilot.system.ui.widgets.html_render import HtmlModal, HtmlRenderer -from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID - - -class MiciFccModal(NavWidget): - BACK_TOUCH_AREA_PERCENTAGE = 0.1 - - def __init__(self, file_path: str | None = None, text: str | None = None): - super().__init__() - self.set_back_callback(lambda: gui_app.set_modal_overlay(None)) - self._content = HtmlRenderer(file_path=file_path, text=text) - self._scroll_panel = GuiScrollPanel2(horizontal=False) - self._fcc_logo = gui_app.texture("icons_mici/settings/device/fcc_logo.png", 76, 64) - - def _render(self, rect: rl.Rectangle): - content_height = self._content.get_total_height(int(rect.width)) - content_height += self._fcc_logo.height + 20 - - scroll_content_rect = rl.Rectangle(rect.x, rect.y, rect.width, content_height) - scroll_offset = round(self._scroll_panel.update(rect, scroll_content_rect.height)) - - fcc_pos = rl.Vector2(rect.x + 20, rect.y + 20 + scroll_offset) - - scroll_content_rect.y += scroll_offset + self._fcc_logo.height + 20 - self._content.render(scroll_content_rect) - - rl.draw_texture_ex(self._fcc_logo, fcc_pos, 0.0, 1.0, rl.WHITE) - - return -1 - - -def _engaged_confirmation_callback(callback: Callable, action_text: str): - if not ui_state.engaged: - def confirm_callback(): - # Check engaged again in case it changed while the dialog was open - if not ui_state.engaged: - callback() - - red = False - if action_text == "power off": - icon = "icons_mici/settings/device/power.png" - red = True - elif action_text == "reboot": - icon = "icons_mici/settings/device/reboot.png" - elif action_text == "reset": - icon = "icons_mici/settings/device/lkas.png" - elif action_text == "uninstall": - icon = "icons_mici/settings/device/uninstall.png" - else: - # TODO: check - icon = "icons_mici/settings/comma_icon.png" - - dlg: BigConfirmationDialogV2 | BigDialog = BigConfirmationDialogV2(f"slide to\n{action_text.lower()}", icon, red=red, - exit_on_confirm=action_text == "reset", - confirm_callback=confirm_callback) - gui_app.set_modal_overlay(dlg) - else: - dlg = BigDialog(f"Disengage to {action_text}", "") - gui_app.set_modal_overlay(dlg) - - -class DeviceInfoLayoutMici(Widget): - def __init__(self): - super().__init__() - - self.set_rect(rl.Rectangle(0, 0, 360, 180)) - - params = Params() - header_color = rl.Color(255, 255, 255, int(255 * 0.9)) - subheader_color = rl.Color(255, 255, 255, int(255 * 0.9 * 0.65)) - max_width = int(self._rect.width - 20) - self._dongle_id_label = MiciLabel("device ID", 48, width=max_width, color=header_color, font_weight=FontWeight.DISPLAY) - self._dongle_id_text_label = MiciLabel(params.get("DongleId") or 'N/A', 32, width=max_width, color=subheader_color, font_weight=FontWeight.ROMAN) - - self._serial_number_label = MiciLabel("serial", 48, color=header_color, font_weight=FontWeight.DISPLAY) - self._serial_number_text_label = MiciLabel(params.get("HardwareSerial") or 'N/A', 32, width=max_width, color=subheader_color, font_weight=FontWeight.ROMAN) - - def _render(self, _): - self._dongle_id_label.set_position(self._rect.x + 20, self._rect.y - 10) - self._dongle_id_label.render() - - self._dongle_id_text_label.set_position(self._rect.x + 20, self._rect.y + 68 - 25) - self._dongle_id_text_label.render() - - self._serial_number_label.set_position(self._rect.x + 20, self._rect.y + 114 - 30) - self._serial_number_label.render() - - self._serial_number_text_label.set_position(self._rect.x + 20, self._rect.y + 161 - 25) - self._serial_number_text_label.render() - - -class UpdaterState(IntEnum): - IDLE = 0 - WAITING_FOR_UPDATER = 1 - UPDATER_RESPONDING = 2 - - -class PairBigButton(BigButton): - def __init__(self): - super().__init__("pair", "connect.comma.ai", "icons_mici/settings/comma_icon.png", icon_size=(33, 60)) - - def _update_state(self): - if ui_state.prime_state.is_paired(): - self.set_text("paired") - if ui_state.prime_state.is_prime(): - self.set_value("subscribed") - else: - self.set_value("upgrade to prime") - else: - self.set_text("pair") - self.set_value("connect.comma.ai") - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - - # TODO: show ad dialog when clicked if not prime - if ui_state.prime_state.is_paired(): - return - dlg: BigDialog | PairingDialog - if not system_time_valid(): - dlg = BigDialog(tr("Please connect to Wi-Fi to complete initial pairing"), "") - elif UNREGISTERED_DONGLE_ID == (ui_state.params.get("DongleId") or UNREGISTERED_DONGLE_ID): - dlg = BigDialog(tr("Device must be registered with the comma.ai backend to pair"), "") - else: - dlg = PairingDialog() - gui_app.set_modal_overlay(dlg) - - -UPDATER_TIMEOUT = 10.0 # seconds to wait for updater to respond - - -class UpdateOpenpilotBigButton(BigButton): - def __init__(self): - self._txt_update_icon = gui_app.texture("icons_mici/settings/device/update.png", 64, 75) - self._txt_reboot_icon = gui_app.texture("icons_mici/settings/device/reboot.png", 64, 70) - self._txt_up_to_date_icon = gui_app.texture("icons_mici/settings/device/up_to_date.png", 64, 64) - super().__init__("update openpilot", "", self._txt_update_icon) - - self._waiting_for_updater_t: float | None = None - self._hide_value_t: float | None = None - self._state: UpdaterState = UpdaterState.IDLE - - ui_state.add_offroad_transition_callback(self.offroad_transition) - - def offroad_transition(self): - if ui_state.is_offroad(): - self.set_enabled(True) - - def _handle_mouse_release(self, mouse_pos: MousePos): - if not system_time_valid(): - dlg = BigDialog(tr("Please connect to Wi-Fi to update"), "") - gui_app.set_modal_overlay(dlg) - return - - self.set_enabled(False) - self._state = UpdaterState.WAITING_FOR_UPDATER - self.set_icon(self._txt_update_icon) - - def run(): - if self.get_value() == "download update": - os.system("pkill -SIGHUP -f system.updated.updated") - elif self.get_value() == "update now": - ui_state.params.put_bool("DoReboot", True) - else: - os.system("pkill -SIGUSR1 -f system.updated.updated") - - threading.Thread(target=run, daemon=True).start() - - def set_value(self, value: str): - super().set_value(value) - if value: - self.set_text("") - else: - self.set_text("update openpilot") - - def _update_state(self): - if ui_state.started: - self.set_enabled(False) - return - - updater_state = ui_state.params.get("UpdaterState") or "" - failed_count = ui_state.params.get("UpdateFailedCount") - failed = False if failed_count is None else int(failed_count) > 0 - - if ui_state.params.get_bool("UpdateAvailable"): - self.set_rotate_icon(False) - self.set_enabled(True) - if self.get_value() != "update now": - self.set_value("update now") - self.set_icon(self._txt_reboot_icon) - - elif self._state == UpdaterState.WAITING_FOR_UPDATER: - self.set_rotate_icon(True) - if updater_state != "idle": - self._state = UpdaterState.UPDATER_RESPONDING - - # Recover from updater not responding (time invalid shortly after boot) - if self._waiting_for_updater_t is None: - self._waiting_for_updater_t = rl.get_time() - - if self._waiting_for_updater_t is not None and rl.get_time() - self._waiting_for_updater_t > UPDATER_TIMEOUT: - self.set_rotate_icon(False) - self.set_value("updater failed to respond") - self._state = UpdaterState.IDLE - self._hide_value_t = rl.get_time() - - elif self._state == UpdaterState.UPDATER_RESPONDING: - if updater_state == "idle": - self.set_rotate_icon(False) - self._state = UpdaterState.IDLE - self._hide_value_t = rl.get_time() - else: - if self.get_value() != updater_state: - self.set_value(updater_state) - - elif self._state == UpdaterState.IDLE: - self.set_rotate_icon(False) - if failed: - if self.get_value() != "failed to update": - self.set_value("failed to update") - - elif ui_state.params.get_bool("UpdaterFetchAvailable"): - self.set_enabled(True) - if self.get_value() != "download update": - self.set_value("download update") - - elif self._hide_value_t is not None: - self.set_enabled(True) - if self.get_value() == "checking...": - self.set_value("up to date") - self.set_icon(self._txt_up_to_date_icon) - - # Hide previous text after short amount of time (up to date or failed) - if rl.get_time() - self._hide_value_t > 3.0: - self._hide_value_t = None - self.set_value("") - self.set_icon(self._txt_update_icon) - else: - if self.get_value() != "": - self.set_value("") - - if self._state != UpdaterState.WAITING_FOR_UPDATER: - self._waiting_for_updater_t = None - - -class DeviceLayoutMici(NavWidget): - def __init__(self, back_callback: Callable): - super().__init__() - - self._fcc_dialog: HtmlModal | None = None - self._driver_camera: DriverCameraDialog | None = None - self._training_guide: TrainingGuide | None = None - - def power_off_callback(): - ui_state.params.put_bool("DoShutdown", True) - - def reboot_callback(): - ui_state.params.put_bool("DoReboot", True) - - def reset_calibration_callback(): - params = ui_state.params - params.remove("CalibrationParams") - params.remove("LiveTorqueParameters") - params.remove("LiveParameters") - params.remove("LiveParametersV2") - params.remove("LiveDelay") - params.put_bool("OnroadCycleRequested", True) - - def uninstall_openpilot_callback(): - ui_state.params.put_bool("DoUninstall", True) - - reset_calibration_btn = BigButton("reset calibration", "", "icons_mici/settings/device/lkas.png", icon_size=(114, 60)) - reset_calibration_btn.set_click_callback(lambda: _engaged_confirmation_callback(reset_calibration_callback, "reset")) - - uninstall_openpilot_btn = BigButton("uninstall openpilot", "", "icons_mici/settings/device/uninstall.png") - uninstall_openpilot_btn.set_click_callback(lambda: _engaged_confirmation_callback(uninstall_openpilot_callback, "uninstall")) - - reboot_btn = BigCircleButton("icons_mici/settings/device/reboot.png", red=False, icon_size=(64, 70)) - reboot_btn.set_click_callback(lambda: _engaged_confirmation_callback(reboot_callback, "reboot")) - - self._power_off_btn = BigCircleButton("icons_mici/settings/device/power.png", red=True, icon_size=(64, 66)) - self._power_off_btn.set_click_callback(lambda: _engaged_confirmation_callback(power_off_callback, "power off")) - - self._load_languages() - - def language_callback(): - def selected_language_callback(): - selected_language = dlg.get_selected_option() - ui_state.params.put("LanguageSetting", self._languages[selected_language]) - - current_language_name = ui_state.params.get("LanguageSetting") - current_language = next(name for name, lang in self._languages.items() if lang == current_language_name) - - dlg = BigMultiOptionDialog(list(self._languages), default=current_language, right_btn_callback=selected_language_callback) - gui_app.set_modal_overlay(dlg) - - # lang_button = BigButton("change language", "", "icons_mici/settings/device/language.png") - # lang_button.set_click_callback(language_callback) - - regulatory_btn = BigButton("regulatory info", "", "icons_mici/settings/device/info.png") - regulatory_btn.set_click_callback(self._on_regulatory) - - driver_cam_btn = BigButton("driver\ncamera preview", "", "icons_mici/settings/device/cameras.png") - driver_cam_btn.set_click_callback(self._show_driver_camera) - driver_cam_btn.set_enabled(lambda: ui_state.is_offroad()) - - review_training_guide_btn = BigButton("review\nntraining guide", "", "icons_mici/settings/device/info.png") - review_training_guide_btn.set_click_callback(self._on_review_training_guide) - review_training_guide_btn.set_enabled(lambda: ui_state.is_offroad()) - - self._scroller = Scroller([ - DeviceInfoLayoutMici(), - UpdateOpenpilotBigButton(), - PairBigButton(), - review_training_guide_btn, - driver_cam_btn, - # lang_button, - reset_calibration_btn, - uninstall_openpilot_btn, - regulatory_btn, - reboot_btn, - self._power_off_btn, - ], snap_items=False) - - # Set up back navigation - self.set_back_callback(back_callback) - - # Hide power off button when onroad - ui_state.add_offroad_transition_callback(self._offroad_transition) - - def _on_regulatory(self): - if not self._fcc_dialog: - self._fcc_dialog = MiciFccModal(os.path.join(BASEDIR, "selfdrive/assets/offroad/mici_fcc.html")) - gui_app.set_modal_overlay(self._fcc_dialog, callback=setattr(self, '_fcc_dialog', None)) - - def _offroad_transition(self): - self._power_off_btn.set_visible(ui_state.is_offroad()) - - def _show_driver_camera(self): - if not self._driver_camera: - self._driver_camera = DriverCameraDialog() - gui_app.set_modal_overlay(self._driver_camera, callback=lambda result: setattr(self, '_driver_camera', None)) - - def _on_review_training_guide(self): - if not self._training_guide: - def completed_callback(): - gui_app.set_modal_overlay(None) - - self._training_guide = TrainingGuide(completed_callback=completed_callback) - gui_app.set_modal_overlay(self._training_guide, callback=lambda result: setattr(self, '_training_guide', None)) - - def _load_languages(self): - with open(os.path.join(BASEDIR, "selfdrive/ui/translations/languages.json")) as f: - self._languages = json.load(f) - - def show_event(self): - super().show_event() - self._scroller.show_event() - - def _render(self, rect: rl.Rectangle): - self._scroller.render(rect) diff --git a/selfdrive/ui/mici/layouts/settings/firehose.py b/selfdrive/ui/mici/layouts/settings/firehose.py deleted file mode 100644 index d305906e13de57..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/firehose.py +++ /dev/null @@ -1,228 +0,0 @@ -import requests -import threading -import time -import pyray as rl - -from openpilot.common.api import api_get -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.ui.lib.api_helpers import get_token -from openpilot.selfdrive.ui.ui_state import ui_state, device -from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2 -from openpilot.system.ui.lib.multilang import tr, trn, tr_noop -from openpilot.system.ui.widgets import Widget, NavWidget - -TITLE = tr_noop("Firehose Mode") -DESCRIPTION = tr_noop( - "openpilot learns to drive by watching humans, like you, drive.\n\n" - + "Firehose Mode allows you to maximize your training data uploads to improve " - + "openpilot's driving models. More data means bigger models, which means better Experimental Mode." -) -INSTRUCTIONS_INTRO = tr_noop( - "For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.\n\n" - + "Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card." -) -FAQ_HEADER = tr_noop("Frequently Asked Questions") -FAQ_ITEMS = [ - (tr_noop("Does it matter how or where I drive?"), tr_noop("Nope, just drive as you normally would.")), - (tr_noop("Do all of my segments get pulled in Firehose Mode?"), tr_noop("No, we selectively pull a subset of your segments.")), - (tr_noop("What's a good USB-C adapter?"), tr_noop("Any fast phone or laptop charger should be fine.")), - (tr_noop("Does it matter which software I run?"), tr_noop("Yes, only upstream openpilot (and particular forks) are able to be used for training.")), -] - - -class FirehoseLayoutBase(Widget): - PARAM_KEY = "ApiCache_FirehoseStats" - GREEN = rl.Color(46, 204, 113, 255) - RED = rl.Color(231, 76, 60, 255) - GRAY = rl.Color(68, 68, 68, 255) - LIGHT_GRAY = rl.Color(228, 228, 228, 255) - UPDATE_INTERVAL = 30 # seconds - - def __init__(self): - super().__init__() - self._params = Params() - self._session = requests.Session() # reuse session to reduce SSL handshake overhead - self._segment_count = self._get_segment_count() - - self._scroll_panel = GuiScrollPanel2(horizontal=False) - self._content_height = 0 - - self._running = True - self._update_thread = threading.Thread(target=self._update_loop, daemon=True) - self._update_thread.start() - - def __del__(self): - self._running = False - try: - if self._update_thread and self._update_thread.is_alive(): - self._update_thread.join(timeout=1.0) - except Exception: - pass - - def show_event(self): - super().show_event() - self._scroll_panel.set_offset(0) - - def _get_segment_count(self) -> int: - stats = self._params.get(self.PARAM_KEY) - if not stats: - return 0 - try: - return int(stats.get("firehose", 0)) - except Exception: - cloudlog.exception(f"Failed to decode firehose stats: {stats}") - return 0 - - def _render(self, rect: rl.Rectangle): - # compute total content height for scrolling - content_height = self._measure_content_height(rect) - scroll_offset = round(self._scroll_panel.update(rect, content_height)) - - # start drawing with offset - x = int(rect.x + 40) - y = int(rect.y + 40 + scroll_offset) - w = int(rect.width - 80) - - # Title - title_text = tr(TITLE) - title_font = gui_app.font(FontWeight.BOLD) - title_size = 64 - rl.draw_text_ex(title_font, title_text, rl.Vector2(x, y), title_size, 0, rl.WHITE) - y += int(title_size * FONT_SCALE) + 20 - - # Description - y = self._draw_wrapped_text(x, y, w, tr(DESCRIPTION), gui_app.font(FontWeight.ROMAN), 36, rl.WHITE) - y += 20 - - # Separator - rl.draw_rectangle(x, y, w, 2, self.GRAY) - y += 20 - - # Status - status_text, status_color = self._get_status() - y = self._draw_wrapped_text(x, y, w, status_text, gui_app.font(FontWeight.BOLD), 48, status_color) - y += 20 - - # Contribution count (if available) - if self._segment_count > 0: - contrib_text = trn("{} segment of your driving is in the training dataset so far.", - "{} segments of your driving is in the training dataset so far.", self._segment_count).format(self._segment_count) - y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 42, rl.WHITE) - y += 20 - - # Separator - rl.draw_rectangle(x, y, w, 2, self.GRAY) - y += 20 - - # Instructions intro - y = self._draw_wrapped_text(x, y, w, tr(INSTRUCTIONS_INTRO), gui_app.font(FontWeight.ROMAN), 32, self.LIGHT_GRAY) - y += 20 - - # FAQ Header - y = self._draw_wrapped_text(x, y, w, tr(FAQ_HEADER), gui_app.font(FontWeight.BOLD), 44, rl.WHITE) - y += 20 - - # FAQ Items - for question, answer in FAQ_ITEMS: - y = self._draw_wrapped_text(x, y, w, tr(question), gui_app.font(FontWeight.BOLD), 32, self.LIGHT_GRAY) - y = self._draw_wrapped_text(x, y, w, tr(answer), gui_app.font(FontWeight.ROMAN), 32, self.LIGHT_GRAY) - y += 20 - - # return value not used by NavWidget - return -1 - - def _draw_wrapped_text(self, x, y, width, text, font, font_size, color): - wrapped = wrap_text(font, text, font_size, width) - for line in wrapped: - rl.draw_text_ex(font, line, rl.Vector2(x, y), font_size, 0, color) - y += int(font_size * FONT_SCALE) - return y - - def _measure_content_height(self, rect: rl.Rectangle) -> int: - # Rough measurement using the same wrapping as rendering - w = int(rect.width - 80) - y = 40 - - # Title - title_size = 72 - y += int(title_size * FONT_SCALE) + 20 - - # Description - desc_lines = wrap_text(gui_app.font(FontWeight.ROMAN), tr(DESCRIPTION), 36, w) - y += int(len(desc_lines) * 36 * FONT_SCALE) + 20 - - # Separator + Status - y += 2 + 20 - status_text, _ = self._get_status() - status_lines = wrap_text(gui_app.font(FontWeight.BOLD), status_text, 48, w) - y += int(len(status_lines) * 48 * FONT_SCALE) + 20 - - # Contribution count - if self._segment_count > 0: - contrib_text = trn("{} segment of your driving is in the training dataset so far.", - "{} segments of your driving is in the training dataset so far.", self._segment_count).format(self._segment_count) - contrib_lines = wrap_text(gui_app.font(FontWeight.BOLD), contrib_text, 42, w) - y += int(len(contrib_lines) * 42 * FONT_SCALE) + 20 - - # Separator + Instructions - y += 2 + 20 - - # Instructions intro - intro_lines = wrap_text(gui_app.font(FontWeight.ROMAN), tr(INSTRUCTIONS_INTRO), 32, w) - y += int(len(intro_lines) * 32 * FONT_SCALE) + 20 - - # FAQ Header - faq_header_lines = wrap_text(gui_app.font(FontWeight.BOLD), tr(FAQ_HEADER), 44, w) - y += int(len(faq_header_lines) * 44 * FONT_SCALE) + 20 - - # FAQ Items - for question, answer in FAQ_ITEMS: - q_lines = wrap_text(gui_app.font(FontWeight.BOLD), tr(question), 32, w) - y += int(len(q_lines) * 32 * FONT_SCALE) - a_lines = wrap_text(gui_app.font(FontWeight.ROMAN), tr(answer), 32, w) - y += int(len(a_lines) * 32 * FONT_SCALE) + 20 - - # bottom padding - y += 40 - return y - - def _get_status(self) -> tuple[str, rl.Color]: - network_type = ui_state.sm["deviceState"].networkType - network_metered = ui_state.sm["deviceState"].networkMetered - - if not network_metered and network_type != 0: # Not metered and connected - return tr("ACTIVE"), self.GREEN - else: - return tr("INACTIVE: connect to an unmetered network"), self.RED - - def _fetch_firehose_stats(self): - try: - dongle_id = self._params.get("DongleId") - if not dongle_id or dongle_id == UNREGISTERED_DONGLE_ID: - return - identity_token = get_token(dongle_id) - response = api_get(f"v1/devices/{dongle_id}/firehose_stats", access_token=identity_token, session=self._session) - if response.status_code == 200: - data = response.json() - self._segment_count = data.get("firehose", 0) - self._params.put(self.PARAM_KEY, data) - except Exception as e: - cloudlog.error(f"Failed to fetch firehose stats: {e}") - - def _update_loop(self): - while self._running: - if not ui_state.started and device._awake: - self._fetch_firehose_stats() - time.sleep(self.UPDATE_INTERVAL) - - -class FirehoseLayout(FirehoseLayoutBase, NavWidget): - BACK_TOUCH_AREA_PERCENTAGE = 0.1 - - def __init__(self, back_callback): - super().__init__() - self.set_back_callback(back_callback) diff --git a/selfdrive/ui/mici/layouts/settings/network/__init__.py b/selfdrive/ui/mici/layouts/settings/network/__init__.py deleted file mode 100644 index fb1d56a1f64125..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/network/__init__.py +++ /dev/null @@ -1,197 +0,0 @@ -import pyray as rl -from enum import IntEnum -from collections.abc import Callable - -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.selfdrive.ui.mici.layouts.settings.network.wifi_ui import WifiUIMici, WifiIcon, normalize_ssid -from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigMultiToggle, BigParamControl, BigToggle -from openpilot.selfdrive.ui.mici.widgets.dialog import BigInputDialog -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.lib.prime_state import PrimeType -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import NavWidget -from openpilot.system.ui.lib.wifi_manager import WifiManager, Network, MeteredType - - -class NetworkPanelType(IntEnum): - NONE = 0 - WIFI = 1 - - -class NetworkLayoutMici(NavWidget): - def __init__(self, back_callback: Callable): - super().__init__() - - self._current_panel = NetworkPanelType.WIFI - self.set_back_enabled(lambda: self._current_panel == NetworkPanelType.NONE) - - self._wifi_manager = WifiManager() - self._wifi_manager.set_active(False) - self._wifi_ui = WifiUIMici(self._wifi_manager, back_callback=lambda: self._switch_to_panel(NetworkPanelType.NONE)) - - self._wifi_manager.add_callbacks( - networks_updated=self._on_network_updated, - ) - - # ******** Tethering ******** - def tethering_toggle_callback(checked: bool): - self._tethering_toggle_btn.set_enabled(False) - self._network_metered_btn.set_enabled(False) - self._wifi_manager.set_tethering_active(checked) - - self._tethering_toggle_btn = BigToggle("enable tethering", "", toggle_callback=tethering_toggle_callback) - - def tethering_password_callback(password: str): - if password: - self._wifi_manager.set_tethering_password(password) - - def tethering_password_clicked(): - tethering_password = self._wifi_manager.tethering_password - dlg = BigInputDialog("enter password...", tethering_password, minimum_length=8, - confirm_callback=tethering_password_callback) - gui_app.set_modal_overlay(dlg) - - txt_tethering = gui_app.texture("icons_mici/settings/network/tethering.png", 64, 54) - self._tethering_password_btn = BigButton("tethering password", "", txt_tethering) - self._tethering_password_btn.set_click_callback(tethering_password_clicked) - - # ******** Network Metered ******** - def network_metered_callback(value: str): - self._network_metered_btn.set_enabled(False) - metered = { - 'default': MeteredType.UNKNOWN, - 'metered': MeteredType.YES, - 'unmetered': MeteredType.NO - }.get(value, MeteredType.UNKNOWN) - self._wifi_manager.set_current_network_metered(metered) - - # TODO: signal for current network metered type when changing networks, this is wrong until you press it once - # TODO: disable when not connected - self._network_metered_btn = BigMultiToggle("network usage", ["default", "metered", "unmetered"], select_callback=network_metered_callback) - self._network_metered_btn.set_enabled(False) - - self._wifi_slash_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_slash.png", 64, 56) - self._wifi_low_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_low.png", 64, 47) - self._wifi_medium_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_medium.png", 64, 47) - self._wifi_full_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 64, 47) - - self._wifi_button = BigButton("wi-fi", "not connected", self._wifi_slash_txt, scroll=True) - self._wifi_button.set_click_callback(lambda: self._switch_to_panel(NetworkPanelType.WIFI)) - - # ******** Advanced settings ******** - # ******** Roaming toggle ******** - self._roaming_btn = BigParamControl("enable roaming", "GsmRoaming", toggle_callback=self._toggle_roaming) - - # ******** APN settings ******** - self._apn_btn = BigButton("apn settings", "edit") - self._apn_btn.set_click_callback(self._edit_apn) - - # ******** Cellular metered toggle ******** - self._cellular_metered_btn = BigParamControl("cellular metered", "GsmMetered", toggle_callback=self._toggle_cellular_metered) - - # Main scroller ---------------------------------- - self._scroller = Scroller([ - self._wifi_button, - self._network_metered_btn, - self._tethering_toggle_btn, - self._tethering_password_btn, - # /* Advanced settings - self._roaming_btn, - self._apn_btn, - self._cellular_metered_btn, - # */ - ], snap_items=False) - - # Set initial config - roaming_enabled = ui_state.params.get_bool("GsmRoaming") - metered = ui_state.params.get_bool("GsmMetered") - self._wifi_manager.update_gsm_settings(roaming_enabled, ui_state.params.get("GsmApn") or "", metered) - - # Set up back navigation - self.set_back_callback(back_callback) - - def _update_state(self): - super()._update_state() - - # If not using prime SIM, show GSM settings and enable IPv4 forwarding - show_cell_settings = ui_state.prime_state.get_type() in (PrimeType.NONE, PrimeType.LITE) - self._wifi_manager.set_ipv4_forward(show_cell_settings) - self._roaming_btn.set_visible(show_cell_settings) - self._apn_btn.set_visible(show_cell_settings) - self._cellular_metered_btn.set_visible(show_cell_settings) - - def show_event(self): - super().show_event() - self._current_panel = NetworkPanelType.NONE - self._wifi_ui.show_event() - self._scroller.show_event() - - def hide_event(self): - super().hide_event() - self._wifi_ui.hide_event() - - def _toggle_roaming(self, checked: bool): - self._wifi_manager.update_gsm_settings(checked, ui_state.params.get("GsmApn") or "", ui_state.params.get_bool("GsmMetered")) - - def _edit_apn(self): - def update_apn(apn: str): - apn = apn.strip() - if apn == "": - ui_state.params.remove("GsmApn") - else: - ui_state.params.put("GsmApn", apn) - - self._wifi_manager.update_gsm_settings(ui_state.params.get_bool("GsmRoaming"), apn, ui_state.params.get_bool("GsmMetered")) - - current_apn = ui_state.params.get("GsmApn") or "" - dlg = BigInputDialog("enter APN", current_apn, minimum_length=0, confirm_callback=update_apn) - gui_app.set_modal_overlay(dlg) - - def _toggle_cellular_metered(self, checked: bool): - self._wifi_manager.update_gsm_settings(ui_state.params.get_bool("GsmRoaming"), ui_state.params.get("GsmApn") or "", checked) - - def _on_network_updated(self, networks: list[Network]): - # Update tethering state - tethering_active = self._wifi_manager.is_tethering_active() - # TODO: use real signals (like activated/settings changed, etc.) to speed up re-enabling buttons - self._tethering_toggle_btn.set_enabled(True) - self._network_metered_btn.set_enabled(lambda: not tethering_active and bool(self._wifi_manager.ipv4_address)) - self._tethering_toggle_btn.set_checked(tethering_active) - - # Update wi-fi button with ssid and ip address - # TODO: make sure we handle hidden ssids - connected_network = next((network for network in networks if network.is_connected), None) - self._wifi_button.set_text(normalize_ssid(connected_network.ssid) if connected_network is not None else "wi-fi") - self._wifi_button.set_value(self._wifi_manager.ipv4_address or "not connected") - if connected_network is not None: - strength = WifiIcon.get_strength_icon_idx(connected_network.strength) - if strength == 2: - strength_icon = self._wifi_full_txt - elif strength == 1: - strength_icon = self._wifi_medium_txt - else: - strength_icon = self._wifi_low_txt - self._wifi_button.set_icon(strength_icon) - else: - self._wifi_button.set_icon(self._wifi_slash_txt) - - # Update network metered - self._network_metered_btn.set_value( - { - MeteredType.UNKNOWN: 'default', - MeteredType.YES: 'metered', - MeteredType.NO: 'unmetered' - }.get(self._wifi_manager.current_network_metered, 'default')) - - def _switch_to_panel(self, panel_type: NetworkPanelType): - if panel_type == NetworkPanelType.WIFI: - self._wifi_ui.show_event() - self._current_panel = panel_type - - def _render(self, rect: rl.Rectangle): - self._wifi_manager.process_callbacks() - - if self._current_panel == NetworkPanelType.WIFI: - self._wifi_ui.render(rect) - else: - self._scroller.render(rect) diff --git a/selfdrive/ui/mici/layouts/settings/network/wifi_ui.py b/selfdrive/ui/mici/layouts/settings/network/wifi_ui.py deleted file mode 100644 index 7791f18cf736fb..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/network/wifi_ui.py +++ /dev/null @@ -1,459 +0,0 @@ -import math -import numpy as np -import pyray as rl -from collections.abc import Callable - -from openpilot.common.swaglog import cloudlog -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.selfdrive.ui.mici.widgets.dialog import BigMultiOptionDialog, BigInputDialog, BigDialogOptionButton, BigConfirmationDialogV2 -from openpilot.system.ui.lib.application import gui_app, MousePos, FontWeight -from openpilot.system.ui.widgets import Widget, NavWidget -from openpilot.system.ui.lib.wifi_manager import WifiManager, Network, SecurityType - - -def normalize_ssid(ssid: str) -> str: - return ssid.replace("’", "'") # for iPhone hotspots - - -class LoadingAnimation(Widget): - def _render(self, _): - cx = int(self._rect.x + 70) - cy = int(self._rect.y + self._rect.height / 2 - 50) - - y_mag = 20 - anim_scale = 5 - spacing = 28 - - for i in range(3): - x = cx - spacing + i * spacing - y = int(cy + min(math.sin((rl.get_time() - i * 0.2) * anim_scale) * y_mag, 0)) - alpha = int(np.interp(cy - y, [0, y_mag], [255 * 0.45, 255 * 0.9])) - rl.draw_circle(x, y, 10, rl.Color(255, 255, 255, alpha)) - - -class WifiIcon(Widget): - def __init__(self): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, 86, 64)) - - self._wifi_low_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_low.png", 86, 64) - self._wifi_medium_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_medium.png", 86, 64) - self._wifi_full_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 86, 64) - self._lock_txt = gui_app.texture("icons_mici/settings/network/new/lock.png", 22, 32) - - self._network: Network | None = None - self._scale = 1.0 - - def set_current_network(self, network: Network): - self._network = network - - def set_scale(self, scale: float): - self._scale = scale - - @staticmethod - def get_strength_icon_idx(strength: int) -> int: - return round(strength / 100 * 2) - - def _render(self, _): - if self._network is None: - return - - # Determine which wifi strength icon to use - strength = self.get_strength_icon_idx(self._network.strength) - if strength == 2: - strength_icon = self._wifi_full_txt - elif strength == 1: - strength_icon = self._wifi_medium_txt - else: - strength_icon = self._wifi_low_txt - - icon_x = int(self._rect.x + (self._rect.width - strength_icon.width * self._scale) // 2) - icon_y = int(self._rect.y + (self._rect.height - strength_icon.height * self._scale) // 2) - rl.draw_texture_ex(strength_icon, (icon_x, icon_y), 0.0, self._scale, rl.WHITE) - - # Render lock icon at lower right of wifi icon if secured - if self._network.security_type not in (SecurityType.OPEN, SecurityType.UNSUPPORTED): - lock_scale = self._scale * 1.1 - lock_x = int(icon_x + 1 + strength_icon.width * self._scale - self._lock_txt.width * lock_scale / 2) - lock_y = int(icon_y + 1 + strength_icon.height * self._scale - self._lock_txt.height * lock_scale / 2) - rl.draw_texture_ex(self._lock_txt, (lock_x, lock_y), 0.0, lock_scale, rl.WHITE) - - -class WifiItem(BigDialogOptionButton): - LEFT_MARGIN = 20 - - def __init__(self, network: Network): - super().__init__(network.ssid) - - self.set_rect(rl.Rectangle(0, 0, gui_app.width, self.HEIGHT)) - - self._selected_txt = gui_app.texture("icons_mici/settings/network/new/wifi_selected.png", 48, 96) - - self._network = network - self._wifi_icon = WifiIcon() - self._wifi_icon.set_current_network(network) - - def set_current_network(self, network: Network): - self._network = network - self._wifi_icon.set_current_network(network) - - def _render(self, _): - if self._network.is_connected: - selected_x = int(self._rect.x - self._selected_txt.width / 2) - selected_y = int(self._rect.y + (self._rect.height - self._selected_txt.height) / 2) - rl.draw_texture(self._selected_txt, selected_x, selected_y, rl.WHITE) - - self._wifi_icon.set_scale((1.0 if self._selected else 0.65) * 0.7) - self._wifi_icon.render(rl.Rectangle( - self._rect.x + self.LEFT_MARGIN, - self._rect.y, - self.SELECTED_HEIGHT, - self._rect.height - )) - - if self._selected: - self._label.set_font_size(self.SELECTED_HEIGHT) - self._label.set_color(rl.Color(255, 255, 255, int(255 * 0.9))) - self._label.set_font_weight(FontWeight.DISPLAY) - else: - self._label.set_font_size(self.HEIGHT) - self._label.set_color(rl.Color(255, 255, 255, int(255 * 0.58))) - self._label.set_font_weight(FontWeight.DISPLAY_REGULAR) - - label_offset = self.LEFT_MARGIN + self._wifi_icon.rect.width + 20 - label_rect = rl.Rectangle(self._rect.x + label_offset, self._rect.y, self._rect.width - label_offset, self._rect.height) - self._label.set_text(normalize_ssid(self._network.ssid)) - self._label.render(label_rect) - - -class ConnectButton(Widget): - def __init__(self): - super().__init__() - self._bg_txt = gui_app.texture("icons_mici/settings/network/new/connect_button.png", 410, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/settings/network/new/connect_button_pressed.png", 410, 100) - self._bg_full_txt = gui_app.texture("icons_mici/settings/network/new/full_connect_button.png", 520, 100) - self._bg_full_pressed_txt = gui_app.texture("icons_mici/settings/network/new/full_connect_button_pressed.png", 520, 100) - - self._full: bool = False - - self._label = UnifiedLabel("", 36, FontWeight.MEDIUM, rl.Color(255, 255, 255, int(255 * 0.9)), - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - @property - def full(self) -> bool: - return self._full - - def set_full(self, full: bool): - self._full = full - self.set_rect(rl.Rectangle(0, 0, 520 if self._full else 410, 100)) - - def set_label(self, text: str): - self._label.set_text(text) - - def _render(self, _): - if self._full: - bg_txt = self._bg_full_pressed_txt if self.is_pressed and self.enabled else self._bg_full_txt - else: - bg_txt = self._bg_pressed_txt if self.is_pressed and self.enabled else self._bg_txt - - rl.draw_texture(bg_txt, int(self._rect.x), int(self._rect.y), rl.WHITE) - - self._label.set_text_color(rl.Color(255, 255, 255, int(255 * 0.9) if self.enabled else int(255 * 0.9 * 0.65))) - self._label.render(self._rect) - - -class ForgetButton(Widget): - HORIZONTAL_MARGIN = 8 - - def __init__(self, forget_network: Callable, open_network_manage_page): - super().__init__() - self._forget_network = forget_network - self._open_network_manage_page = open_network_manage_page - - self._bg_txt = gui_app.texture("icons_mici/settings/network/new/forget_button.png", 100, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/settings/network/new/forget_button_pressed.png", 100, 100) - self._trash_txt = gui_app.texture("icons_mici/settings/network/new/trash.png", 35, 42) - self.set_rect(rl.Rectangle(0, 0, 100 + self.HORIZONTAL_MARGIN * 2, 100)) - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - dlg = BigConfirmationDialogV2("slide to forget", "icons_mici/settings/network/new/trash.png", red=True, - confirm_callback=self._forget_network) - gui_app.set_modal_overlay(dlg, callback=self._open_network_manage_page) - - def _render(self, _): - bg_txt = self._bg_pressed_txt if self.is_pressed else self._bg_txt - rl.draw_texture(bg_txt, int(self._rect.x + self.HORIZONTAL_MARGIN), int(self._rect.y), rl.WHITE) - - trash_x = int(self._rect.x + (self._rect.width - self._trash_txt.width) // 2) - trash_y = int(self._rect.y + (self._rect.height - self._trash_txt.height) // 2) - rl.draw_texture(self._trash_txt, trash_x, trash_y, rl.WHITE) - - -class NetworkInfoPage(NavWidget): - def __init__(self, wifi_manager, connect_callback: Callable, forget_callback: Callable, open_network_manage_page: Callable): - super().__init__() - self._wifi_manager = wifi_manager - - self.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - - self._wifi_icon = WifiIcon() - self._forget_btn = ForgetButton(lambda: forget_callback(self._network.ssid) if self._network is not None else None, - open_network_manage_page) - self._connect_btn = ConnectButton() - self._connect_btn.set_click_callback(lambda: connect_callback(self._network.ssid) if self._network is not None else None) - - self._title = UnifiedLabel("", 64, FontWeight.DISPLAY, rl.Color(255, 255, 255, int(255 * 0.9)), - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, scroll=True) - self._subtitle = UnifiedLabel("", 36, FontWeight.ROMAN, rl.Color(255, 255, 255, int(255 * 0.9 * 0.65)), - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - self.set_back_callback(lambda: gui_app.set_modal_overlay(None)) - - # State - self._network: Network | None = None - self._connecting: Callable[[], str | None] | None = None - - def show_event(self): - super().show_event() - self._title.reset_scroll() - - def update_networks(self, networks: dict[str, Network]): - # update current network from latest scan results - for ssid, network in networks.items(): - if self._network is not None and ssid == self._network.ssid: - self.set_current_network(network) - break - else: - # network disappeared, close page - gui_app.set_modal_overlay(None) - - def _update_state(self): - super()._update_state() - # Modal overlays stop main UI rendering, so we need to call here - self._wifi_manager.process_callbacks() - - if self._network is None: - return - - self._connect_btn.set_full(not self._network.is_saved and not self._is_connecting) - if self._is_connecting: - self._connect_btn.set_label("connecting...") - self._connect_btn.set_enabled(False) - elif self._network.is_connected: - self._connect_btn.set_label("connected") - self._connect_btn.set_enabled(False) - elif self._network.security_type == SecurityType.UNSUPPORTED: - self._connect_btn.set_label("connect") - self._connect_btn.set_enabled(False) - else: # saved or unknown - self._connect_btn.set_label("connect") - self._connect_btn.set_enabled(True) - - self._title.set_text(normalize_ssid(self._network.ssid)) - if self._network.security_type == SecurityType.OPEN: - self._subtitle.set_text("open") - elif self._network.security_type == SecurityType.UNSUPPORTED: - self._subtitle.set_text("unsupported") - else: - self._subtitle.set_text("secured") - - def set_current_network(self, network: Network): - self._network = network - self._wifi_icon.set_current_network(network) - - def set_connecting(self, is_connecting: Callable[[], str | None]): - self._connecting = is_connecting - - @property - def _is_connecting(self): - if self._connecting is None or self._network is None: - return False - is_connecting = self._connecting() == self._network.ssid - return is_connecting - - def _render(self, _): - self._wifi_icon.render(rl.Rectangle( - self._rect.x + 32, - self._rect.y + (self._rect.height - self._connect_btn.rect.height - self._wifi_icon.rect.height) / 2, - self._wifi_icon.rect.width, - self._wifi_icon.rect.height, - )) - - self._title.render(rl.Rectangle( - self._rect.x + self._wifi_icon.rect.width + 32 + 32, - self._rect.y + 32 - 16, - self._rect.width - (self._wifi_icon.rect.width + 32 + 32), - 64, - )) - - self._subtitle.render(rl.Rectangle( - self._rect.x + self._wifi_icon.rect.width + 32 + 32, - self._rect.y + 32 + 64 - 16, - self._rect.width - (self._wifi_icon.rect.width + 32 + 32), - 48, - )) - - self._connect_btn.render(rl.Rectangle( - self._rect.x + 8, - self._rect.y + self._rect.height - self._connect_btn.rect.height, - self._connect_btn.rect.width, - self._connect_btn.rect.height, - )) - - if not self._connect_btn.full: - self._forget_btn.render(rl.Rectangle( - self._rect.x + self._rect.width - self._forget_btn.rect.width, - self._rect.y + self._rect.height - self._forget_btn.rect.height, - self._forget_btn.rect.width, - self._forget_btn.rect.height, - )) - - return -1 - - -class WifiUIMici(BigMultiOptionDialog): - # Wait this long after user interacts with widget to update network list - INACTIVITY_TIMEOUT = 1 - - def __init__(self, wifi_manager: WifiManager, back_callback: Callable): - super().__init__([], None, None, right_btn_callback=None) - - # Set up back navigation - self.set_back_callback(back_callback) - - self._network_info_page = NetworkInfoPage(wifi_manager, self._connect_to_network, self._forget_network, self._open_network_manage_page) - self._network_info_page.set_connecting(lambda: self._connecting) - - self._loading_animation = LoadingAnimation() - - self._wifi_manager = wifi_manager - self._connecting: str | None = None - self._networks: dict[str, Network] = {} - - # widget state - self._last_interaction_time = -float('inf') - self._restore_selection = False - - self._wifi_manager.add_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): - # Call super to prepare scroller; selection scroll is handled dynamically - super().show_event() - self._wifi_manager.set_active(True) - self._last_interaction_time = -float('inf') - - def hide_event(self): - super().hide_event() - self._wifi_manager.set_active(False) - - def _open_network_manage_page(self, result=None): - self._network_info_page.update_networks(self._networks) - gui_app.set_modal_overlay(self._network_info_page) - - def _forget_network(self, ssid: str): - network = self._networks.get(ssid) - if network is None: - cloudlog.warning(f"Trying to forget unknown network: {ssid}") - return - - self._wifi_manager.forget_connection(network.ssid) - - def _on_network_updated(self, networks: list[Network]): - self._networks = {network.ssid: network for network in networks} - self._update_buttons() - self._network_info_page.update_networks(self._networks) - - def _update_buttons(self): - # Don't update buttons while user is actively interacting - if rl.get_time() - self._last_interaction_time < self.INACTIVITY_TIMEOUT: - return - - for network in self._networks.values(): - # pop and re-insert to eliminate stuttering on update (prevents position lost for a frame) - network_button_idx = next((i for i, btn in enumerate(self._scroller._items) if btn.option == network.ssid), None) - if network_button_idx is not None: - network_button = self._scroller._items.pop(network_button_idx) - # Update network on existing button - network_button.set_current_network(network) - else: - network_button = WifiItem(network) - - self._scroller.add_widget(network_button) - - # remove networks no longer present - self._scroller._items[:] = [btn for btn in self._scroller._items if btn.option in self._networks] - - # try to restore previous selection to prevent jumping from adding/removing/reordering buttons - self._restore_selection = True - - def _connect_with_password(self, ssid: str, password: str): - if password: - self._connecting = ssid - self._wifi_manager.connect_to_network(ssid, password) - self._update_buttons() - - def _on_option_selected(self, option: str): - super()._on_option_selected(option) - - if option in self._networks: - self._network_info_page.set_current_network(self._networks[option]) - self._open_network_manage_page() - - def _connect_to_network(self, ssid: str): - network = self._networks.get(ssid) - if network is None: - cloudlog.warning(f"Trying to connect to unknown network: {ssid}") - return - - if network.is_saved: - self._connecting = network.ssid - self._wifi_manager.activate_connection(network.ssid) - self._update_buttons() - elif network.security_type == SecurityType.OPEN: - self._connecting = network.ssid - self._wifi_manager.connect_to_network(network.ssid, "") - self._update_buttons() - else: - self._on_need_auth(network.ssid, False) - - def _on_need_auth(self, ssid, incorrect_password=True): - hint = "incorrect password..." if incorrect_password else "enter password..." - dlg = BigInputDialog(hint, "", minimum_length=8, - confirm_callback=lambda _password: self._connect_with_password(ssid, _password)) - # go back to the manage network page - gui_app.set_modal_overlay(dlg, self._open_network_manage_page) - - def _on_activated(self): - self._connecting = None - - def _on_forgotten(self): - self._connecting = None - - def _on_disconnected(self): - self._connecting = None - - def _update_state(self): - super()._update_state() - if self.is_pressed: - self._last_interaction_time = rl.get_time() - - def _render(self, _): - # Update Scroller layout and restore current selection whenever buttons are updated, before first render - current_selection = self.get_selected_option() - if self._restore_selection and current_selection in self._networks: - self._scroller._layout() - BigMultiOptionDialog._on_option_selected(self, current_selection) - self._restore_selection = None - - super()._render(_) - - if not self._networks: - self._loading_animation.render(self._rect) diff --git a/selfdrive/ui/mici/layouts/settings/settings.py b/selfdrive/ui/mici/layouts/settings/settings.py deleted file mode 100644 index 3917899032e743..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/settings.py +++ /dev/null @@ -1,113 +0,0 @@ -import pyray as rl -from dataclasses import dataclass -from enum import IntEnum -from collections.abc import Callable - -from openpilot.common.params import Params -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.selfdrive.ui.mici.widgets.button import BigButton -from openpilot.selfdrive.ui.mici.layouts.settings.toggles import TogglesLayoutMici -from openpilot.selfdrive.ui.mici.layouts.settings.network import NetworkLayoutMici -from openpilot.selfdrive.ui.mici.layouts.settings.device import DeviceLayoutMici, PairBigButton -from openpilot.selfdrive.ui.mici.layouts.settings.developer import DeveloperLayoutMici -from openpilot.selfdrive.ui.mici.layouts.settings.firehose import FirehoseLayout -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.widgets import Widget, NavWidget - - -class PanelType(IntEnum): - TOGGLES = 0 - NETWORK = 1 - DEVICE = 2 - DEVELOPER = 3 - USER_MANUAL = 4 - FIREHOSE = 5 - - -@dataclass -class PanelInfo: - name: str - instance: Widget - - -class SettingsLayout(NavWidget): - def __init__(self): - super().__init__() - self._params = Params() - self._current_panel = None # PanelType.DEVICE - - toggles_btn = BigButton("toggles", "", "icons_mici/settings.png") - toggles_btn.set_click_callback(lambda: self._set_current_panel(PanelType.TOGGLES)) - network_btn = BigButton("network", "", "icons_mici/settings/network/wifi_strength_full.png", icon_size=(76, 56)) - network_btn.set_click_callback(lambda: self._set_current_panel(PanelType.NETWORK)) - device_btn = BigButton("device", "", "icons_mici/settings/device_icon.png", icon_size=(74, 60)) - device_btn.set_click_callback(lambda: self._set_current_panel(PanelType.DEVICE)) - developer_btn = BigButton("developer", "", "icons_mici/settings/developer_icon.png", icon_size=(64, 60)) - developer_btn.set_click_callback(lambda: self._set_current_panel(PanelType.DEVELOPER)) - - firehose_btn = BigButton("firehose", "", "icons_mici/settings/firehose.png", icon_size=(52, 62)) - firehose_btn.set_click_callback(lambda: self._set_current_panel(PanelType.FIREHOSE)) - - self._scroller = Scroller([ - toggles_btn, - network_btn, - device_btn, - PairBigButton(), - #BigDialogButton("manual", "", "icons_mici/settings/manual_icon.png", "Check out the mici user\nmanual at comma.ai/setup"), - firehose_btn, - developer_btn, - ], snap_items=False) - - # Set up back navigation - self.set_back_callback(self.close_settings) - self.set_back_enabled(lambda: self._current_panel is None) - - self._panels = { - PanelType.TOGGLES: PanelInfo("Toggles", TogglesLayoutMici(back_callback=lambda: self._set_current_panel(None))), - PanelType.NETWORK: PanelInfo("Network", NetworkLayoutMici(back_callback=lambda: self._set_current_panel(None))), - PanelType.DEVICE: PanelInfo("Device", DeviceLayoutMici(back_callback=lambda: self._set_current_panel(None))), - PanelType.DEVELOPER: PanelInfo("Developer", DeveloperLayoutMici(back_callback=lambda: self._set_current_panel(None))), - PanelType.FIREHOSE: PanelInfo("Firehose", FirehoseLayout(back_callback=lambda: self._set_current_panel(None))), - } - - self._font_medium = gui_app.font(FontWeight.MEDIUM) - - # Callbacks - self._close_callback: Callable | None = None - - def show_event(self): - super().show_event() - self._set_current_panel(None) - self._scroller.show_event() - if self._current_panel is not None: - self._panels[self._current_panel].instance.show_event() - - def hide_event(self): - super().hide_event() - if self._current_panel is not None: - self._panels[self._current_panel].instance.hide_event() - - def set_callbacks(self, on_close: Callable): - self._close_callback = on_close - - def _render(self, rect: rl.Rectangle): - if self._current_panel is not None: - self._draw_current_panel() - else: - self._scroller.render(rect) - - def _draw_current_panel(self): - panel = self._panels[self._current_panel] - panel.instance.render(self._rect) - - def _set_current_panel(self, panel_type: PanelType | None): - if panel_type != self._current_panel: - if self._current_panel is not None: - self._panels[self._current_panel].instance.hide_event() - self._current_panel = panel_type - if self._current_panel is not None: - self._panels[self._current_panel].instance.show_event() - - def close_settings(self): - if self._close_callback: - self._close_callback() diff --git a/selfdrive/ui/mici/layouts/settings/toggles.py b/selfdrive/ui/mici/layouts/settings/toggles.py deleted file mode 100644 index c16504fac8ba3a..00000000000000 --- a/selfdrive/ui/mici/layouts/settings/toggles.py +++ /dev/null @@ -1,95 +0,0 @@ -import pyray as rl -from collections.abc import Callable -from cereal import log - -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.selfdrive.ui.mici.widgets.button import BigParamControl, BigMultiParamToggle -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import NavWidget -from openpilot.selfdrive.ui.layouts.settings.common import restart_needed_callback -from openpilot.selfdrive.ui.ui_state import ui_state - -PERSONALITY_TO_INT = log.LongitudinalPersonality.schema.enumerants - - -class TogglesLayoutMici(NavWidget): - def __init__(self, back_callback: Callable): - super().__init__() - self.set_back_callback(back_callback) - - self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"]) - self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode") - is_metric_toggle = BigParamControl("use metric units", "IsMetric") - ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled") - always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM") - record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback) - record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback) - enable_openpilot = BigParamControl("enable openpilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback) - - self._scroller = Scroller([ - self._personality_toggle, - self._experimental_btn, - is_metric_toggle, - ldw_toggle, - always_on_dm_toggle, - record_front, - record_mic, - enable_openpilot, - ], snap_items=False) - - # Toggle lists - self._refresh_toggles = ( - ("ExperimentalMode", self._experimental_btn), - ("IsMetric", is_metric_toggle), - ("IsLdwEnabled", ldw_toggle), - ("AlwaysOnDM", always_on_dm_toggle), - ("RecordFront", record_front), - ("RecordAudio", record_mic), - ("OpenpilotEnabledToggle", enable_openpilot), - ) - - enable_openpilot.set_enabled(lambda: not ui_state.engaged) - record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged)) - record_mic.set_enabled(lambda: not ui_state.engaged) - - if ui_state.params.get_bool("ShowDebugInfo"): - gui_app.set_show_touches(True) - gui_app.set_show_fps(True) - - ui_state.add_engaged_transition_callback(self._update_toggles) - - def _update_state(self): - super()._update_state() - - if ui_state.sm.updated["selfdriveState"]: - personality = PERSONALITY_TO_INT[ui_state.sm["selfdriveState"].personality] - if personality != ui_state.personality and ui_state.started: - self._personality_toggle.set_value(self._personality_toggle._options[personality]) - ui_state.personality = personality - - def show_event(self): - super().show_event() - self._scroller.show_event() - self._update_toggles() - - def _update_toggles(self): - ui_state.update_params() - - # CP gating for experimental mode - if ui_state.CP is not None: - if ui_state.has_longitudinal_control: - self._experimental_btn.set_visible(True) - self._personality_toggle.set_visible(True) - else: - # no long for now - self._experimental_btn.set_visible(False) - self._experimental_btn.set_checked(False) - self._personality_toggle.set_visible(False) - ui_state.params.remove("ExperimentalMode") - - # Refresh toggles from params to mirror external changes - for key, item in self._refresh_toggles: - item.set_checked(ui_state.params.get_bool(key)) - - def _render(self, rect: rl.Rectangle): - self._scroller.render(rect) diff --git a/selfdrive/ui/mici/onroad/__init__.py b/selfdrive/ui/mici/onroad/__init__.py deleted file mode 100644 index bb45117b944e96..00000000000000 --- a/selfdrive/ui/mici/onroad/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -import pyray as rl - -SIDE_PANEL_WIDTH = 60 - - -def blend_colors(a: rl.Color, b: rl.Color, f: float) -> rl.Color: - h0, s0, v0 = (hsv0 := rl.color_to_hsv(a)).x, hsv0.y, hsv0.z - h1, s1, v1 = (hsv1 := rl.color_to_hsv(b)).x, hsv1.y, hsv1.z - dh = ((h1 - h0 + 180) % 360) - 180 # shortest hue delta - return rl.color_from_hsv((h0 + f * dh) % 360, - s0 + f * (s1 - s0), - v0 + f * (v1 - v0)) diff --git a/selfdrive/ui/mici/onroad/alert_renderer.py b/selfdrive/ui/mici/onroad/alert_renderer.py deleted file mode 100644 index 64dd04c31034dc..00000000000000 --- a/selfdrive/ui/mici/onroad/alert_renderer.py +++ /dev/null @@ -1,357 +0,0 @@ -import time -from enum import StrEnum -from typing import NamedTuple -import pyray as rl -import random -import string -from dataclasses import dataclass -from cereal import messaging, log, car -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import UnifiedLabel - -AlertSize = log.SelfdriveState.AlertSize -AlertStatus = log.SelfdriveState.AlertStatus - -ALERT_MARGIN = 18 - -ALERT_FONT_SMALL = 66 - 50 -ALERT_FONT_BIG = 88 - 40 - -SELFDRIVE_STATE_TIMEOUT = 5 # Seconds -SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds - -# Constants -ALERT_COLORS = { - AlertStatus.normal: rl.Color(0, 0, 0, 255), - AlertStatus.userPrompt: rl.Color(255, 115, 0, 255), - AlertStatus.critical: rl.Color(255, 0, 21, 255), -} - -TURN_SIGNAL_BLINK_PERIOD = 1 / (80 / 60) # Mazda heartbeat turn signal BPM - -DEBUG = False - - -class IconSide(StrEnum): - left = 'left' - right = 'right' - - -class IconLayout(NamedTuple): - texture: rl.Texture - side: IconSide - margin_x: int - margin_y: int - - -class AlertLayout(NamedTuple): - text_rect: rl.Rectangle - icon: IconLayout | None - - -@dataclass -class Alert: - text1: str = "" - text2: str = "" - size: int = 0 - status: int = 0 - visual_alert: int = car.CarControl.HUDControl.VisualAlert.none - alert_type: str = "" - - -# Pre-defined alert instances -ALERT_STARTUP_PENDING = Alert( - text1="openpilot Unavailable", - text2="Waiting to start", - size=AlertSize.mid, - status=AlertStatus.normal, -) - -ALERT_CRITICAL_TIMEOUT = Alert( - text1="TAKE CONTROL IMMEDIATELY", - text2="System Unresponsive", - size=AlertSize.full, - status=AlertStatus.critical, -) - -ALERT_CRITICAL_REBOOT = Alert( - text1="System Unresponsive", - text2="Reboot Device", - size=AlertSize.full, - status=AlertStatus.critical, -) - - -class AlertRenderer(Widget): - def __init__(self): - super().__init__() - - self._alert_text1_label = UnifiedLabel(text="", font_size=ALERT_FONT_BIG, font_weight=FontWeight.DISPLAY, line_height=0.86, - letter_spacing=-0.02) - self._alert_text2_label = UnifiedLabel(text="", font_size=ALERT_FONT_SMALL, font_weight=FontWeight.ROMAN, line_height=0.86, - letter_spacing=0.025) - - self._prev_alert: Alert | None = None - self._text_gen_time = 0 - self._alert_text2_gen = '' - - # animation filters - # TODO: use 0.1 but with proper alert height calculation - self._alert_y_filter = BounceFilter(0, 0.1, 1 / gui_app.target_fps) - self._alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - - self._turn_signal_timer = 0.0 - self._turn_signal_alpha_filter = FirstOrderFilter(0.0, 0.3, 1 / gui_app.target_fps) - self._last_icon_side: IconSide | None = None - - self._load_icons() - - def _load_icons(self): - self._txt_turn_signal_left = gui_app.texture('icons_mici/onroad/turn_signal_left.png', 104, 96) - self._txt_turn_signal_right = gui_app.texture('icons_mici/onroad/turn_signal_right.png', 104, 96) - self._txt_blind_spot_left = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 134, 150) - self._txt_blind_spot_right = gui_app.texture('icons_mici/onroad/blind_spot_right.png', 134, 150) - - def get_alert(self, sm: messaging.SubMaster) -> Alert | None: - """Generate the current alert based on selfdrive state.""" - ss = sm['selfdriveState'] - - # Check if selfdriveState messages have stopped arriving - if not sm.updated['selfdriveState']: - recv_frame = sm.recv_frame['selfdriveState'] - time_since_onroad = time.monotonic() - ui_state.started_time - - # 1. Never received selfdriveState since going onroad - waiting_for_startup = recv_frame < ui_state.started_frame - if waiting_for_startup and time_since_onroad > 5: - return ALERT_STARTUP_PENDING - - # 2. Lost communication with selfdriveState after receiving it - if TICI and not waiting_for_startup: - ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] - if ss_missing > SELFDRIVE_STATE_TIMEOUT: - if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT: - return ALERT_CRITICAL_TIMEOUT - return ALERT_CRITICAL_REBOOT - - # No alert if size is none - if ss.alertSize == 0: - return None - - # Return current alert - ret = Alert(text1=ss.alertText1, text2=ss.alertText2, size=ss.alertSize.raw, status=ss.alertStatus.raw, - visual_alert=ss.alertHudVisual, alert_type=ss.alertType) - self._prev_alert = ret - return ret - - def will_render(self) -> tuple[Alert | None, bool]: - alert = self.get_alert(ui_state.sm) - return alert or self._prev_alert, alert is None - - def _icon_helper(self, alert: Alert) -> AlertLayout: - icon_side = None - txt_icon = None - icon_margin_x = 20 - icon_margin_y = 18 - - # alert_type format is "EventName/eventType" (e.g., "preLaneChangeLeft/warning") - event_name = alert.alert_type.split('/')[0] if alert.alert_type else '' - - if event_name == 'preLaneChangeLeft': - icon_side = IconSide.left - txt_icon = self._txt_turn_signal_left - icon_margin_x = 2 - icon_margin_y = 5 - - elif event_name == 'preLaneChangeRight': - icon_side = IconSide.right - txt_icon = self._txt_turn_signal_right - icon_margin_x = 2 - icon_margin_y = 5 - - elif event_name == 'laneChange': - icon_side = self._last_icon_side - txt_icon = self._txt_turn_signal_left if self._last_icon_side == 'left' else self._txt_turn_signal_right - icon_margin_x = 2 - icon_margin_y = 5 - - elif event_name == 'laneChangeBlocked': - CS = ui_state.sm['carState'] - if CS.leftBlinker: - icon_side = IconSide.left - elif CS.rightBlinker: - icon_side = IconSide.right - else: - icon_side = self._last_icon_side - txt_icon = self._txt_blind_spot_left if icon_side == 'left' else self._txt_blind_spot_right - icon_margin_x = 8 - icon_margin_y = 0 - - else: - self._turn_signal_timer = 0.0 - - self._last_icon_side = icon_side - - # create text rect based on icon presence - text_x = self._rect.x + ALERT_MARGIN - text_width = self._rect.width - ALERT_MARGIN - if icon_side == 'left': - text_x = self._rect.x + self._txt_turn_signal_right.width - text_width = self._rect.width - ALERT_MARGIN - self._txt_turn_signal_right.width - elif icon_side == 'right': - text_x = self._rect.x + ALERT_MARGIN - text_width = self._rect.width - ALERT_MARGIN - self._txt_turn_signal_right.width - - text_rect = rl.Rectangle( - text_x, - self._alert_y_filter.x, - text_width, - self._rect.height, - ) - icon_layout = IconLayout(txt_icon, icon_side, icon_margin_x, icon_margin_y) if txt_icon is not None and icon_side is not None else None - return AlertLayout(text_rect, icon_layout) - - def _render(self, rect: rl.Rectangle) -> bool: - alert = self.get_alert(ui_state.sm) - - # Animate fade and slide in/out - self._alert_y_filter.update(self._rect.y - 50 if alert is None else self._rect.y) - self._alpha_filter.update(0 if alert is None else 1) - - if alert is None: - # If still animating out, keep the previous alert - if self._alpha_filter.x > 0.01 and self._prev_alert is not None: - alert = self._prev_alert - else: - self._prev_alert = None - return False - - self._draw_background(alert) - - alert_layout = self._icon_helper(alert) - self._draw_text(alert, alert_layout) - self._draw_icons(alert_layout) - - return True - - def _draw_icons(self, alert_layout: AlertLayout) -> None: - if alert_layout.icon is None: - return - - if time.monotonic() - self._turn_signal_timer > TURN_SIGNAL_BLINK_PERIOD: - self._turn_signal_timer = time.monotonic() - self._turn_signal_alpha_filter.x = 255 * 2 - else: - self._turn_signal_alpha_filter.update(255 * 0.2) - - if alert_layout.icon.side == 'left': - pos_x = int(self._rect.x + alert_layout.icon.margin_x) - else: - pos_x = int(self._rect.x + self._rect.width - alert_layout.icon.margin_x - alert_layout.icon.texture.width) - - if alert_layout.icon.texture not in (self._txt_turn_signal_left, self._txt_turn_signal_right): - icon_alpha = 255 - else: - icon_alpha = int(min(self._turn_signal_alpha_filter.x, 255)) - - rl.draw_texture(alert_layout.icon.texture, pos_x, int(self._rect.y + alert_layout.icon.margin_y), - rl.Color(255, 255, 255, int(icon_alpha * self._alpha_filter.x))) - - def _draw_background(self, alert: Alert) -> None: - # draw top gradient for alert text at top - color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal]) - color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x)) - translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x)) - - small_alert_height = round(self._rect.height * 0.583) # 140px at mici height - medium_alert_height = round(self._rect.height * 0.833) # 200px at mici height - - # alert_type format is "EventName/eventType" (e.g., "preLaneChangeLeft/warning") - event_name = alert.alert_type.split('/')[0] if alert.alert_type else '' - - if event_name == 'preLaneChangeLeft': - bg_height = small_alert_height - elif event_name == 'preLaneChangeRight': - bg_height = small_alert_height - elif event_name == 'laneChange': - bg_height = small_alert_height - elif event_name == 'laneChangeBlocked': - bg_height = medium_alert_height - else: - bg_height = int(self._rect.height) - - solid_height = round(bg_height * 0.2) - rl.draw_rectangle(int(self._rect.x), int(self._rect.y), int(self._rect.width), solid_height, color) - rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y + solid_height), int(self._rect.width), - int(bg_height - solid_height), - color, translucent_color) - - def _draw_text(self, alert: Alert, alert_layout: AlertLayout) -> None: - icon_side = alert_layout.icon.side if alert_layout.icon is not None else None - - # TODO: hack - alert_text1 = alert.text1.lower().replace('calibrating: ', 'calibrating:\n') - can_draw_second_line = False - # TODO: there should be a common way to determine font size based on text length to maximize rect - if len(alert_text1) <= 12: - can_draw_second_line = True - font_size = 92 - 10 - elif len(alert_text1) <= 16: - can_draw_second_line = True - font_size = 70 - else: - font_size = 64 - 10 - - if icon_side is not None: - font_size -= 10 - - color = rl.Color(255, 255, 255, int(255 * 0.9 * self._alpha_filter.x)) - - text1_y_offset = 11 if font_size >= 70 else 4 - text_rect1 = rl.Rectangle( - alert_layout.text_rect.x, - alert_layout.text_rect.y - text1_y_offset, - alert_layout.text_rect.width, - alert_layout.text_rect.height, - ) - self._alert_text1_label.set_text(alert_text1) - self._alert_text1_label.set_text_color(color) - self._alert_text1_label.set_font_size(font_size) - self._alert_text1_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT) - self._alert_text1_label.render(text_rect1) - - alert_text2 = alert.text2.lower() - - # randomize chars and length for testing - if DEBUG: - if time.monotonic() - self._text_gen_time > 0.5: - self._alert_text2_gen = ''.join(random.choices(string.ascii_lowercase + ' ', k=random.randint(0, 40))) - self._text_gen_time = time.monotonic() - alert_text2 = self._alert_text2_gen or alert_text2 - - if can_draw_second_line and alert_text2: - last_line_h = self._alert_text1_label.rect.y + self._alert_text1_label.get_content_height(int(alert_layout.text_rect.width)) - last_line_h -= 4 - if len(alert_text2) > 18: - small_font_size = 36 - elif len(alert_text2) > 24: - small_font_size = 32 - else: - small_font_size = 40 - text_rect2 = rl.Rectangle( - alert_layout.text_rect.x, - last_line_h, - alert_layout.text_rect.width, - alert_layout.text_rect.height - last_line_h - ) - color = rl.Color(255, 255, 255, int(255 * 0.65 * self._alpha_filter.x)) - - self._alert_text2_label.set_text(alert_text2) - self._alert_text2_label.set_text_color(color) - self._alert_text2_label.set_font_size(small_font_size) - self._alert_text2_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT) - self._alert_text2_label.render(text_rect2) diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py deleted file mode 100644 index 69bcca401d91a6..00000000000000 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ /dev/null @@ -1,377 +0,0 @@ -import time -import numpy as np -import pyray as rl -from cereal import messaging, car, log -from msgq.visionipc import VisionStreamType -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH -from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer -from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer -from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer -from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer -from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall -from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView -from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.system.ui.widgets import Widget -from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter -from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame -from openpilot.common.transformations.orientation import rot_from_euler -from enum import IntEnum - -OpState = log.SelfdriveState.OpenpilotState -CALIBRATED = log.LiveCalibrationData.Status.calibrated -ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD -WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD -DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] - - -class BookmarkState(IntEnum): - HIDDEN = 0 - DRAGGING = 1 - TRIGGERED = 2 - -WIDE_CAM_MAX_SPEED = 5.0 # m/s (10 mph) -ROAD_CAM_MIN_SPEED = 10 # m/s (25 mph) - -CAM_Y_OFFSET = 20 - - -class BookmarkIcon(Widget): - PEEK_THRESHOLD = 50 # If icon peeks out this much, snap it fully visible - FULL_VISIBLE_OFFSET = 200 # How far onscreen when fully visible - HIDDEN_OFFSET = -50 # How far offscreen when hidden - - def __init__(self, bookmark_callback): - super().__init__() - self._bookmark_callback = bookmark_callback - self._icon = gui_app.texture("icons_mici/onroad/bookmark.png", 180, 180) - self._icon_fill = gui_app.texture("icons_mici/onroad/bookmark_fill.png", 180, 180) - self._active_icon = self._icon - self._offset_filter = BounceFilter(0.0, 0.1, 1 / gui_app.target_fps) - - # State - self._interacting = False - self._state = BookmarkState.HIDDEN - self._swipe_start_x = 0.0 - self._swipe_current_x = 0.0 - self._is_swiping = False - self._is_swiping_left: bool = False - self._triggered_time: float = 0.0 - - def is_swiping_left(self) -> bool: - """Check if currently swiping left (for scroller to disable).""" - return self._is_swiping_left - - def interacting(self): - interacting, self._interacting = self._interacting, False - return interacting - - def _update_state(self): - if self._state == BookmarkState.DRAGGING: - # Allow pulling past activated position with rubber band effect - swipe_offset = self._swipe_start_x - self._swipe_current_x - swipe_offset = min(swipe_offset, self.FULL_VISIBLE_OFFSET + 50) - self._offset_filter.update(swipe_offset) - - elif self._state == BookmarkState.TRIGGERED: - # Continue animating to fully visible - self._offset_filter.update(self.FULL_VISIBLE_OFFSET) - # Stay in TRIGGERED state for 1 second - if rl.get_time() - self._triggered_time >= 1.5: - self._state = BookmarkState.HIDDEN - - elif self._state == BookmarkState.HIDDEN: - self._offset_filter.update(self.HIDDEN_OFFSET) - - if self._offset_filter.x < 1e-3: - self._interacting = False - self._active_icon = self._icon - - def _handle_mouse_event(self, mouse_event: MouseEvent): - if not ui_state.started: - return - - if mouse_event.left_pressed: - # Store relative position within widget - self._swipe_start_x = mouse_event.pos.x - self._swipe_current_x = mouse_event.pos.x - self._is_swiping = True - self._is_swiping_left = False - self._state = BookmarkState.DRAGGING - self._active_icon = self._icon - - elif mouse_event.left_down and self._is_swiping: - self._swipe_current_x = mouse_event.pos.x - swipe_offset = self._swipe_start_x - self._swipe_current_x - self._is_swiping_left = swipe_offset > 0 - if self._is_swiping_left: - self._interacting = True - - elif mouse_event.left_released: - if self._is_swiping: - swipe_distance = self._swipe_start_x - self._swipe_current_x - - # If peeking past threshold, transition to animating to fully visible and bookmark - if swipe_distance > self.PEEK_THRESHOLD: - self._state = BookmarkState.TRIGGERED - self._triggered_time = rl.get_time() - self._active_icon = self._icon_fill - self._bookmark_callback() - else: - # Otherwise, transition back to hidden - self._state = BookmarkState.HIDDEN - - # Reset swipe state - self._is_swiping = False - self._is_swiping_left = False - - def _render(self, _): - """Render the bookmark icon.""" - if self._offset_filter.x > 0: - icon_x = self.rect.x + self.rect.width - round(self._offset_filter.x) - icon_y = self.rect.y + (self.rect.height - self._active_icon.height) / 2 # Vertically centered - rl.draw_texture(self._active_icon, int(icon_x), int(icon_y), rl.WHITE) - - -class AugmentedRoadView(CameraView): - def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD): - super().__init__("camerad", stream_type) - self._bookmark_callback = bookmark_callback - self._set_placeholder_color(rl.BLACK) - - self.device_camera: DeviceCameraConfig | None = None - self.view_from_calib = view_frame_from_device_frame.copy() - self.view_from_wide_calib = view_frame_from_device_frame.copy() - - self._last_calib_time: float = 0 - self._last_rect_dims = (0.0, 0.0) - self._last_stream_type = stream_type - self._cached_matrix: np.ndarray | None = None - self._content_rect = rl.Rectangle() - self._last_click_time = 0.0 - - # Bookmark icon with swipe gesture - self._bookmark_icon = BookmarkIcon(bookmark_callback) - - self._model_renderer = ModelRenderer() - self._hud_renderer = HudRenderer() - self._alert_renderer = AlertRenderer() - self._driver_state_renderer = DriverStateRenderer() - self._confidence_ball = ConfidenceBall() - self._offroad_label = UnifiedLabel("start the car to\nuse openpilot", 54, FontWeight.DISPLAY, - text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png") - self._fade_alpha_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) - - # debug - self._pm = messaging.PubMaster(['uiDebug']) - - def is_swiping_left(self) -> bool: - """Check if currently swiping left (for scroller to disable).""" - return self._bookmark_icon.is_swiping_left() - - def _update_state(self): - super()._update_state() - - # update offroad label - if ui_state.panda_type == log.PandaState.PandaType.unknown: - self._offroad_label.set_text("system booting") - else: - self._offroad_label.set_text("start the car to\nuse openpilot") - - def _handle_mouse_release(self, mouse_pos: MousePos): - # Don't trigger click callback if bookmark was triggered - if not self._bookmark_icon.interacting(): - super()._handle_mouse_release(mouse_pos) - - def _render(self, _): - start_draw = time.monotonic() - self._switch_stream_if_needed(ui_state.sm) - - # Update calibration before rendering - self._update_calibration() - - # Create inner content area with border padding - self._content_rect = rl.Rectangle( - self.rect.x, - self.rect.y, - self.rect.width - SIDE_PANEL_WIDTH, - self.rect.height, - ) - - # Enable scissor mode to clip all rendering within content rectangle boundaries - # This creates a rendering viewport that prevents graphics from drawing outside the border - rl.begin_scissor_mode( - int(self._content_rect.x), - int(self._content_rect.y), - int(self._content_rect.width), - int(self._content_rect.height) - ) - - # Render the base camera view - super()._render(self._content_rect) - - # Draw all UI overlays - self._model_renderer.render(self._content_rect) - - # Fade out bottom of overlays for looks (only when engaged) - fade_alpha = self._fade_alpha_filter.update(ui_state.status != UIStatus.DISENGAGED) - if fade_alpha > 1e-2: - rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, - rl.Color(255, 255, 255, int(255 * fade_alpha))) - - alert_to_render, not_animating_out = self._alert_renderer.will_render() - - # Hide DMoji when disengaged unless AlwaysOnDM is enabled - should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and ui_state.is_onroad() and - (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm)) - self._driver_state_renderer.set_should_draw(should_draw_dmoji) - self._driver_state_renderer.set_position(self._rect.x + 16, self._rect.y + 10) - self._driver_state_renderer.render() - - self._hud_renderer.set_can_draw_top_icons(alert_to_render is None) - self._hud_renderer.set_wheel_critical_icon(alert_to_render is not None and not not_animating_out and - alert_to_render.visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired) - # TODO: have alert renderer draw offroad mici label below - if ui_state.started: - self._alert_renderer.render(self._content_rect) - self._hud_renderer.render(self._content_rect) - - # Draw fake rounded border - rl.draw_rectangle_rounded_lines_ex(self._content_rect, 0.2 * 1.02, 10, 50, rl.BLACK) - - # End clipping region - rl.end_scissor_mode() - - # Custom UI extension point - add custom overlays here - # Use self._content_rect for positioning within camera bounds - self._confidence_ball.render(self.rect) - - self._bookmark_icon.render(self.rect) - - # Draw darkened background and text if not onroad - if not ui_state.started: - rl.draw_rectangle(int(self.rect.x), int(self.rect.y), int(self.rect.width), int(self.rect.height), rl.Color(0, 0, 0, 175)) - self._offroad_label.render(self._content_rect) - - # publish uiDebug - msg = messaging.new_message('uiDebug') - msg.uiDebug.drawTimeMillis = (time.monotonic() - start_draw) * 1000 - self._pm.send('uiDebug', msg) - - def _switch_stream_if_needed(self, sm): - if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: - v_ego = sm['carState'].vEgo - if v_ego < WIDE_CAM_MAX_SPEED: - target = WIDE_CAM - elif v_ego > ROAD_CAM_MIN_SPEED: - target = ROAD_CAM - else: - # Hysteresis zone - keep current stream - target = self.stream_type - else: - target = ROAD_CAM - - if self.stream_type != target: - self.switch_stream(target) - - def _update_calibration(self): - # Update device camera if not already set - sm = ui_state.sm - if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']: - self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] - - # Check if live calibration data is available and valid - if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): - return - - calib = sm['liveCalibration'] - if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: - return - - # Update view_from_calib matrix - device_from_calib = rot_from_euler(calib.rpyCalib) - self.view_from_calib = view_frame_from_device_frame @ device_from_calib - - # Update wide calibration if available - if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3: - wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) - self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - # Get camera configuration - # TODO: cache with vEgo? - calib_time = ui_state.sm.recv_frame['liveCalibration'] - current_dims = (self._content_rect.width, self._content_rect.height) - device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA - is_wide_camera = self.stream_type == WIDE_CAM - intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics - calibration = self.view_from_wide_calib if is_wide_camera else self.view_from_calib - if is_wide_camera: - zoom = 0.7 * 1.5 - else: - zoom = np.interp(ui_state.sm['carState'].vEgo, [10, 30], [0.8, 1.0]) - - # Calculate transforms for vanishing point - inf_point = np.array([1000.0, 0.0, 0.0]) - calib_transform = intrinsic @ calibration - kep = calib_transform @ inf_point - - # Calculate center points and dimensions - x, y = self._content_rect.x, self._content_rect.y - w, h = self._content_rect.width, self._content_rect.height - cx, cy = intrinsic[0, 2], intrinsic[1, 2] - - # Calculate max allowed offsets with margins - margin = 5 - max_x_offset = cx * zoom - w / 2 - margin - max_y_offset = cy * zoom - h / 2 - margin - - # Calculate and clamp offsets to prevent out-of-bounds issues - try: - if abs(kep[2]) > 1e-6: - x_offset = np.clip((kep[0] / kep[2] - cx) * zoom, -max_x_offset, max_x_offset) - y_offset = np.clip((kep[1] / kep[2] - cy) * zoom + CAM_Y_OFFSET, -max_y_offset, max_y_offset) - else: - x_offset, y_offset = 0, 0 - except (ZeroDivisionError, OverflowError): - x_offset, y_offset = 0, 0 - - # Cache the computed transformation matrix to avoid recalculations - self._last_calib_time = calib_time - self._last_rect_dims = current_dims - self._last_stream_type = self.stream_type - self._cached_matrix = np.array([ - [zoom * 2 * cx / w, 0, -x_offset / w * 2], - [0, zoom * 2 * cy / h, -y_offset / h * 2], - [0, 0, 1.0] - ]) - - video_transform = np.array([ - [zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)], - [0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)], - [0.0, 0.0, 1.0] - ]) - self._model_renderer.set_transform(video_transform @ calib_transform) - - return self._cached_matrix - - -if __name__ == "__main__": - gui_app.init_window("OnRoad Camera View") - road_camera_view = AugmentedRoadView(ROAD_CAM) - print("***press space to switch camera view***") - try: - for _ in gui_app.render(): - ui_state.update() - if rl.is_key_released(rl.KeyboardKey.KEY_SPACE): - if WIDE_CAM in road_camera_view.available_streams: - stream = ROAD_CAM if road_camera_view.stream_type == WIDE_CAM else WIDE_CAM - road_camera_view.switch_stream(stream) - road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - road_camera_view.close() diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py deleted file mode 100644 index 89a4926ce9af7e..00000000000000 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ /dev/null @@ -1,417 +0,0 @@ -import platform -import numpy as np -import pyray as rl - -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus - -CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts - -VERSION = """ -#version 300 es -precision mediump float; -""" -if platform.system() == "Darwin": - VERSION = """ - #version 330 core - """ - - -VERTEX_SHADER = VERSION + """ -in vec3 vertexPosition; -in vec2 vertexTexCoord; -in vec3 vertexNormal; -in vec4 vertexColor; -uniform mat4 mvp; -out vec2 fragTexCoord; -out vec4 fragColor; -void main() { - fragTexCoord = vertexTexCoord; - fragColor = vertexColor; - gl_Position = mvp * vec4(vertexPosition, 1.0); -} -""" - -# Choose fragment shader based on platform capabilities -if TICI: - FRAME_FRAGMENT_SHADER = """ - #version 300 es - #extension GL_OES_EGL_image_external_essl3 : enable - precision mediump float; - in vec2 fragTexCoord; - uniform samplerExternalOES texture0; - out vec4 fragColor; - uniform int engaged; - uniform int enhance_driver; - - void main() { - vec4 color = texture(texture0, fragTexCoord); - if (engaged == 1) { - float gray = dot(color.rgb, vec3(0.299, 0.587, 0.114)); // Luma - color.rgb = mix(vec3(gray), color.rgb, 0.2); // 20% saturation - color.rgb = clamp((color.rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0); // +20% contrast - color.rgb = pow(color.rgb, vec3(1.0/1.28)); - fragColor = vec4(color.rgb, color.a); - } else { - color.rgb *= 0.85; // 85% opacity - } - if (enhance_driver == 1) { - float brightness = 1.1; - color.rgb = color.rgb + 0.15; - color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); - color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb); - color.rgb = pow(color.rgb, vec3(0.8)); - } - fragColor = vec4(color.rgb, color.a); - } - """ -else: - FRAME_FRAGMENT_SHADER = VERSION + """ - in vec2 fragTexCoord; - uniform sampler2D texture0; - uniform sampler2D texture1; - out vec4 fragColor; - uniform int engaged; - uniform int enhance_driver; - - void main() { - float y = texture(texture0, fragTexCoord).r; - vec2 uv = texture(texture1, fragTexCoord).ra - 0.5; - vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x); - if (engaged == 1) { - float gray = dot(rgb, vec3(0.299, 0.587, 0.114)); - rgb = mix(vec3(gray), rgb, 0.2); // 20% saturation - rgb = clamp((rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0); // +20% contrast - } else { - rgb *= 0.85; // 85% opacity - } - // TODO: the images out of camerad need some more correction and - // the ui should apply a gamma curve for the device display - if (enhance_driver == 1) { - float brightness = 1.1; - rgb = rgb + 0.15; - rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); - rgb = rgb * rgb * (3.0 - 2.0 * rgb); - rgb = pow(rgb, vec3(0.8)); - } - fragColor = vec4(rgb, 1.0); - } - """ - - -class CameraView(Widget): - def __init__(self, name: str, stream_type: VisionStreamType): - super().__init__() - self._name = name - # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) - self._stream_type = stream_type - self.available_streams: list[VisionStreamType] = [] - - # Target stream for switching - self._target_client: VisionIpcClient | None = None - self._target_stream_type: VisionStreamType | None = None - self._switching: bool = False - - self._texture_needs_update = True - self.last_connection_attempt: float = 0.0 - self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) - self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1 - self._engaged_loc = rl.get_shader_location(self.shader, "engaged") - self._engaged_val = rl.ffi.new("int[1]", [1]) - self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver") - self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0]) - - self.frame: VisionBuf | None = None - self.texture_y: rl.Texture | None = None - self.texture_uv: rl.Texture | None = None - - # EGL resources - self.egl_images: dict[int, EGLImage] = {} - self.egl_texture: rl.Texture | None = None - - self._placeholder_color: rl.Color | None = None - - # Initialize EGL for zero-copy rendering on TICI - if TICI: - if not init_egl(): - raise RuntimeError("Failed to initialize EGL") - - # Create a 1x1 pixel placeholder texture for EGL image binding - temp_image = rl.gen_image_color(1, 1, rl.BLACK) - self.egl_texture = rl.load_texture_from_image(temp_image) - rl.unload_image(temp_image) - - ui_state.add_offroad_transition_callback(self._offroad_transition) - - def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.frame = None - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) - - def _set_placeholder_color(self, color: rl.Color): - """Set a placeholder color to be drawn when no frame is available.""" - self._placeholder_color = color - - def switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: - return - - if self._switching and self._target_stream_type == stream_type: - return - - cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}') - - if self._target_client: - del self._target_client - - self._target_stream_type = stream_type - self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) - self._switching = True - - @property - def stream_type(self) -> VisionStreamType: - return self._stream_type - - def close(self) -> None: - self._clear_textures() - - # Clean up EGL texture - if TICI and self.egl_texture: - rl.unload_texture(self.egl_texture) - self.egl_texture = None - - # Clean up shader - if self.shader and self.shader.id: - rl.unload_shader(self.shader) - self.shader.id = 0 - - self.frame = None - self.available_streams.clear() - self.client = None - - def __del__(self): - self.close() - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - if not self.frame: - return np.eye(3) - - # Calculate aspect ratios - widget_aspect_ratio = rect.width / rect.height - frame_aspect_ratio = self.frame.width / self.frame.height - - # Calculate scaling factors to maintain aspect ratio - zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0) - zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0) - - return np.array([ - [zx, 0.0, 0.0], - [0.0, zy, 0.0], - [0.0, 0.0, 1.0] - ]) - - def _render(self, rect: rl.Rectangle): - if self._switching: - self._handle_switch() - - if not self._ensure_connection(): - self._draw_placeholder(rect) - return - - # Try to get a new buffer without blocking - buffer = self.client.recv(timeout_ms=0) - if buffer: - self._texture_needs_update = True - self.frame = buffer - elif not self.client.is_connected(): - # ensure we clear the displayed frame when the connection is lost - self.frame = None - - if not self.frame: - self._draw_placeholder(rect) - return - - transform = self._calc_frame_matrix(rect) - src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height)) - # Flip driver camera horizontally - if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER: - src_rect.width = -src_rect.width - - # Calculate scale - scale_x = rect.width * transform[0, 0] # zx - scale_y = rect.height * transform[1, 1] # zy - - # Calculate base position (centered) - x_offset = rect.x + (rect.width - scale_x) / 2 - y_offset = rect.y + (rect.height - scale_y) / 2 - - x_offset += transform[0, 2] * rect.width / 2 - y_offset += transform[1, 2] * rect.height / 2 - - dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y) - - # Render with appropriate method - if TICI: - self._render_egl(src_rect, dst_rect) - else: - self._render_textures(src_rect, dst_rect) - - def _draw_placeholder(self, rect: rl.Rectangle): - if self._placeholder_color: - rl.draw_rectangle_rec(rect, self._placeholder_color) - - def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None: - return - - idx = self.frame.idx - egl_image = self.egl_images.get(idx) - - # Create EGL image if needed - if egl_image is None: - egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset) - if egl_image: - self.egl_images[idx] = egl_image - else: - return - - # Update texture dimensions to match current frame - self.egl_texture.width = self.frame.width - self.egl_texture.height = self.frame.height - - # Bind the EGL image to our texture - bind_egl_image_to_texture(self.egl_texture.id, egl_image) - - # Render with shader - rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() - rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using texture copies""" - if not self.texture_y or not self.texture_uv or self.frame is None: - return - - # Update textures with new frame data - if self._texture_needs_update: - y_data = self.frame.data[: self.frame.uv_offset] - uv_data = self.frame.data[self.frame.uv_offset:] - - rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) - self._texture_needs_update = False - - # Render with shader - rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() - rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv) - rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _update_texture_color_filtering(self): - self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0 - rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - - def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - self.available_streams.clear() - - # Throttle connection attempts - current_time = rl.get_time() - if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: - return False - self.last_connection_attempt = current_time - - if not self.client.connect(False) or not self.client.num_buffers: - return False - - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) - - return True - - def _handle_switch(self) -> None: - """Check if target stream is ready and switch immediately.""" - if not self._target_client or not self._switching: - return - - # Try to connect target if needed - if not self._target_client.is_connected(): - if not self._target_client.connect(False) or not self._target_client.num_buffers: - return - - cloudlog.debug(f"Target stream connected: {self._target_stream_type}") - - # Check if target has frames ready - target_frame = self._target_client.recv(timeout_ms=0) - if target_frame: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() - - def _complete_switch(self) -> None: - """Instantly switch to target stream.""" - cloudlog.debug(f"Switching to {self._target_stream_type}") - # Clean up current resources - if self.client: - del self.client - - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - self._texture_needs_update = True - - # Reset state - self._target_client = None - self._target_stream_type = None - self._switching = False - - # Initialize textures for new stream - self._initialize_textures() - - def _initialize_textures(self): - self._clear_textures() - if not TICI: - self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride), - int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE)) - self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2), - int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA)) - - def _clear_textures(self): - if self.texture_y and self.texture_y.id: - rl.unload_texture(self.texture_y) - self.texture_y = None - - if self.texture_uv and self.texture_uv.id: - rl.unload_texture(self.texture_uv) - self.texture_uv = None - - # Clean up EGL resources - if TICI: - for data in self.egl_images.values(): - destroy_egl_image(data) - self.egl_images = {} - - -if __name__ == "__main__": - gui_app.init_window("camera view") - road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) - for _ in gui_app.render(): - road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) diff --git a/selfdrive/ui/mici/onroad/confidence_ball.py b/selfdrive/ui/mici/onroad/confidence_ball.py deleted file mode 100644 index a5c95470f54c1e..00000000000000 --- a/selfdrive/ui/mici/onroad/confidence_ball.py +++ /dev/null @@ -1,78 +0,0 @@ -import math -import pyray as rl -from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.lib.application import gui_app -from openpilot.common.filter_simple import FirstOrderFilter - - -def draw_circle_gradient(center_x: float, center_y: float, radius: int, - top: rl.Color, bottom: rl.Color) -> None: - # Draw a square with the gradient - rl.draw_rectangle_gradient_v(int(center_x - radius), int(center_y - radius), - radius * 2, radius * 2, - top, bottom) - - # Paint over square with a ring - outer_radius = math.ceil(radius * math.sqrt(2)) + 1 - rl.draw_ring(rl.Vector2(int(center_x), int(center_y)), radius, outer_radius, - 0.0, 360.0, - 20, rl.BLACK) - - -class ConfidenceBall(Widget): - def __init__(self, demo: bool = False): - super().__init__() - self._demo = demo - self._confidence_filter = FirstOrderFilter(-0.5, 0.5, 1 / gui_app.target_fps) - - def update_filter(self, value: float): - self._confidence_filter.update(value) - - def _update_state(self): - if self._demo: - return - - # animate status dot in from bottom - if ui_state.status == UIStatus.DISENGAGED: - self._confidence_filter.update(-0.5) - else: - self._confidence_filter.update((1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs or [1])) * - (1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.steerOverrideProbs or [1]))) - - def _render(self, _): - content_rect = rl.Rectangle( - self.rect.x + self.rect.width - SIDE_PANEL_WIDTH, - self.rect.y, - SIDE_PANEL_WIDTH, - self.rect.height, - ) - - status_dot_radius = 24 - dot_height = (1 - self._confidence_filter.x) * (content_rect.height - 2 * status_dot_radius) + status_dot_radius - dot_height = self._rect.y + dot_height - - # confidence zones - if ui_state.status == UIStatus.ENGAGED or self._demo: - if self._confidence_filter.x > 0.5: - top_dot_color = rl.Color(0, 255, 204, 255) - bottom_dot_color = rl.Color(0, 255, 38, 255) - elif self._confidence_filter.x > 0.2: - top_dot_color = rl.Color(255, 200, 0, 255) - bottom_dot_color = rl.Color(255, 115, 0, 255) - else: - top_dot_color = rl.Color(255, 0, 21, 255) - bottom_dot_color = rl.Color(255, 0, 89, 255) - - elif ui_state.status == UIStatus.OVERRIDE: - top_dot_color = rl.Color(255, 255, 255, 255) - bottom_dot_color = rl.Color(82, 82, 82, 255) - - else: - top_dot_color = rl.Color(50, 50, 50, 255) - bottom_dot_color = rl.Color(13, 13, 13, 255) - - draw_circle_gradient(content_rect.x + content_rect.width - status_dot_radius, - dot_height, status_dot_radius, - top_dot_color, bottom_dot_color) diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py deleted file mode 100644 index bab3d6e6f1df21..00000000000000 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ /dev/null @@ -1,243 +0,0 @@ -import pyray as rl -from cereal import log, messaging -from msgq.visionipc import VisionStreamType -from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView -from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer -from openpilot.selfdrive.ui.ui_state import ui_state, device -from openpilot.selfdrive.selfdrived.events import EVENTS, ET -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import NavWidget -from openpilot.system.ui.widgets.label import gui_label - -EventName = log.OnroadEvent.EventName - -EVENT_TO_INT = EventName.schema.enumerants - - -class DriverCameraView(CameraView): - def _calc_frame_matrix(self, rect: rl.Rectangle): - base = super()._calc_frame_matrix(rect) - driver_view_ratio = 1.5 - base[0, 0] *= driver_view_ratio - base[1, 1] *= driver_view_ratio - return base - - -class DriverCameraDialog(NavWidget): - def __init__(self, no_escape=False): - super().__init__() - self._camera_view = DriverCameraView("camerad", VisionStreamType.VISION_STREAM_DRIVER) - self.driver_state_renderer = DriverStateRenderer(lines=True) - self.driver_state_renderer.set_rect(rl.Rectangle(0, 0, 200, 200)) - self.driver_state_renderer.load_icons() - self._pm: messaging.PubMaster | None = None - if not no_escape: - # TODO: this can grow unbounded, should be given some thought - device.add_interactive_timeout_callback(lambda: gui_app.set_modal_overlay(None)) - self.set_back_callback(lambda: gui_app.set_modal_overlay(None)) - self.set_back_enabled(not no_escape) - - # Load eye icons - self._eye_fill_texture = None - self._eye_orange_texture = None - self._eye_size = 74 - self._glasses_texture = None - self._glasses_size = 171 - - self._load_eye_textures() - - def show_event(self): - super().show_event() - ui_state.params.put_bool("IsDriverViewEnabled", True) - self._publish_alert_sound(None) - device.set_override_interactive_timeout(300) - ui_state.params.remove("DriverTooDistracted") - self._pm = messaging.PubMaster(['selfdriveState']) - - def hide_event(self): - super().hide_event() - ui_state.params.put_bool("IsDriverViewEnabled", False) - device.set_override_interactive_timeout(None) - - def _handle_mouse_release(self, _): - ui_state.params.remove("DriverTooDistracted") - - def __del__(self): - self.close() - - def close(self): - if self._camera_view: - self._camera_view.close() - - def _update_state(self): - if self._camera_view: - self._camera_view._update_state() - # Enable driver state renderer to show Dmoji in preview - self.driver_state_renderer.set_should_draw(True) - self.driver_state_renderer.set_force_active(True) - super()._update_state() - - def _render(self, rect): - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - self._camera_view._render(rect) - - if not self._camera_view.frame: - gui_label(rect, tr("camera starting"), font_size=54, font_weight=FontWeight.BOLD, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - rl.end_scissor_mode() - self._publish_alert_sound(None) - return -1 - - driver_data = self._draw_face_detection(rect) - if driver_data is not None: - self._draw_eyes(rect, driver_data) - - # Position dmoji on opposite side from driver - driver_state_rect = ( - rect.x if self.driver_state_renderer.is_rhd else rect.x + rect.width - self.driver_state_renderer.rect.width, - rect.y + (rect.height - self.driver_state_renderer.rect.height) / 2, - ) - self.driver_state_renderer.set_position(*driver_state_rect) - self.driver_state_renderer.render() - - # Render driver monitoring alerts - self._render_dm_alerts(rect) - - rl.end_scissor_mode() - return -1 - - def _publish_alert_sound(self, dm_state): - """Publish selfdriveState with only alertSound field set""" - if self._pm is None: - return - - msg = messaging.new_message('selfdriveState') - if dm_state is not None and len(dm_state.events): - event_name = EVENT_TO_INT[dm_state.events[0].name] - if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]: - msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert - self._pm.send('selfdriveState', msg) - - def _render_dm_alerts(self, rect: rl.Rectangle): - """Render driver monitoring event names""" - dm_state = ui_state.sm["driverMonitoringState"] - self._publish_alert_sound(dm_state) - - gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height), - f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, - color=rl.Color(0, 0, 0, 180)) - gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, - color=rl.Color(255, 255, 255, int(255 * 0.9))) - - if not dm_state.events: - return - - # Show first event (only one should be active at a time) - event_name_str = str(dm_state.events[0].name).split('.')[-1] - alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT - - shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height) - gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, - alignment=alignment, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, - color=rl.Color(0, 0, 0, 180)) - gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, - alignment=alignment, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, - color=rl.Color(255, 255, 255, int(255 * 0.9))) - - def _load_eye_textures(self): - """Lazy load eye textures""" - if self._eye_fill_texture is None: - self._eye_fill_texture = gui_app.texture("icons_mici/onroad/eye_fill.png", self._eye_size, self._eye_size) - if self._eye_orange_texture is None: - self._eye_orange_texture = gui_app.texture("icons_mici/onroad/eye_orange.png", self._eye_size, self._eye_size) - if self._glasses_texture is None: - self._glasses_texture = gui_app.texture("icons_mici/onroad/glasses.png", self._glasses_size, self._glasses_size) - - def _draw_face_detection(self, rect: rl.Rectangle): - dm_state = ui_state.sm["driverMonitoringState"] - driver_data = self.driver_state_renderer.get_driver_data() - if not dm_state.faceDetected: - return - - # Get face position and orientation - face_x, face_y = driver_data.facePosition - face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) - alpha = 0.7 - if face_std > 0.15: - alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) - - # use approx instead of distort_points - # TODO: replace with distort_points - tici_x = 1080.0 - 1714.0 * face_x - tici_y = -135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y - - # Tici coords are relative to center, scale offset - offset_x = (tici_x - 1080.0) * 1.25 - offset_y = (tici_y - 540.0) * 1.25 - - # Map to mici screen (scale from 2160x1080 to rect dimensions) - scale_x = rect.width / 2160.0 - scale_y = rect.height / 1080.0 - fbox_x = rect.x + rect.width / 2 + offset_x * scale_x - fbox_y = rect.y + rect.height / 2 + offset_y * scale_y - box_size = 75 - line_thickness = 3 - - line_color = rl.Color(255, 255, 255, int(alpha * 255)) - rl.draw_rectangle_rounded_lines_ex( - rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), - 35.0 / box_size / 2, - line_thickness, - line_thickness, - line_color, - ) - return driver_data - - def _draw_eyes(self, rect: rl.Rectangle, driver_data): - # Draw eye indicators based on eye probabilities - eye_offset_x = 10 - eye_offset_y = 10 - eye_spacing = self._eye_size + 15 - - left_eye_x = rect.x + eye_offset_x - left_eye_y = rect.y + eye_offset_y - left_eye_prob = driver_data.leftEyeProb - - right_eye_x = rect.x + eye_offset_x + eye_spacing - right_eye_y = rect.y + eye_offset_y - right_eye_prob = driver_data.rightEyeProb - - # Draw eyes with opacity based on probability - for eye_x, eye_y, eye_prob in [(left_eye_x, left_eye_y, left_eye_prob), (right_eye_x, right_eye_y, right_eye_prob)]: - fill_opacity = eye_prob - orange_opacity = 1.0 - eye_prob - - rl.draw_texture_v(self._eye_orange_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * orange_opacity))) - rl.draw_texture_v(self._eye_fill_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * fill_opacity))) - - # Draw sunglasses indicator based on sunglasses probability - # Position glasses centered between the two eyes at top left - glasses_x = rect.x + eye_offset_x - 4 - glasses_y = rect.y - glasses_pos = rl.Vector2(glasses_x, glasses_y) - glasses_prob = driver_data.sunglassesProb - rl.draw_texture_v(self._glasses_texture, glasses_pos, rl.Color(70, 80, 161, int(255 * glasses_prob))) - - -if __name__ == "__main__": - gui_app.init_window("Driver Camera View (mici)") - - driver_camera_view = DriverCameraDialog() - try: - for _ in gui_app.render(): - ui_state.update() - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - driver_camera_view.close() diff --git a/selfdrive/ui/mici/onroad/driver_state.py b/selfdrive/ui/mici/onroad/driver_state.py deleted file mode 100644 index 356d7ac832cfa1..00000000000000 --- a/selfdrive/ui/mici/onroad/driver_state.py +++ /dev/null @@ -1,220 +0,0 @@ -import pyray as rl -import numpy as np -import math -from cereal import log -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state - -AlertSize = log.SelfdriveState.AlertSize - -DEBUG = False - -LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) -LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) - - -class DriverStateRenderer(Widget): - BASE_SIZE = 60 - LINES_ANGLE_INCREMENT = 5 - LINES_STALE_ANGLES = 3.0 # seconds - - def __init__(self, lines: bool = False, inset: bool = False): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, self.BASE_SIZE, self.BASE_SIZE)) - self._lines = lines - self._inset = inset - - # In line mode, track smoothed angles - assert 360 % self.LINES_ANGLE_INCREMENT == 0 - self._head_angles = {i * self.LINES_ANGLE_INCREMENT: FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) for i in range(360 // self.LINES_ANGLE_INCREMENT)} - - self._is_active = False - self._is_rhd = False - self._face_detected = False - self._should_draw = False - self._force_active = False - self._looking_center = False - - self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) - self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) - self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) - self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) - self._looking_center_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - - # Load the driver face icons - self.load_icons() - - def load_icons(self): - """Load or reload the driver face icon texture""" - cone_and_person_size = round(52 / self.BASE_SIZE * self._rect.width) - - # If inset is enabled, push cone and person smaller by 2x the current inset space - if self._inset: - # Current inset space = (rect.width - cone_and_person_size) / 2 - current_inset = (self._rect.width - cone_and_person_size) / 2 - # Reduce size by 2x the current inset (1x on each side) - cone_and_person_size = round(cone_and_person_size - current_inset * 2) - - self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", cone_and_person_size, cone_and_person_size) - self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size) - center_size = round(36 / self.BASE_SIZE * self._rect.width) - self._dm_center = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_center.png", center_size, center_size) - self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", self._rect.width, self._rect.height) - - def set_should_draw(self, should_draw: bool): - self._should_draw = should_draw - - @property - def should_draw(self): - return (self._should_draw and ui_state.sm["selfdriveState"].alertSize == AlertSize.none and - ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame) - - def set_force_active(self, force_active: bool): - """Force the dmoji to always appear active (green) regardless of actual state""" - self._force_active = force_active - - @property - def effective_active(self) -> bool: - """Returns True if dmoji should appear active (either actually active or forced)""" - return bool(self._force_active or self._is_active) - - @property - def is_rhd(self) -> bool: - return self._is_rhd - - def _render(self, _): - if DEBUG: - rl.draw_rectangle_lines_ex(self._rect, 1, rl.RED) - - rl.draw_texture(self._dm_background, - int(self._rect.x), - int(self._rect.y), - rl.Color(255, 255, 255, int(255 * self._fade_filter.x))) - - rl.draw_texture(self._dm_person, - int(self._rect.x + (self._rect.width - self._dm_person.width) / 2), - int(self._rect.y + (self._rect.height - self._dm_person.height) / 2), - rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) - - if self.effective_active: - source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) - dest_rect = rl.Rectangle( - self._rect.x + self._rect.width / 2, - self._rect.y + self._rect.height / 2, - self._dm_cone.width, - self._dm_cone.height, - ) - - if not self._lines: - rl.draw_texture_pro( - self._dm_cone, - source_rect, - dest_rect, - rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), - self._rotation_filter.x - 90, - rl.Color(255, 255, 255, int(255 * self._fade_filter.x * (1 - self._looking_center_filter.x))), - ) - - rl.draw_texture_ex( - self._dm_center, - (int(self._rect.x + (self._rect.width - self._dm_center.width) / 2), - int(self._rect.y + (self._rect.height - self._dm_center.height) / 2)), - 0, - 1.0, - rl.Color(255, 255, 255, int(255 * self._fade_filter.x * self._looking_center_filter.x)), - ) - - else: - # remove old angles - for angle, f in self._head_angles.items(): - dst_from_current = ((angle - self._rotation_filter.x) % 360) - 180 - target = 1.0 if abs(dst_from_current) <= self.LINES_ANGLE_INCREMENT * 5 else 0.0 - if not self._face_detected: - target = 0.0 - - # Reduce all line lengths when looking center - if self._looking_center: - target = np.interp(self._looking_center_filter.x, [0.0, 1.0], [target, 0.45]) - - f.update(target) - self._draw_line(angle, f, self._looking_center) - - def _draw_line(self, angle: int, f: FirstOrderFilter, grey: bool): - line_length = self._rect.width / 6 - line_length = round(np.interp(f.x, [0.0, 1.0], [0, line_length])) - line_offset = self._rect.width / 2 - line_length * 2 # ensure line ends within rect - center_x = self._rect.x + self._rect.width / 2 - center_y = self._rect.y + self._rect.height / 2 - start_x = center_x + (line_offset + line_length) * math.cos(math.radians(angle)) - start_y = center_y + (line_offset + line_length) * math.sin(math.radians(angle)) - end_x = start_x + line_length * math.cos(math.radians(angle)) - end_y = start_y + line_length * math.sin(math.radians(angle)) - color = rl.Color(0, 255, 64, 255) - - if grey: - color = rl.Color(166, 166, 166, 255) - - if f.x > 0.01: - rl.draw_line_ex((start_x, start_y), (end_x, end_y), 12, color) - - def get_driver_data(self): - sm = ui_state.sm - - dm_state = sm["driverMonitoringState"] - self._is_active = dm_state.isActiveMode - self._is_rhd = dm_state.isRHD - self._face_detected = dm_state.faceDetected - - driverstate = sm["driverStateV2"] - driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData - return driver_data - - def _update_state(self): - # Get monitoring state - driver_data = self.get_driver_data() - driver_orient = driver_data.faceOrientation - - if len(driver_orient) != 3: - return - - pitch, yaw, roll = driver_orient - pitch = self._pitch_filter.update(pitch) - yaw = self._yaw_filter.update(yaw) - - # hysteresis on looking center - if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: - self._looking_center = True - elif abs(pitch) > LOOKING_CENTER_THRESHOLD_UPPER or abs(yaw) > LOOKING_CENTER_THRESHOLD_UPPER: - self._looking_center = False - self._looking_center_filter.update(1 if self._looking_center else 0) - - if DEBUG: - pitchd = math.degrees(pitch) - yawd = math.degrees(yaw) - rolld = math.degrees(roll) - - rl.draw_line_ex((0, 100), (200, 100), 3, rl.RED) - rl.draw_line_ex((0, 120), (200, 120), 3, rl.RED) - rl.draw_line_ex((0, 140), (200, 140), 3, rl.RED) - - pitch_x = 100 + pitchd - yaw_x = 100 + yawd - roll_x = 100 + rolld - rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) - rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) - rl.draw_circle(int(roll_x), 140, 5, rl.GREEN) - - # filter head rotation, handling wrap-around - rotation = math.degrees(math.atan2(pitch, yaw)) - angle_diff = rotation - self._rotation_filter.x - angle_diff = ((angle_diff + 180) % 360) - 180 - self._rotation_filter.update(self._rotation_filter.x + angle_diff) - - if not self.should_draw: - self._fade_filter.update(0.0) - elif not self.effective_active: - self._fade_filter.update(0.35) - else: - self._fade_filter.update(1.0) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py deleted file mode 100644 index a6fa1a62bbac41..00000000000000 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ /dev/null @@ -1,277 +0,0 @@ -import pyray as rl -from dataclasses import dataclass -from openpilot.common.constants import CV -from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget -from openpilot.common.filter_simple import FirstOrderFilter -from cereal import log - -EventName = log.OnroadEvent.EventName - -# Constants -SET_SPEED_NA = 255 -KM_TO_MILE = 0.621371 -CRUISE_DISABLED_CHAR = '–' - -SET_SPEED_PERSISTENCE = 2.5 # seconds - - -@dataclass(frozen=True) -class FontSizes: - current_speed: int = 176 - speed_unit: int = 66 - max_speed: int = 36 - set_speed: int = 112 - - -@dataclass(frozen=True) -class Colors: - WHITE = rl.WHITE - WHITE_TRANSLUCENT = rl.Color(255, 255, 255, 200) - - -FONT_SIZES = FontSizes() -COLORS = Colors() - - -class TurnIntent(Widget): - FADE_IN_ANGLE = 30 # degrees - - def __init__(self): - super().__init__() - self._pre = False - self._turn_intent_direction: int = 0 - - self._turn_intent_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - self._turn_intent_rotation_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) - - self._txt_turn_intent_left: rl.Texture = gui_app.texture('icons_mici/turn_intent_left.png', 50, 20) - self._txt_turn_intent_right: rl.Texture = gui_app.texture('icons_mici/turn_intent_right.png', 50, 20) - - def _render(self, _): - if self._turn_intent_alpha_filter.x > 1e-2: - turn_intent_texture = self._txt_turn_intent_right if self._turn_intent_direction == 1 else self._txt_turn_intent_left - src_rect = rl.Rectangle(0, 0, turn_intent_texture.width, turn_intent_texture.height) - dest_rect = rl.Rectangle(self._rect.x + self._rect.width / 2, self._rect.y + self._rect.height / 2, - turn_intent_texture.width, turn_intent_texture.height) - - origin = (turn_intent_texture.width / 2, self._rect.height / 2) - color = rl.Color(255, 255, 255, int(255 * self._turn_intent_alpha_filter.x)) - rl.draw_texture_pro(turn_intent_texture, src_rect, dest_rect, origin, self._turn_intent_rotation_filter.x, color) - - def _update_state(self) -> None: - sm = ui_state.sm - - left = any(e.name == EventName.preLaneChangeLeft for e in sm['onroadEvents']) - right = any(e.name == EventName.preLaneChangeRight for e in sm['onroadEvents']) - if left or right: - # pre lane change - if not self._pre: - self._turn_intent_rotation_filter.x = self.FADE_IN_ANGLE if left else -self.FADE_IN_ANGLE - - self._pre = True - self._turn_intent_direction = -1 if left else 1 - self._turn_intent_alpha_filter.update(1) - self._turn_intent_rotation_filter.update(0) - elif any(e.name == EventName.laneChange for e in sm['onroadEvents']): - # fade out and rotate away - self._pre = False - self._turn_intent_alpha_filter.update(0) - - if self._turn_intent_direction == 0: - # unknown. missed pre frame? - self._turn_intent_rotation_filter.update(0) - else: - self._turn_intent_rotation_filter.update(self._turn_intent_direction * self.FADE_IN_ANGLE) - else: - # didn't complete lane change, just hide - self._pre = False - self._turn_intent_direction = 0 - self._turn_intent_alpha_filter.update(0) - self._turn_intent_rotation_filter.update(0) - - -class HudRenderer(Widget): - def __init__(self): - super().__init__() - """Initialize the HUD renderer.""" - self.is_cruise_set: bool = False - self.is_cruise_available: bool = True - self.set_speed: float = SET_SPEED_NA - self._set_speed_changed_time: float = 0 - self.speed: float = 0.0 - self.v_ego_cluster_seen: bool = False - self._engaged: bool = False - - self._can_draw_top_icons = True - self._show_wheel_critical = False - - self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD) - self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM) - self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD) - self._font_display: rl.Font = gui_app.font(FontWeight.DISPLAY) - - self._turn_intent = TurnIntent() - self._torque_bar = TorqueBar() - - self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50) - self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50) - self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44) - - self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) - - self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - - def set_wheel_critical_icon(self, critical: bool): - """Set the wheel icon to critical or normal state.""" - self._show_wheel_critical = critical - - def set_can_draw_top_icons(self, can_draw_top_icons: bool): - """Set whether to draw the top part of the HUD.""" - self._can_draw_top_icons = can_draw_top_icons - - def drawing_top_icons(self) -> bool: - # whether we're drawing any top icons currently - return bool(self._set_speed_alpha_filter.x > 1e-2) - - def _update_state(self) -> None: - """Update HUD state based on car state and controls state.""" - sm = ui_state.sm - if sm.recv_frame["carState"] < ui_state.started_frame: - self.is_cruise_set = False - self.set_speed = SET_SPEED_NA - self.speed = 0.0 - return - - controls_state = sm['controlsState'] - car_state = sm['carState'] - - v_cruise_cluster = car_state.vCruiseCluster - set_speed = ( - controls_state.vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster - ) - engaged = sm['selfdriveState'].enabled - if (set_speed != self.set_speed and engaged) or (engaged and not self._engaged): - self._set_speed_changed_time = rl.get_time() - self._engaged = engaged - self.set_speed = set_speed - self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA - self.is_cruise_available = self.set_speed != -1 - - v_ego_cluster = car_state.vEgoCluster - self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0 - v_ego = v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo - speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH - self.speed = max(0.0, v_ego * speed_conversion) - - def _render(self, rect: rl.Rectangle) -> None: - """Render HUD elements to the screen.""" - - if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState': - self._torque_bar.render(rect) - - if self.is_cruise_set: - self._draw_set_speed(rect) - - self._draw_steering_wheel(rect) - - def _draw_steering_wheel(self, rect: rl.Rectangle) -> None: - wheel_txt = self._txt_wheel_critical if self._show_wheel_critical else self._txt_wheel - - if self._show_wheel_critical: - self._wheel_alpha_filter.update(255) - self._wheel_y_filter.update(0) - else: - if ui_state.status == UIStatus.DISENGAGED: - self._wheel_alpha_filter.update(0) - self._wheel_y_filter.update(wheel_txt.height / 2) - else: - self._wheel_alpha_filter.update(255 * 0.9) - self._wheel_y_filter.update(0) - - # pos - pos_x = int(rect.x + 21 + wheel_txt.width / 2) - pos_y = int(rect.y + rect.height - 14 - wheel_txt.height / 2 + self._wheel_y_filter.x) - rotation = -ui_state.sm['carState'].steeringAngleDeg - - turn_intent_margin = 25 - self._turn_intent.render(rl.Rectangle( - pos_x - wheel_txt.width / 2 - turn_intent_margin, - pos_y - wheel_txt.height / 2 - turn_intent_margin, - wheel_txt.width + turn_intent_margin * 2, - wheel_txt.height + turn_intent_margin * 2, - )) - - src_rect = rl.Rectangle(0, 0, wheel_txt.width, wheel_txt.height) - dest_rect = rl.Rectangle(pos_x, pos_y, wheel_txt.width, wheel_txt.height) - origin = (wheel_txt.width / 2, wheel_txt.height / 2) - - # color and draw - color = rl.Color(255, 255, 255, int(self._wheel_alpha_filter.x)) - rl.draw_texture_pro(wheel_txt, src_rect, dest_rect, origin, rotation, color) - - if self._show_wheel_critical: - # Draw exclamation point icon - EXCLAMATION_POINT_SPACING = 10 - exclamation_pos_x = pos_x - self._txt_exclamation_point.width / 2 + wheel_txt.width / 2 + EXCLAMATION_POINT_SPACING - exclamation_pos_y = pos_y - self._txt_exclamation_point.height / 2 - rl.draw_texture(self._txt_exclamation_point, int(exclamation_pos_x), int(exclamation_pos_y), rl.WHITE) - - def _draw_set_speed(self, rect: rl.Rectangle) -> None: - """Draw the MAX speed indicator box.""" - alpha = self._set_speed_alpha_filter.update(0 < rl.get_time() - self._set_speed_changed_time < SET_SPEED_PERSISTENCE and - self._can_draw_top_icons and self._engaged) - if alpha < 1e-2: - return - - x = rect.x - y = rect.y - - # draw drop shadow - circle_radius = 162 // 2 - rl.draw_circle_gradient(int(x + circle_radius), int(y + circle_radius), circle_radius, - rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK) - - set_speed_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha)) - max_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha)) - - set_speed = self.set_speed - if self.is_cruise_set and not ui_state.is_metric: - set_speed *= KM_TO_MILE - - set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(set_speed)) - rl.draw_text_ex( - self._font_display, - set_speed_text, - rl.Vector2(x + 13 + 4, y + 3 - 8 - 3 + 4), - FONT_SIZES.set_speed, - 0, - set_speed_color, - ) - - max_text = tr("MAX") - rl.draw_text_ex( - self._font_semi_bold, - max_text, - rl.Vector2(x + 25, y + FONT_SIZES.set_speed - 7 + 4), - FONT_SIZES.max_speed, - 0, - max_color, - ) - - def _draw_current_speed(self, rect: rl.Rectangle) -> None: - """Draw the current vehicle speed and unit.""" - speed_text = str(round(self.speed)) - speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed) - speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2) - rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.WHITE) - - unit_text = tr("km/h") if ui_state.is_metric else tr("mph") - unit_text_size = measure_text_cached(self._font_medium, unit_text, FONT_SIZES.speed_unit) - unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2) - rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.WHITE_TRANSLUCENT) diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py deleted file mode 100644 index 3f1badfe84411f..00000000000000 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ /dev/null @@ -1,479 +0,0 @@ -import colorsys -import numpy as np -import pyray as rl -from cereal import messaging, car -from dataclasses import dataclass, field -from openpilot.common.params import Params -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.selfdrive.ui.mici.onroad import blend_colors -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient -from openpilot.system.ui.widgets import Widget - -CLIP_MARGIN = 500 -MIN_DRAW_DISTANCE = 10.0 -MAX_DRAW_DISTANCE = 100.0 - -THROTTLE_COLORS = [ - rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) - rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35) - rl.Color(114, 255, 92, 0), # HSLF(112/360, 1.0, 0.68, 0.0) -] - -NO_THROTTLE_COLORS = [ - rl.Color(242, 242, 242, 102), # HSLF(148/360, 0.0, 0.95, 0.4) - rl.Color(242, 242, 242, 89), # HSLF(112/360, 0.0, 0.95, 0.35) - rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0) -] - -LANE_LINE_COLORS = { - UIStatus.DISENGAGED: rl.Color(200, 200, 200, 255), - UIStatus.OVERRIDE: rl.Color(255, 255, 255, 255), - UIStatus.ENGAGED: rl.Color(0, 255, 64, 255), -} - - -@dataclass -class ModelPoints: - raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32)) - projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32)) - - -@dataclass -class LeadVehicle: - glow: list[float] = field(default_factory=list) - chevron: list[float] = field(default_factory=list) - fill_alpha: int = 0 - - -class ModelRenderer(Widget): - def __init__(self): - super().__init__() - self._longitudinal_control = False - self._experimental_mode = False - self._blend_filter = FirstOrderFilter(1.0, 0.25, 1 / gui_app.target_fps) - self._prev_allow_throttle = True - self._lane_line_probs = np.zeros(4, dtype=np.float32) - self._road_edge_stds = np.zeros(2, dtype=np.float32) - self._lead_vehicles = [LeadVehicle(), LeadVehicle()] - self._path_offset_z = HEIGHT_INIT[0] - - # Initialize ModelPoints objects - self._path = ModelPoints() - self._lane_lines = [ModelPoints() for _ in range(4)] - self._road_edges = [ModelPoints() for _ in range(2)] - self._acceleration_x = np.empty((0,), dtype=np.float32) - - self._acceleration_x_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - self._acceleration_x_filter2 = FirstOrderFilter(0.0, 1, 1 / gui_app.target_fps) - - self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) - self._ll_color_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - - # Transform matrix (3x3 for car space to screen space) - self._car_space_transform = np.zeros((3, 3), dtype=np.float32) - self._transform_dirty = True - self._clip_region = None - - self._exp_gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=[], - stops=[], - ) - - # Get longitudinal control setting from car parameters - if car_params := Params().get("CarParams"): - cp = messaging.log_from_bytes(car_params, car.CarParams) - self._longitudinal_control = cp.openpilotLongitudinalControl - - def set_transform(self, transform: np.ndarray): - self._car_space_transform = transform.astype(np.float32) - self._transform_dirty = True - - def _render(self, rect: rl.Rectangle): - sm = ui_state.sm - - self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) - - # Check if data is up-to-date - if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or - sm.recv_frame["modelV2"] < ui_state.started_frame): - return - - # Set up clipping region - self._clip_region = rl.Rectangle( - rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN - ) - - # Update state - self._experimental_mode = sm['selfdriveState'].experimentalMode - - live_calib = sm['liveCalibration'] - self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0] - - if sm.updated['carParams']: - self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl - - model = sm['modelV2'] - radar_state = sm['radarState'] if sm.valid['radarState'] else None - lead_one = radar_state.leadOne if radar_state else None - render_lead_indicator = self._longitudinal_control and radar_state is not None - - # Update model data when needed - model_updated = sm.updated['modelV2'] - if model_updated or sm.updated['radarState'] or self._transform_dirty: - if model_updated: - self._update_raw_points(model) - - path_x_array = self._path.raw_points[:, 0] - if path_x_array.size == 0: - return - - self._update_model(lead_one, path_x_array) - if render_lead_indicator: - self._update_leads(radar_state, path_x_array) - self._transform_dirty = False - - # Draw elements (hide when disengaged) - if ui_state.status != UIStatus.DISENGAGED: - self._draw_lane_lines() - self._draw_path(sm) - - # if render_lead_indicator and radar_state: - # self._draw_lead_indicator() - - def _update_raw_points(self, model): - """Update raw 3D points from model data""" - self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T - - for i, lane_line in enumerate(model.laneLines): - self._lane_lines[i].raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T - - for i, road_edge in enumerate(model.roadEdges): - self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T - - self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32) - self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32) - self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32) - - def _update_leads(self, radar_state, path_x_array): - """Update positions of lead vehicles""" - self._lead_vehicles = [LeadVehicle(), LeadVehicle()] - leads = [radar_state.leadOne, radar_state.leadTwo] - - for i, lead_data in enumerate(leads): - if lead_data and lead_data.status: - d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel - idx = self._get_path_length_idx(path_x_array, d_rel) - - # Get z-coordinate from path at the lead vehicle position - z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 - point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) - if point: - self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) - - def _update_model(self, lead, path_x_array): - """Update model visualization data based on model message""" - max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) - max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) - - # Update lane lines using raw points - line_width_factor = 0.12 - for i, lane_line in enumerate(self._lane_lines): - if i in (1, 2): - line_width_factor = 0.16 - lane_line.projected_points = self._map_line_to_polygon( - lane_line.raw_points, line_width_factor * self._lane_line_probs[i], 0.0, max_idx - ) - - # Update road edges using raw points - for road_edge in self._road_edges: - road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, line_width_factor, 0.0, max_idx) - - # Update path using raw points - if lead and lead.status: - lead_d = lead.dRel * 2.0 - max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) - - soon_acceleration = self._acceleration_x[len(self._acceleration_x) // 4] if len(self._acceleration_x) > 0 else 0 - self._acceleration_x_filter.update(soon_acceleration) - self._acceleration_x_filter2.update(soon_acceleration) - - # make path width wider/thinner when initially braking/accelerating - if self._experimental_mode and False: - high_pass_acceleration = self._acceleration_x_filter.x - self._acceleration_x_filter2.x - y_off = np.interp(high_pass_acceleration, [-1, 0, 1], [0.9 * 2, 0.9, 0.9 / 2]) - else: - y_off = 0.9 - - max_idx = self._get_path_length_idx(path_x_array, max_distance) - self._path.projected_points = self._map_line_to_polygon( - self._path.raw_points, y_off, self._path_offset_z, max_idx, allow_invert=False - ) - - self._update_experimental_gradient() - - def _update_experimental_gradient(self): - """Pre-calculate experimental mode gradient colors""" - if not self._experimental_mode: - return - - max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) - - segment_colors = [] - gradient_stops = [] - - i = 0 - while i < max_len: - # Some points (screen space) are out of frame (rect space) - track_y = self._path.projected_points[i][1] - if track_y < self._rect.y or track_y > (self._rect.y + self._rect.height): - i += 1 - continue - - # Calculate color based on acceleration (0 is bottom, 1 is top) - lin_grad_point = 1 - (track_y - self._rect.y) / self._rect.height - - # speed up: 120, slow down: 0 - path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120) - - saturation = min(abs(self._acceleration_x[i] * 1.5), 1) - lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62]) - alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0]) - - # Use HSL to RGB conversion - color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) - - gradient_stops.append(lin_grad_point) - segment_colors.append(color) - - # Skip a point, unless next is last - i += 1 + (1 if (i + 2) < max_len else 0) - - # Store the gradient in the path object - self._exp_gradient.colors = segment_colors - self._exp_gradient.stops = gradient_stops - - def _update_lead_vehicle(self, d_rel, v_rel, point, rect): - speed_buff, lead_buff = 10.0, 40.0 - - # Calculate fill alpha - fill_alpha = 0 - if d_rel < lead_buff: - fill_alpha = 255 * (1.0 - (d_rel / lead_buff)) - if v_rel < 0: - fill_alpha += 255 * (-1 * (v_rel / speed_buff)) - fill_alpha = min(fill_alpha, 255) - - # Calculate size and position - sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 1 - x = np.clip(point[0], 0.0, rect.width - sz / 2) - y = min(point[1], rect.height - sz * 0.6) - - g_xo = sz / 5 - g_yo = sz / 10 - - glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)] - chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)] - - return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha)) - - def _get_ll_color(self, prob: float, adjacent: bool, left: bool): - alpha = np.clip(prob, 0.0, 0.7) - if adjacent: - _base_color = LANE_LINE_COLORS.get(ui_state.status, LANE_LINE_COLORS[UIStatus.DISENGAGED]) - color = rl.Color(_base_color.r, _base_color.g, _base_color.b, int(alpha * 255)) - - # turn adjacent lls orange if torque is high - torque = self._torque_filter.x - high_torque = abs(torque) > 0.6 - if high_torque and (left == (torque > 0)): - color = blend_colors( - color, - rl.Color(255, 115, 0, int(alpha * 255)), # orange - np.interp(abs(torque), [0.6, 0.8], [0.0, 1.0]) - ) - else: - color = rl.Color(255, 255, 255, int(alpha * 255)) - - if ui_state.status == UIStatus.DISENGAGED: - color = rl.Color(0, 0, 0, int(alpha * 255)) - - return color - - def _draw_lane_lines(self): - """Draw lane lines and road edges""" - """Two closest lines should be green (lane line or road edges)""" - for i, lane_line in enumerate(self._lane_lines): - if lane_line.projected_points.size == 0: - continue - - color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1)) - draw_polygon(self._rect, lane_line.projected_points, color) - - for i, road_edge in enumerate(self._road_edges): - if road_edge.projected_points.size == 0: - continue - - # if closest lane lines are not confident, make road edges green - color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0) - draw_polygon(self._rect, road_edge.projected_points, color) - - def _draw_path(self, sm): - """Draw path with dynamic coloring based on mode and throttle state.""" - if not self._path.projected_points.size: - return - - allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control - self._blend_filter.update(int(allow_throttle)) - - if self._experimental_mode: - # Draw with acceleration coloring - if ui_state.status == UIStatus.DISENGAGED: - draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) - elif len(self._exp_gradient.colors) > 1: - draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) - else: - draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) - else: - # Blend throttle/no throttle colors based on transition - blend_factor = round(self._blend_filter.x * 100) / 100 - blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) - gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=blended_colors, - stops=[0.0, 0.5, 1.0], - ) - - if ui_state.status == UIStatus.DISENGAGED: - draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) - else: - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - - def _draw_lead_indicator(self): - # Draw lead vehicles if available - for lead in self._lead_vehicles: - if not lead.glow or not lead.chevron: - continue - - rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) - rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha)) - - @staticmethod - def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: - """Get the index corresponding to the given path height""" - if len(pos_x_array) == 0: - return 0 - indices = np.where(pos_x_array <= path_height)[0] - return indices[-1] if indices.size > 0 else 0 - - def _map_to_screen(self, in_x, in_y, in_z): - """Project a point in car space to screen space""" - input_pt = np.array([in_x, in_y, in_z]) - pt = self._car_space_transform @ input_pt - - if abs(pt[2]) < 1e-6: - return None - - x, y = pt[0] / pt[2], pt[1] / pt[2] - - clip = self._clip_region - if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height): - return None - - return (x, y) - - def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray: - """Convert 3D line to 2D polygon for rendering.""" - if line.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) - - # Slice points and filter non-negative x-coordinates - points = line[:max_idx + 1] - points = points[points[:, 0] >= 0] - if points.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) - - N = points.shape[0] - # Generate left and right 3D points in one array using broadcasting - offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32) - points_3d = points[None, :, :] + offsets[:, None, :] # Shape: 2xNx3 - points_3d = points_3d.reshape(2 * N, 3) # Shape: (2*N)x3 - - # Transform all points to projected space in one operation - proj = self._car_space_transform @ points_3d.T # Shape: 3x(2*N) - proj = proj.reshape(3, 2, N) - left_proj = proj[:, 0, :] - right_proj = proj[:, 1, :] - - # Filter points where z is sufficiently large - valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6) - if not np.any(valid_proj): - return np.empty((0, 2), dtype=np.float32) - - # Compute screen coordinates - left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :] - right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :] - - # Define clip region bounds - clip = self._clip_region - x_min, x_max = clip.x, clip.x + clip.width - y_min, y_max = clip.y, clip.y + clip.height - - # Filter points within clip region - left_in_clip = ( - (left_screen[0] >= x_min) & (left_screen[0] <= x_max) & - (left_screen[1] >= y_min) & (left_screen[1] <= y_max) - ) - right_in_clip = ( - (right_screen[0] >= x_min) & (right_screen[0] <= x_max) & - (right_screen[1] >= y_min) & (right_screen[1] <= y_max) - ) - both_in_clip = left_in_clip & right_in_clip - - if not np.any(both_in_clip): - return np.empty((0, 2), dtype=np.float32) - - # Select valid and clipped points - left_screen = left_screen[:, both_in_clip] - right_screen = right_screen[:, both_in_clip] - - # Handle Y-coordinate inversion on hills - if not allow_invert and left_screen.shape[1] > 1: - y = left_screen[1, :] # y-coordinates - keep = y == np.minimum.accumulate(y) - if not np.any(keep): - return np.empty((0, 2), dtype=np.float32) - left_screen = left_screen[:, keep] - right_screen = right_screen[:, keep] - - return np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32) - - @staticmethod - def _hsla_to_color(h, s, l, a): - rgb = colorsys.hls_to_rgb(h, l, s) - return rl.Color( - int(rgb[0] * 255), - int(rgb[1] * 255), - int(rgb[2] * 255), - int(a * 255) - ) - - @staticmethod - def _blend_colors(begin_colors, end_colors, t): - if t >= 1.0: - return end_colors - if t <= 0.0: - return begin_colors - - inv_t = 1.0 - t - return [rl.Color( - int(inv_t * start.r + t * end.r), - int(inv_t * start.g + t * end.g), - int(inv_t * start.b + t * end.b), - int(inv_t * start.a + t * end.a) - ) for start, end in zip(begin_colors, end_colors, strict=True)] diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py deleted file mode 100644 index d7c9f27a92d225..00000000000000 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ /dev/null @@ -1,256 +0,0 @@ -import math -import time -from functools import wraps -from collections import OrderedDict - -import numpy as np -import pyray as rl -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY -from openpilot.selfdrive.ui.mici.onroad import blend_colors -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient -from openpilot.system.ui.widgets import Widget -from openpilot.common.filter_simple import FirstOrderFilter - -# TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span -TORQUE_ANGLE_SPAN = 12.7 - -DEBUG = False - - -def quantized_lru_cache(maxsize=128): - def decorator(func): - cache = OrderedDict() - @wraps(func) - def wrapper(cx, cy, r_mid, thickness, a0_deg, a1_deg, **kwargs): - # Quantize inputs: balanced for smoothness vs cache effectiveness - key = (round(cx), round(cy), round(r_mid), - round(thickness), # 1px precision for smoother height transitions - round(a0_deg * 10) / 10, # 0.1° precision for smoother angle transitions - round(a1_deg * 10) / 10, - tuple(sorted(kwargs.items()))) - - if key in cache: - cache.move_to_end(key) - else: - if len(cache) >= maxsize: - cache.popitem(last=False) - - result = func(cx, cy, r_mid, thickness, a0_deg, a1_deg, **kwargs) - cache[key] = result - return cache[key] - return wrapper - return decorator - - -@quantized_lru_cache(maxsize=256) -def arc_bar_pts(cx: float, cy: float, - r_mid: float, thickness: float, - a0_deg: float, a1_deg: float, - *, max_points: int = 100, cap_segs: int = 10, - cap_radius: float = 7, px_per_seg: float = 2.0) -> np.ndarray: - """Return Nx2 np.float32 points for a single closed polygon (rounded thick arc).""" - - def get_cap(left: bool, a_deg: float): - # end cap at a1: center (a1), sweep a1→a1+180 (skip endpoints to avoid dupes) - # quarter arc (outer corner) at a1 with fixed pixel radius cap_radius - - nx, ny = math.cos(math.radians(a_deg)), math.sin(math.radians(a_deg)) # outward normal - tx, ty = -ny, nx # tangent (CCW) - - mx, my = cx + nx * r_mid, cy + ny * r_mid # mid-point at a1 - if DEBUG: - rl.draw_circle(int(mx), int(my), 4, rl.PURPLE) - - ex = mx + nx * (half - cap_radius) - ey = my + ny * (half - cap_radius) - - if DEBUG: - rl.draw_circle(int(ex), int(ey), 2, rl.WHITE) - - # sweep 90° in the local (t,n) frame: from outer edge toward inside - if not left: - alpha = np.deg2rad(np.linspace(90, 0, cap_segs + 2))[1:-1] - else: - alpha = np.deg2rad(np.linspace(180, 90, cap_segs + 2))[1:-1] - cap_end = np.c_[ex + np.cos(alpha) * cap_radius * tx + np.sin(alpha) * cap_radius * nx, - ey + np.cos(alpha) * cap_radius * ty + np.sin(alpha) * cap_radius * ny] - - # bottom quarter (inner corner) at a1 - ex2 = mx + nx * (-half + cap_radius) - ey2 = my + ny * (-half + cap_radius) - if DEBUG: - rl.draw_circle(int(ex2), int(ey2), 2, rl.WHITE) - - if not left: - alpha2 = np.deg2rad(np.linspace(0, -90, cap_segs + 1))[:-1] # include 0 once, exclude -90 - else: - alpha2 = np.deg2rad(np.linspace(90 - 90 - 90, 0 - 90 - 90, cap_segs + 1))[:-1] - cap_end_bot = np.c_[ex2 + np.cos(alpha2) * cap_radius * tx + np.sin(alpha2) * cap_radius * nx, - ey2 + np.cos(alpha2) * cap_radius * ty + np.sin(alpha2) * cap_radius * ny] - - # append to the top quarter - if not left: - cap_end = np.vstack((cap_end, cap_end_bot)) - else: - cap_end = np.vstack((cap_end_bot, cap_end)) - - return cap_end - - if a1_deg < a0_deg: - a0_deg, a1_deg = a1_deg, a0_deg - half = thickness * 0.5 - - cap_radius = min(cap_radius, half) - - span = max(1e-3, a1_deg - a0_deg) - - # pick arc segment count from arc length, clamp to shader points[] budget - arc_len = r_mid * math.radians(span) - arc_segs = max(6, int(arc_len / px_per_seg)) - max_arc = (max_points - (4 * cap_segs + 3)) // 2 - arc_segs = max(6, min(arc_segs, max_arc)) - - # outer arc a0→a1 - ang_o = np.deg2rad(np.linspace(a0_deg, a1_deg, arc_segs + 1)) - outer = np.c_[cx + np.cos(ang_o) * (r_mid + half), - cy + np.sin(ang_o) * (r_mid + half)] - - # end cap at a1 - cap_end = get_cap(False, a1_deg) - - # inner arc a1→a0 - ang_i = np.deg2rad(np.linspace(a1_deg, a0_deg, arc_segs + 1)) - inner = np.c_[cx + np.cos(ang_i) * (r_mid - half), - cy + np.sin(ang_i) * (r_mid - half)] - - # start cap at a0 - cap_start = get_cap(True, a0_deg) - - pts = np.vstack((outer, cap_end, inner, cap_start, outer[:1])).astype(np.float32) - - # Rotate to start from middle of cap for proper triangulation - pts = np.roll(pts, cap_segs, axis=0) - - if DEBUG: - n = len(pts) - idx = int(time.monotonic() * 12) % max(1, n) # speed: 12 pts/sec - for i, (x, y) in enumerate(pts): - j = (i - idx) % n # rotate the gradient - t = j / n - color = rl.Color(255, int(255 * (1 - t)), int(255 * t), 255) - rl.draw_circle(int(x), int(y), 2, color) - - return pts - - -class TorqueBar(Widget): - def __init__(self, demo: bool = False): - super().__init__() - self._demo = demo - self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) - self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - - def update_filter(self, value: float): - """Update the torque filter value (for demo mode).""" - self._torque_filter.update(value) - - def _update_state(self): - if self._demo: - return - - # torque line - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - controls_state = ui_state.sm['controlsState'] - car_state = ui_state.sm['carState'] - live_parameters = ui_state.sm['liveParameters'] - lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY - # TODO: pull from carparams - max_lateral_acceleration = 3 - - # from selfdrived - actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 - accel_diff = (desired_lateral_accel - actual_lateral_accel) - - self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) - else: - self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) - - def _render(self, rect: rl.Rectangle) -> None: - # adjust y pos with torque - torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26]) - torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56]) - - # animate alpha and angle span - if not self._demo: - self._torque_line_alpha_filter.update(ui_state.status != UIStatus.DISENGAGED) - else: - self._torque_line_alpha_filter.update(1.0) - - torque_line_bg_alpha = np.interp(abs(self._torque_filter.x), [0.5, 1.0], [0.25, 0.5]) - torque_line_bg_color = rl.Color(255, 255, 255, int(255 * torque_line_bg_alpha * self._torque_line_alpha_filter.x)) - if ui_state.status != UIStatus.ENGAGED and not self._demo: - torque_line_bg_color = rl.Color(255, 255, 255, int(255 * 0.15 * self._torque_line_alpha_filter.x)) - - # draw curved line polygon torque bar - torque_line_radius = 1200 - top_angle = -90 - torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN - torque_start_angle = top_angle - torque_bg_angle_span / 2 - torque_end_angle = top_angle + torque_bg_angle_span / 2 - # centerline radius & center (you already have these values) - mid_r = torque_line_radius + torque_line_height / 2 - - cx = rect.x + rect.width / 2 + 8 # offset 8px to right of camera feed - cy = rect.y + rect.height + torque_line_radius - torque_line_offset - - # draw bg torque indicator line - bg_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, torque_start_angle, torque_end_angle) - draw_polygon(rect, bg_pts, color=torque_line_bg_color) - - # draw torque indicator line - a0s = top_angle - a1s = a0s + torque_bg_angle_span / 2 * self._torque_filter.x - sl_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, a0s, a1s) - - # draw beautiful gradient from center to 65% of the bg torque bar width - start_grad_pt = cx / rect.width - if self._torque_filter.x < 0: - end_grad_pt = (cx * (1 - 0.65) + (min(bg_pts[:, 0]) * 0.65)) / rect.width - else: - end_grad_pt = (cx * (1 - 0.65) + (max(bg_pts[:, 0]) * 0.65)) / rect.width - - # fade to orange as we approach max torque - start_color = blend_colors( - rl.Color(255, 255, 255, int(255 * 0.9 * self._torque_line_alpha_filter.x)), - rl.Color(255, 200, 0, int(255 * self._torque_line_alpha_filter.x)), # yellow - max(0, abs(self._torque_filter.x) - 0.75) * 4, - ) - end_color = blend_colors( - rl.Color(255, 255, 255, int(255 * 0.9 * self._torque_line_alpha_filter.x)), - rl.Color(255, 115, 0, int(255 * self._torque_line_alpha_filter.x)), # orange - max(0, abs(self._torque_filter.x) - 0.75) * 4, - ) - - if ui_state.status != UIStatus.ENGAGED and not self._demo: - start_color = end_color = rl.Color(255, 255, 255, int(255 * 0.35 * self._torque_line_alpha_filter.x)) - - gradient = Gradient( - start=(start_grad_pt, 0), - end=(end_grad_pt, 0), - colors=[ - start_color, - end_color, - ], - stops=[0.0, 1.0], - ) - - draw_polygon(rect, sl_pts, gradient=gradient) - - # draw center torque bar dot - if abs(self._torque_filter.x) < 0.5: - dot_y = self._rect.y + self._rect.height - torque_line_offset - torque_line_height / 2 - rl.draw_circle(int(cx), int(dot_y), 10 // 2, - rl.Color(182, 182, 182, int(255 * 0.9 * self._torque_line_alpha_filter.x))) diff --git a/selfdrive/ui/mici/widgets/button.py b/selfdrive/ui/mici/widgets/button.py deleted file mode 100644 index a5f7ee686ed88c..00000000000000 --- a/selfdrive/ui/mici/widgets/button.py +++ /dev/null @@ -1,338 +0,0 @@ -import pyray as rl -from typing import Union -from enum import Enum -from collections.abc import Callable -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.system.ui.widgets.scroller import DO_ZOOM -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos -from openpilot.common.filter_simple import BounceFilter - -try: - from openpilot.common.params import Params -except ImportError: - Params = None - -SCROLLING_SPEED_PX_S = 50 -COMPLICATION_SIZE = 36 -LABEL_COLOR = rl.Color(255, 255, 255, int(255 * 0.9)) -LABEL_HORIZONTAL_PADDING = 40 -COMPLICATION_GREY = rl.Color(0xAA, 0xAA, 0xAA, 255) -PRESSED_SCALE = 1.15 if DO_ZOOM else 1.07 - - -class ScrollState(Enum): - PRE_SCROLL = 0 - SCROLLING = 1 - POST_SCROLL = 2 - - -class BigCircleButton(Widget): - def __init__(self, icon: str, red: bool = False, icon_size: tuple[int, int] = (64, 53), icon_offset: tuple[int, int] = (0, 0)): - super().__init__() - self._red = red - self._icon_offset = icon_offset - - # State - self.set_rect(rl.Rectangle(0, 0, 180, 180)) - self._press_state_enabled = True - self._scale_filter = BounceFilter(1.0, 0.1, 1 / gui_app.target_fps) - - # Icons - self._txt_icon = gui_app.texture(icon, *icon_size) - self._txt_btn_disabled_bg = gui_app.texture("icons_mici/buttons/button_circle_disabled.png", 180, 180) - - self._txt_btn_bg = gui_app.texture("icons_mici/buttons/button_circle.png", 180, 180) - self._txt_btn_pressed_bg = gui_app.texture("icons_mici/buttons/button_circle_hover.png", 180, 180) - - self._txt_btn_red_bg = gui_app.texture("icons_mici/buttons/button_circle_red.png", 180, 180) - self._txt_btn_red_pressed_bg = gui_app.texture("icons_mici/buttons/button_circle_red_hover.png", 180, 180) - - def set_enable_pressed_state(self, pressed: bool): - self._press_state_enabled = pressed - - def _render(self, _): - # draw background - txt_bg = self._txt_btn_bg if not self._red else self._txt_btn_red_bg - if not self.enabled: - txt_bg = self._txt_btn_disabled_bg - elif self.is_pressed and self._press_state_enabled: - txt_bg = self._txt_btn_pressed_bg if not self._red else self._txt_btn_red_pressed_bg - - scale = self._scale_filter.update(PRESSED_SCALE if self.is_pressed and self._press_state_enabled else 1.0) - btn_x = self._rect.x + (self._rect.width * (1 - scale)) / 2 - btn_y = self._rect.y + (self._rect.height * (1 - scale)) / 2 - rl.draw_texture_ex(txt_bg, (btn_x, btn_y), 0, scale, rl.WHITE) - - # draw icon - icon_color = rl.WHITE if self.enabled else rl.Color(255, 255, 255, int(255 * 0.35)) - rl.draw_texture(self._txt_icon, int(self._rect.x + (self._rect.width - self._txt_icon.width) / 2 + self._icon_offset[0]), - int(self._rect.y + (self._rect.height - self._txt_icon.height) / 2 + self._icon_offset[1]), icon_color) - - -class BigCircleToggle(BigCircleButton): - def __init__(self, icon: str, toggle_callback: Callable | None = None, icon_size: tuple[int, int] = (64, 53), icon_offset: tuple[int, int] = (0, 0)): - super().__init__(icon, False, icon_size=icon_size, icon_offset=icon_offset) - self._toggle_callback = toggle_callback - - # State - self._checked = False - - # Icons - self._txt_toggle_enabled = gui_app.texture("icons_mici/buttons/toggle_dot_enabled.png", 66, 66) - self._txt_toggle_disabled = gui_app.texture("icons_mici/buttons/toggle_dot_disabled.png", 66, 66) - - def set_checked(self, checked: bool): - self._checked = checked - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - - self._checked = not self._checked - if self._toggle_callback: - self._toggle_callback(self._checked) - - def _render(self, _): - super()._render(_) - - # draw status icon - rl.draw_texture(self._txt_toggle_enabled if self._checked else self._txt_toggle_disabled, - int(self._rect.x + (self._rect.width - self._txt_toggle_enabled.width) / 2), - int(self._rect.y + 5), rl.WHITE) - - -class BigButton(Widget): - """A lightweight stand-in for the Qt BigButton, drawn & updated each frame.""" - - def __init__(self, text: str, value: str = "", icon: Union[str, rl.Texture] = "", icon_size: tuple[int, int] = (64, 64), - scroll: bool = False): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, 402, 180)) - self.text = text - self.value = value - self._icon_size = icon_size - self._scroll = scroll - self.set_icon(icon) - - self._scale_filter = BounceFilter(1.0, 0.1, 1 / gui_app.target_fps) - - self._rotate_icon_t: float | None = None - - self._label_font = gui_app.font(FontWeight.DISPLAY) - self._value_font = gui_app.font(FontWeight.ROMAN) - - self._label = UnifiedLabel(text, font_size=self._get_label_font_size(), font_weight=FontWeight.DISPLAY, - text_color=LABEL_COLOR, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, scroll=scroll, - line_height=0.9) - self._sub_label = UnifiedLabel(value, font_size=COMPLICATION_SIZE, font_weight=FontWeight.ROMAN, - text_color=COMPLICATION_GREY, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM) - - self._load_images() - - def set_icon(self, icon: Union[str, rl.Texture]): - self._txt_icon = gui_app.texture(icon, *self._icon_size) if isinstance(icon, str) and len(icon) else icon - - def set_rotate_icon(self, rotate: bool): - if rotate and self._rotate_icon_t is not None: - return - self._rotate_icon_t = rl.get_time() if rotate else None - - def _load_images(self): - self._txt_default_bg = gui_app.texture("icons_mici/buttons/button_rectangle.png", 402, 180) - self._txt_pressed_bg = gui_app.texture("icons_mici/buttons/button_rectangle_pressed.png", 402, 180) - self._txt_disabled_bg = gui_app.texture("icons_mici/buttons/button_rectangle_disabled.png", 402, 180) - self._txt_hover_bg = gui_app.texture("icons_mici/buttons/button_rectangle_hover.png", 402, 180) - - def _width_hint(self) -> int: - # Single line if scrolling, so hide behind icon if exists - icon_size = self._icon_size[0] if self._txt_icon and self._scroll and self.value else 0 - return int(self._rect.width - LABEL_HORIZONTAL_PADDING * 2 - icon_size) - - def _get_label_font_size(self): - if len(self.text) < 12: - font_size = 64 - elif len(self.text) < 17: - font_size = 48 - else: - font_size = 42 - - if self.value: - font_size -= 20 - - return font_size - - def set_text(self, text: str): - self.text = text - self._label.set_text(text) - - def set_value(self, value: str): - self.value = value - self._sub_label.set_text(value) - - def get_value(self) -> str: - return self.value - - def get_text(self): - return self.text - - def _render(self, _): - # draw _txt_default_bg - txt_bg = self._txt_default_bg - if not self.enabled: - txt_bg = self._txt_disabled_bg - elif self.is_pressed: - txt_bg = self._txt_hover_bg - - scale = self._scale_filter.update(PRESSED_SCALE if self.is_pressed else 1.0) - btn_x = self._rect.x + (self._rect.width * (1 - scale)) / 2 - btn_y = self._rect.y + (self._rect.height * (1 - scale)) / 2 - rl.draw_texture_ex(txt_bg, (btn_x, btn_y), 0, scale, rl.WHITE) - - # LABEL ------------------------------------------------------------------ - lx = self._rect.x + LABEL_HORIZONTAL_PADDING - ly = btn_y + self._rect.height - 33 # - 40# - self._get_label_font_size() / 2 - - if self.value: - sub_label_height = self._sub_label.get_content_height(self._width_hint()) - sub_label_rect = rl.Rectangle(lx, ly - sub_label_height, self._width_hint(), sub_label_height) - self._sub_label.render(sub_label_rect) - ly -= sub_label_height - - label_color = LABEL_COLOR if self.enabled else rl.Color(255, 255, 255, int(255 * 0.35)) - self._label.set_color(label_color) - label_height = self._label.get_content_height(self._width_hint()) - label_rect = rl.Rectangle(lx, ly - label_height, self._width_hint(), label_height) - self._label.render(label_rect) - - # ICON ------------------------------------------------------------------- - if self._txt_icon: - rotation = 0 - if self._rotate_icon_t is not None: - rotation = (rl.get_time() - self._rotate_icon_t) * 180 - - # drop top right with 30px padding - x = self._rect.x + self._rect.width - 30 - self._txt_icon.width / 2 - y = self._rect.y + 30 + self._txt_icon.height / 2 - source_rec = rl.Rectangle(0, 0, self._txt_icon.width, self._txt_icon.height) - dest_rec = rl.Rectangle(int(x), int(y), self._txt_icon.width, self._txt_icon.height) - origin = rl.Vector2(self._txt_icon.width / 2, self._txt_icon.height / 2) - rl.draw_texture_pro(self._txt_icon, source_rec, dest_rec, origin, rotation, rl.WHITE) - - -class BigToggle(BigButton): - def __init__(self, text: str, value: str = "", initial_state: bool = False, toggle_callback: Callable | None = None): - super().__init__(text, value, "") - self._checked = initial_state - self._toggle_callback = toggle_callback - - def _load_images(self): - super()._load_images() - self._txt_enabled_toggle = gui_app.texture("icons_mici/buttons/toggle_pill_enabled.png", 84, 66) - self._txt_disabled_toggle = gui_app.texture("icons_mici/buttons/toggle_pill_disabled.png", 84, 66) - - def set_checked(self, checked: bool): - self._checked = checked - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - self._checked = not self._checked - if self._toggle_callback: - self._toggle_callback(self._checked) - - def _draw_pill(self, x: float, y: float, checked: bool): - # draw toggle icon top right - if checked: - rl.draw_texture(self._txt_enabled_toggle, int(x), int(y), rl.WHITE) - else: - rl.draw_texture(self._txt_disabled_toggle, int(x), int(y), rl.WHITE) - - def _render(self, _): - super()._render(_) - - x = self._rect.x + self._rect.width - self._txt_enabled_toggle.width - y = self._rect.y - self._draw_pill(x, y, self._checked) - - -class BigMultiToggle(BigToggle): - def __init__(self, text: str, options: list[str], toggle_callback: Callable | None = None, - select_callback: Callable | None = None): - super().__init__(text, "", toggle_callback=toggle_callback) - assert len(options) > 0 - self._options = options - self._select_callback = select_callback - - self.set_value(self._options[0]) - - def _width_hint(self) -> int: - return int(self._rect.width - LABEL_HORIZONTAL_PADDING * 2 - self._txt_enabled_toggle.width) - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - cur_idx = self._options.index(self.value) - new_idx = (cur_idx + 1) % len(self._options) - self.set_value(self._options[new_idx]) - if self._select_callback: - self._select_callback(self.value) - - def _render(self, _): - BigButton._render(self, _) - - checked_idx = self._options.index(self.value) - - x = self._rect.x + self._rect.width - self._txt_enabled_toggle.width - y = self._rect.y - - for i in range(len(self._options)): - self._draw_pill(x, y, checked_idx == i) - y += 35 - - -class BigMultiParamToggle(BigMultiToggle): - def __init__(self, text: str, param: str, options: list[str], toggle_callback: Callable | None = None, - select_callback: Callable | None = None): - super().__init__(text, options, toggle_callback, select_callback) - self._param = param - - self._params = Params() - self._load_value() - - def _load_value(self): - self.set_value(self._options[self._params.get(self._param) or 0]) - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - new_idx = self._options.index(self.value) - self._params.put_nonblocking(self._param, new_idx) - - -class BigParamControl(BigToggle): - def __init__(self, text: str, param: str, toggle_callback: Callable | None = None): - super().__init__(text, "", toggle_callback=toggle_callback) - self.param = param - self.params = Params() - self.set_checked(self.params.get_bool(self.param, False)) - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - self.params.put_bool(self.param, self._checked) - - def refresh(self): - self.set_checked(self.params.get_bool(self.param, False)) - - -# TODO: param control base class -class BigCircleParamControl(BigCircleToggle): - def __init__(self, icon: str, param: str, toggle_callback: Callable | None = None, icon_size: tuple[int, int] = (64, 53), - icon_offset: tuple[int, int] = (0, 0)): - super().__init__(icon, toggle_callback, icon_size=icon_size, icon_offset=icon_offset) - self._param = param - self.params = Params() - self.set_checked(self.params.get_bool(self._param, False)) - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - self.params.put_bool(self._param, self._checked) - - def refresh(self): - self.set_checked(self.params.get_bool(self._param, False)) diff --git a/selfdrive/ui/mici/widgets/dialog.py b/selfdrive/ui/mici/widgets/dialog.py deleted file mode 100644 index 49d73f7b0d44b7..00000000000000 --- a/selfdrive/ui/mici/widgets/dialog.py +++ /dev/null @@ -1,425 +0,0 @@ -import abc -import math -import pyray as rl -from typing import Union -from collections.abc import Callable -from typing import cast -from openpilot.system.ui.widgets import Widget, NavWidget, DialogResult -from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label -from openpilot.system.ui.widgets.mici_keyboard import MiciKeyboard -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, MouseEvent -from openpilot.system.ui.widgets.scroller import Scroller -from openpilot.system.ui.widgets.slider import RedBigSlider, BigSlider -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.ui.mici.widgets.button import BigButton -from openpilot.selfdrive.ui.mici.widgets.side_button import SideButton - -DEBUG = False - -PADDING = 20 - - -class BigDialogBase(NavWidget, abc.ABC): - def __init__(self, right_btn: str | None = None, right_btn_callback: Callable | None = None): - super().__init__() - self._ret = DialogResult.NO_ACTION - self.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - self.set_back_callback(lambda: setattr(self, '_ret', DialogResult.CANCEL)) - - self._right_btn = None - if right_btn: - def right_btn_callback_wrapper(): - gui_app.set_modal_overlay(None) - if right_btn_callback: - right_btn_callback() - - self._right_btn = SideButton(right_btn) - self._right_btn.set_click_callback(right_btn_callback_wrapper) - # move to right side - self._right_btn._rect.x = self._rect.x + self._rect.width - self._right_btn._rect.width - - def _render(self, _) -> DialogResult: - """ - Allows `gui_app.set_modal_overlay(BigDialog(...))`. - The overlay runner keeps calling until result != NO_ACTION. - """ - if self._right_btn: - self._right_btn.set_position(self._right_btn._rect.x, self._rect.y) - self._right_btn.render() - - return self._ret - - -class BigDialog(BigDialogBase): - def __init__(self, - title: str, - description: str, - right_btn: str | None = None, - right_btn_callback: Callable | None = None): - super().__init__(right_btn, right_btn_callback) - self._title = title - self._description = description - - def _render(self, _) -> DialogResult: - super()._render(_) - - # draw title - # TODO: we desperately need layouts - # TODO: coming up with these numbers manually is a pain and not scalable - # TODO: no clue what any of these numbers mean. VBox and HBox would remove all of this shite - max_width = self._rect.width - PADDING * 2 - if self._right_btn: - max_width -= self._right_btn._rect.width - - title_wrapped = '\n'.join(wrap_text(gui_app.font(FontWeight.BOLD), self._title, 50, int(max_width))) - title_size = measure_text_cached(gui_app.font(FontWeight.BOLD), title_wrapped, 50) - text_x_offset = 0 - title_rect = rl.Rectangle(int(self._rect.x + text_x_offset + PADDING), - int(self._rect.y + PADDING), - int(max_width), - int(title_size.y)) - gui_label(title_rect, title_wrapped, 50, font_weight=FontWeight.BOLD, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - - # draw description - desc_wrapped = '\n'.join(wrap_text(gui_app.font(FontWeight.MEDIUM), self._description, 30, int(max_width))) - desc_size = measure_text_cached(gui_app.font(FontWeight.MEDIUM), desc_wrapped, 30) - desc_rect = rl.Rectangle(int(self._rect.x + text_x_offset + PADDING), - int(self._rect.y + self._rect.height / 3), - int(max_width), - int(desc_size.y)) - # TODO: text align doesn't seem to work properly with newlines - gui_label(desc_rect, desc_wrapped, 30, font_weight=FontWeight.MEDIUM, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - - return self._ret - - -class BigConfirmationDialogV2(BigDialogBase): - def __init__(self, title: str, icon: str, red: bool = False, - exit_on_confirm: bool = True, - confirm_callback: Callable | None = None): - super().__init__() - self._confirm_callback = confirm_callback - self._exit_on_confirm = exit_on_confirm - - icon_txt = gui_app.texture(icon, 64, 53) - self._slider: BigSlider | RedBigSlider - if red: - self._slider = RedBigSlider(title, icon_txt, confirm_callback=self._on_confirm) - else: - self._slider = BigSlider(title, icon_txt, confirm_callback=self._on_confirm) - self._slider.set_enabled(lambda: not self._swiping_away) - - def _on_confirm(self): - if self._confirm_callback: - self._confirm_callback() - if self._exit_on_confirm: - self._ret = DialogResult.CONFIRM - - def _update_state(self): - super()._update_state() - if self._swiping_away and not self._slider.confirmed: - self._slider.reset() - - def _render(self, _) -> DialogResult: - self._slider.render(self._rect) - return self._ret - - -class BigInputDialog(BigDialogBase): - BACK_TOUCH_AREA_PERCENTAGE = 0.2 - BACKSPACE_RATE = 25 # hz - TEXT_INPUT_SIZE = 35 - - def __init__(self, - hint: str, - default_text: str = "", - minimum_length: int = 1, - confirm_callback: Callable[[str], None] | None = None): - super().__init__(None, None) - self._hint_label = UnifiedLabel(hint, font_size=35, text_color=rl.Color(255, 255, 255, int(255 * 0.35)), - font_weight=FontWeight.MEDIUM) - self._keyboard = MiciKeyboard() - self._keyboard.set_text(default_text) - self._minimum_length = minimum_length - - self._backspace_held_time: float | None = None - - self._backspace_img = gui_app.texture("icons_mici/settings/keyboard/backspace.png", 42, 36) - self._backspace_img_alpha = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - - self._enter_img = gui_app.texture("icons_mici/settings/keyboard/enter.png", 76, 62) - self._enter_disabled_img = gui_app.texture("icons_mici/settings/keyboard/enter_disabled.png", 76, 62) - self._enter_img_alpha = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - - # rects for top buttons - self._top_left_button_rect = rl.Rectangle(0, 0, 0, 0) - self._top_right_button_rect = rl.Rectangle(0, 0, 0, 0) - - def confirm_callback_wrapper(): - self._ret = DialogResult.CONFIRM - if confirm_callback: - confirm_callback(self._keyboard.text()) - self._confirm_callback = confirm_callback_wrapper - - def _update_state(self): - super()._update_state() - - last_mouse_event = gui_app.last_mouse_event - if last_mouse_event.left_down and rl.check_collision_point_rec(last_mouse_event.pos, self._top_right_button_rect) and self._backspace_img_alpha.x > 1: - if self._backspace_held_time is None: - self._backspace_held_time = rl.get_time() - - if rl.get_time() - self._backspace_held_time > 0.5: - if gui_app.frame % round(gui_app.target_fps / self.BACKSPACE_RATE) == 0: - self._keyboard.backspace() - - else: - self._backspace_held_time = None - - def _render(self, _): - # draw current text so far below everything. text floats left but always stays in view - text = self._keyboard.text() - candidate_char = self._keyboard.get_candidate_character() - text_size = measure_text_cached(gui_app.font(FontWeight.ROMAN), text + candidate_char or self._hint_label.text, self.TEXT_INPUT_SIZE) - - bg_block_margin = 5 - text_x = PADDING / 2 + self._enter_img.width + PADDING - text_field_rect = rl.Rectangle(text_x, int(self._rect.y + PADDING) - bg_block_margin, - int(self._rect.width - text_x * 2), - int(text_size.y)) - - # draw text input - # push text left with a gradient on left side if too long - if text_size.x > text_field_rect.width: - text_x -= text_size.x - text_field_rect.width - - rl.begin_scissor_mode(int(text_field_rect.x), int(text_field_rect.y), int(text_field_rect.width), int(text_field_rect.height)) - rl.draw_text_ex(gui_app.font(FontWeight.ROMAN), text, rl.Vector2(text_x, text_field_rect.y), self.TEXT_INPUT_SIZE, 0, rl.WHITE) - - # draw grayed out character user is hovering over - if candidate_char: - candidate_char_size = measure_text_cached(gui_app.font(FontWeight.ROMAN), candidate_char, self.TEXT_INPUT_SIZE) - rl.draw_text_ex(gui_app.font(FontWeight.ROMAN), candidate_char, - rl.Vector2(min(text_x + text_size.x, text_field_rect.x + text_field_rect.width) - candidate_char_size.x, text_field_rect.y), - self.TEXT_INPUT_SIZE, 0, rl.Color(255, 255, 255, 128)) - - rl.end_scissor_mode() - - # draw gradient on left side to indicate more text - if text_size.x > text_field_rect.width: - rl.draw_rectangle_gradient_h(int(text_field_rect.x), int(text_field_rect.y), 80, int(text_field_rect.height), - rl.BLACK, rl.BLANK) - - # draw cursor - if text: - blink_alpha = (math.sin(rl.get_time() * 6) + 1) / 2 - cursor_x = min(text_x + text_size.x + 3, text_field_rect.x + text_field_rect.width) - rl.draw_rectangle_rounded(rl.Rectangle(int(cursor_x), int(text_field_rect.y), 4, int(text_size.y)), - 1, 4, rl.Color(255, 255, 255, int(255 * blink_alpha))) - - # draw backspace icon with nice fade - self._backspace_img_alpha.update(255 * bool(text)) - if self._backspace_img_alpha.x > 1: - color = rl.Color(255, 255, 255, int(self._backspace_img_alpha.x)) - rl.draw_texture(self._backspace_img, int(self._rect.width - self._backspace_img.width - 27), int(self._rect.y + 14), color) - - if not text and self._hint_label.text and not candidate_char: - # draw description if no text entered yet and not drawing candidate char - self._hint_label.render(text_field_rect) - - # TODO: move to update state - # make rect take up entire area so it's easier to click - self._top_left_button_rect = rl.Rectangle(self._rect.x, self._rect.y, text_field_rect.x, self._rect.height - self._keyboard.get_keyboard_height()) - self._top_right_button_rect = rl.Rectangle(text_field_rect.x + text_field_rect.width, self._rect.y, - self._rect.width - (text_field_rect.x + text_field_rect.width), self._top_left_button_rect.height) - - # draw enter button - self._enter_img_alpha.update(255 if len(text) >= self._minimum_length else 0) - color = rl.Color(255, 255, 255, int(self._enter_img_alpha.x)) - rl.draw_texture(self._enter_img, int(self._rect.x + PADDING / 2), int(self._rect.y), color) - color = rl.Color(255, 255, 255, 255 - int(self._enter_img_alpha.x)) - rl.draw_texture(self._enter_disabled_img, int(self._rect.x + PADDING / 2), int(self._rect.y), color) - - # keyboard goes over everything - self._keyboard.render(self._rect) - - # draw debugging rect bounds - if DEBUG: - rl.draw_rectangle_lines_ex(text_field_rect, 1, rl.Color(100, 100, 100, 255)) - rl.draw_rectangle_lines_ex(self._top_right_button_rect, 1, rl.Color(0, 255, 0, 255)) - rl.draw_rectangle_lines_ex(self._top_left_button_rect, 1, rl.Color(0, 255, 0, 255)) - - return self._ret - - def _handle_mouse_press(self, mouse_pos: MousePos): - super()._handle_mouse_press(mouse_pos) - # TODO: need to track where press was so enter and back can activate on release rather than press - # or turn into icon widgets :eyes_open: - # handle backspace icon click - if rl.check_collision_point_rec(mouse_pos, self._top_right_button_rect) and self._backspace_img_alpha.x > 254: - self._keyboard.backspace() - elif rl.check_collision_point_rec(mouse_pos, self._top_left_button_rect) and self._enter_img_alpha.x > 254: - # handle enter icon click - self._confirm_callback() - - -class BigDialogOptionButton(Widget): - HEIGHT = 64 - SELECTED_HEIGHT = 74 - - def __init__(self, option: str): - super().__init__() - self.option = option - self.set_rect(rl.Rectangle(0, 0, int(gui_app.width / 2 + 220), self.HEIGHT)) - - self._selected = False - - self._label = UnifiedLabel(option, font_size=70, text_color=rl.Color(255, 255, 255, int(255 * 0.58)), - font_weight=FontWeight.DISPLAY_REGULAR, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, - scroll=True) - - def show_event(self): - super().show_event() - self._label.reset_scroll() - - def set_selected(self, selected: bool): - self._selected = selected - self._rect.height = self.SELECTED_HEIGHT if selected else self.HEIGHT - - def _render(self, _): - if DEBUG: - rl.draw_rectangle_lines_ex(self._rect, 1, rl.Color(0, 255, 0, 255)) - - # FIXME: offset x by -45 because scroller centers horizontally - if self._selected: - self._label.set_font_size(self.SELECTED_HEIGHT) - self._label.set_color(rl.Color(255, 255, 255, int(255 * 0.9))) - self._label.set_font_weight(FontWeight.DISPLAY) - else: - self._label.set_font_size(self.HEIGHT) - self._label.set_color(rl.Color(255, 255, 255, int(255 * 0.58))) - self._label.set_font_weight(FontWeight.DISPLAY_REGULAR) - - self._label.render(self._rect) - - -class BigMultiOptionDialog(BigDialogBase): - BACK_TOUCH_AREA_PERCENTAGE = 0.1 - - def __init__(self, options: list[str], default: str | None, - right_btn: str | None = 'check', right_btn_callback: Callable[[], None] | None = None): - super().__init__(right_btn, right_btn_callback=right_btn_callback) - self._options = options - if default is not None: - assert default in options - - self._default_option: str | None = default - self._selected_option: str = self._default_option or (options[0] if len(options) > 0 else "") - self._last_selected_option: str = self._selected_option - - # Widget doesn't differentiate between click and drag - self._can_click = True - - self._scroller = Scroller([], horizontal=False, pad_start=100, pad_end=100, spacing=0, snap_items=True) - if self._right_btn is not None: - self._scroller.set_enabled(lambda: not cast(Widget, self._right_btn).is_pressed) - - for option in options: - self._scroller.add_widget(BigDialogOptionButton(option)) - - def show_event(self): - super().show_event() - self._scroller.show_event() - if self._default_option is not None: - self._on_option_selected(self._default_option) - - def get_selected_option(self) -> str: - return self._selected_option - - def _on_option_selected(self, option: str): - y_pos = 0.0 - for btn in self._scroller._items: - btn = cast(BigDialogOptionButton, btn) - if btn.option == option: - rect_center_y = self._rect.y + self._rect.height / 2 - if btn._selected: - height = btn.rect.height - else: - # when selecting an option under current, account for changing heights - btn_center_y = btn.rect.y + btn.rect.height / 2 # not accurate, just to determine direction - height_offset = BigDialogOptionButton.SELECTED_HEIGHT - BigDialogOptionButton.HEIGHT - height = (BigDialogOptionButton.HEIGHT - height_offset) if rect_center_y < btn_center_y else BigDialogOptionButton.SELECTED_HEIGHT - y_pos = rect_center_y - (btn.rect.y + height / 2) - break - - self._scroller.scroll_to(-y_pos) - - def _selected_option_changed(self): - pass - - def _handle_mouse_press(self, mouse_pos: MousePos): - super()._handle_mouse_press(mouse_pos) - self._can_click = True - - def _handle_mouse_event(self, mouse_event: MouseEvent) -> None: - super()._handle_mouse_event(mouse_event) - - # # TODO: add generic _handle_mouse_click handler to Widget - if not self._scroller.scroll_panel.is_touch_valid(): - self._can_click = False - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - - if not self._can_click: - return - - # select current option - for btn in self._scroller._items: - btn = cast(BigDialogOptionButton, btn) - if btn.option == self._selected_option: - self._on_option_selected(btn.option) - break - - def _update_state(self): - super()._update_state() - - # get selection by whichever button is closest to center - center_y = self._rect.y + self._rect.height / 2 - closest_btn = (None, float('inf')) - for btn in self._scroller._items: - dist_y = abs((btn.rect.y + btn.rect.height / 2) - center_y) - if dist_y < closest_btn[1]: - closest_btn = (btn, dist_y) - - if closest_btn[0]: - for btn in self._scroller._items: - btn.set_selected(btn.option == closest_btn[0].option) - self._selected_option = closest_btn[0].option - - # Signal to subclasses if selection changed - if self._selected_option != self._last_selected_option: - self._selected_option_changed() - self._last_selected_option = self._selected_option - - def _render(self, _): - super()._render(_) - self._scroller.render(self._rect) - - return self._ret - - -class BigDialogButton(BigButton): - def __init__(self, text: str, value: str = "", icon: Union[str, rl.Texture] = "", description: str = ""): - super().__init__(text, value, icon) - self._description = description - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - - dlg = BigDialog(self.text, self._description) - gui_app.set_modal_overlay(dlg) diff --git a/selfdrive/ui/mici/widgets/pairing_dialog.py b/selfdrive/ui/mici/widgets/pairing_dialog.py deleted file mode 100644 index 88bab2d00112bd..00000000000000 --- a/selfdrive/ui/mici/widgets/pairing_dialog.py +++ /dev/null @@ -1,116 +0,0 @@ -import pyray as rl -import qrcode -import numpy as np -import time - -from openpilot.common.api import Api -from openpilot.common.swaglog import cloudlog -from openpilot.common.params import Params -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.widgets import NavWidget -from openpilot.system.ui.lib.application import FontWeight, gui_app -from openpilot.system.ui.widgets.label import MiciLabel - - -class PairingDialog(NavWidget): - """Dialog for device pairing with QR code.""" - - QR_REFRESH_INTERVAL = 300 # 5 minutes in seconds - - def __init__(self): - super().__init__() - self.set_back_callback(lambda: gui_app.set_modal_overlay(None)) - self._params = Params() - self._qr_texture: rl.Texture | None = None - self._last_qr_generation = float("-inf") - - self._txt_pair = gui_app.texture("icons_mici/settings/device/pair.png", 33, 60) - self._pair_label = MiciLabel("pair with comma connect", 48, font_weight=FontWeight.BOLD, - color=rl.Color(255, 255, 255, int(255 * 0.9)), line_height=40, wrap_text=True) - - def _get_pairing_url(self) -> str: - try: - dongle_id = self._params.get("DongleId") or "" - token = Api(dongle_id).get_token({'pair': True}) - except Exception as e: - cloudlog.warning(f"Failed to get pairing token: {e}") - token = "" - return f"https://connect.comma.ai/?pair={token}" - - def _generate_qr_code(self) -> None: - try: - qr = qrcode.QRCode(version=1, error_correction=qrcode.constants.ERROR_CORRECT_L, box_size=10, border=0) - qr.add_data(self._get_pairing_url()) - qr.make(fit=True) - - pil_img = qr.make_image(fill_color="white", back_color="black").convert('RGBA') - img_array = np.array(pil_img, dtype=np.uint8) - - if self._qr_texture and self._qr_texture.id != 0: - rl.unload_texture(self._qr_texture) - - rl_image = rl.Image() - rl_image.data = rl.ffi.cast("void *", img_array.ctypes.data) - rl_image.width = pil_img.width - rl_image.height = pil_img.height - rl_image.mipmaps = 1 - rl_image.format = rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 - - self._qr_texture = rl.load_texture_from_image(rl_image) - except Exception as e: - cloudlog.warning(f"QR code generation failed: {e}") - self._qr_texture = None - - def _check_qr_refresh(self) -> None: - current_time = time.monotonic() - if current_time - self._last_qr_generation >= self.QR_REFRESH_INTERVAL: - self._generate_qr_code() - self._last_qr_generation = current_time - - def _update_state(self): - super()._update_state() - if ui_state.prime_state.is_paired(): - self._playing_dismiss_animation = True - - def _render(self, rect: rl.Rectangle) -> int: - self._check_qr_refresh() - - self._render_qr_code() - - label_x = self._rect.x + 8 + self._rect.height + 24 - self._pair_label.set_width(int(self._rect.width - label_x)) - self._pair_label.set_position(label_x, self._rect.y + 16) - self._pair_label.render() - - rl.draw_texture_ex(self._txt_pair, rl.Vector2(label_x, self._rect.y + self._rect.height - self._txt_pair.height - 16), - 0.0, 1.0, rl.Color(255, 255, 255, int(255 * 0.35))) - - return -1 - - def _render_qr_code(self) -> None: - if not self._qr_texture: - error_font = gui_app.font(FontWeight.BOLD) - rl.draw_text_ex( - error_font, "QR Code Error", rl.Vector2(self._rect.x + 20, self._rect.y + self._rect.height // 2 - 15), 30, 0.0, rl.RED - ) - return - - scale = self._rect.height / self._qr_texture.height - pos = rl.Vector2(self._rect.x + 8, self._rect.y) - rl.draw_texture_ex(self._qr_texture, pos, 0.0, scale, rl.WHITE) - - def __del__(self): - if self._qr_texture and self._qr_texture.id != 0: - rl.unload_texture(self._qr_texture) - - -if __name__ == "__main__": - gui_app.init_window("pairing device") - pairing = PairingDialog() - try: - for _ in gui_app.render(): - result = pairing.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - if result != -1: - break - finally: - del pairing diff --git a/selfdrive/ui/mici/widgets/side_button.py b/selfdrive/ui/mici/widgets/side_button.py deleted file mode 100644 index 4803b6d208c931..00000000000000 --- a/selfdrive/ui/mici/widgets/side_button.py +++ /dev/null @@ -1,31 +0,0 @@ -import pyray as rl -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.lib.application import gui_app - -# --------------------------------------------------------------------------- -# Constants extracted from the original Qt style -# --------------------------------------------------------------------------- -# TODO: this should be corrected, but Scroller relies on this being incorrect :/ -WIDTH, HEIGHT = 112, 240 - - -class SideButton(Widget): - def __init__(self, btn_type: str): - super().__init__() - self.type = btn_type - self.set_rect(rl.Rectangle(0, 0, WIDTH, HEIGHT)) - - # load pre-rendered button images - if btn_type not in ("check", "back"): - btn_type = "back" - btn_img_path = f"icons_mici/buttons/button_side_{btn_type}.png" - btn_img_pressed_path = f"icons_mici/buttons/button_side_{btn_type}_pressed.png" - self._txt_btn, self._txt_btn_back = gui_app.texture(btn_img_path, 100, 224), gui_app.texture(btn_img_pressed_path, 100, 224) - - def _render(self, _) -> bool: - x = int(self._rect.x + 12) - y = int(self._rect.y + (self._rect.height - self._txt_btn.height) / 2) - rl.draw_texture(self._txt_btn if not self.is_pressed else self._txt_btn_back, - x, y, rl.WHITE) - - return False diff --git a/selfdrive/ui/mui.cc b/selfdrive/ui/mui.cc new file mode 100644 index 00000000000000..55b9a474742076 --- /dev/null +++ b/selfdrive/ui/mui.cc @@ -0,0 +1,138 @@ +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/qt_window.h" + +class StatusBar : public QGraphicsRectItem { + private: + QLinearGradient linear_gradient; + QRadialGradient radial_gradient; + QTimer animation_timer; + const int animation_length = 10; + int animation_index = 0; + + public: + StatusBar(double x, double y, double width, double height) : QGraphicsRectItem {x, y, width, height} { + linear_gradient = QLinearGradient(0, 0, 0, height/2); + linear_gradient.setSpread(QGradient::ReflectSpread); + + radial_gradient = QRadialGradient(width/2, height/2, width/8); + QObject::connect(&animation_timer, &QTimer::timeout, [=]() { + animation_index++; + animation_index %= animation_length; + }); + animation_timer.start(50); + } + + void solidColor(QColor color) { + QColor dark_color = QColor(color); + dark_color.setAlphaF(0.5); + + linear_gradient.setColorAt(0, dark_color); + linear_gradient.setColorAt(1, color); + setBrush(QBrush(linear_gradient)); + } + + // these need to be called continuously for the animations to work. + // can probably clean that up with some more abstractions + void blinkingColor(QColor color) { + QColor dark_color = QColor(color); + dark_color.setAlphaF(0.1); + + int radius = (rect().width() / animation_length) * animation_index; + QPoint center = QPoint(rect().width()/2, rect().height()/2); + radial_gradient.setCenter(center); + radial_gradient.setFocalPoint(center); + radial_gradient.setRadius(radius); + + radial_gradient.setColorAt(1, dark_color); + radial_gradient.setColorAt(0, color); + setBrush(QBrush(radial_gradient)); + } + + void laneChange(cereal::LateralPlan::LaneChangeDirection direction) { + QColor dark_color = QColor(bg_colors[STATUS_ENGAGED]); + dark_color.setAlphaF(0.1); + + int x = (rect().width() / animation_length) * animation_index; + QPoint center = QPoint(((direction == cereal::LateralPlan::LaneChangeDirection::RIGHT) ? x : (rect().width() - x)), rect().height()/2); + radial_gradient.setCenter(center); + radial_gradient.setFocalPoint(center); + radial_gradient.setRadius(rect().width()/5); + + radial_gradient.setColorAt(1, dark_color); + radial_gradient.setColorAt(0, bg_colors[STATUS_ENGAGED]); + setBrush(QBrush(radial_gradient)); + } + + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget = nullptr) override { + painter->setPen(QPen()); + painter->setBrush(brush()); + + double rounding_radius = rect().height()/2; + painter->drawRoundedRect(rect(), rounding_radius, rounding_radius); + } +}; + +int main(int argc, char *argv[]) { + QApplication a(argc, argv); + QWidget w; + setMainWindow(&w); + + w.setStyleSheet("background-color: black;"); + + // our beautiful UI + QVBoxLayout *layout = new QVBoxLayout(&w); + + QGraphicsScene *scene = new QGraphicsScene(); + StatusBar *status_bar = new StatusBar(0, 0, 1000, 50); + scene->addItem(status_bar); + + QGraphicsView *graphics_view = new QGraphicsView(scene); + layout->insertSpacing(0, 400); + layout->addWidget(graphics_view, 0, Qt::AlignCenter); + + QTimer timer; + QObject::connect(&timer, &QTimer::timeout, [=]() { + static SubMaster sm({"deviceState", "controlsState", "lateralPlan"}); + + bool onroad_prev = sm.allAliveAndValid({"deviceState"}) && + sm["deviceState"].getDeviceState().getStarted(); + sm.update(0); + + bool onroad = sm.allAliveAndValid({"deviceState"}) && + sm["deviceState"].getDeviceState().getStarted(); + + if (onroad) { + auto cs = sm["controlsState"].getControlsState(); + UIStatus status = cs.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; + if (cs.getAlertStatus() == cereal::ControlsState::AlertStatus::USER_PROMPT) { + status = STATUS_WARNING; + } else if (cs.getAlertStatus() == cereal::ControlsState::AlertStatus::CRITICAL) { + status = STATUS_ALERT; + } + + auto lp = sm["lateralPlan"].getLateralPlan(); + if (lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::PRE_LANE_CHANGE || status == STATUS_ALERT) { + status_bar->blinkingColor(bg_colors[status]); + } else if (lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::LANE_CHANGE_STARTING || + lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::LANE_CHANGE_FINISHING) { + status_bar->laneChange(lp.getLaneChangeDirection()); + } else { + status_bar->solidColor(bg_colors[status]); + } + } + + if ((onroad != onroad_prev) || sm.frame < 2) { + Hardware::set_brightness(50); + Hardware::set_display_power(onroad); + } + }); + timer.start(50); + + return a.exec(); +} diff --git a/selfdrive/ui/onroad/alert_renderer.py b/selfdrive/ui/onroad/alert_renderer.py deleted file mode 100644 index a81fbfc440b0df..00000000000000 --- a/selfdrive/ui/onroad/alert_renderer.py +++ /dev/null @@ -1,178 +0,0 @@ -import time -import pyray as rl -from dataclasses import dataclass -from cereal import messaging, log -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import Label - -AlertSize = log.SelfdriveState.AlertSize -AlertStatus = log.SelfdriveState.AlertStatus - -ALERT_MARGIN = 40 -ALERT_PADDING = 60 -ALERT_LINE_SPACING = 45 -ALERT_BORDER_RADIUS = 30 - -ALERT_FONT_SMALL = 66 -ALERT_FONT_MEDIUM = 74 -ALERT_FONT_BIG = 88 - -ALERT_HEIGHTS = { - AlertSize.small: 271, - AlertSize.mid: 420, -} - -SELFDRIVE_STATE_TIMEOUT = 5 # Seconds -SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds - -# Constants -ALERT_COLORS = { - AlertStatus.normal: rl.Color(0x15, 0x15, 0x15, 0xF1), # #151515 with alpha 0xF1 - AlertStatus.userPrompt: rl.Color(0xDA, 0x6F, 0x25, 0xF1), # #DA6F25 with alpha 0xF1 - AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 0xF1), # #C92231 with alpha 0xF1 -} - - -@dataclass -class Alert: - text1: str = "" - text2: str = "" - size: int = 0 - status: int = 0 - - -# Pre-defined alert instances -ALERT_STARTUP_PENDING = Alert( - text1=tr("openpilot Unavailable"), - text2=tr("Waiting to start"), - size=AlertSize.mid, - status=AlertStatus.normal, -) - -ALERT_CRITICAL_TIMEOUT = Alert( - text1=tr("TAKE CONTROL IMMEDIATELY"), - text2=tr("System Unresponsive"), - size=AlertSize.full, - status=AlertStatus.critical, -) - -ALERT_CRITICAL_REBOOT = Alert( - text1=tr("System Unresponsive"), - text2=tr("Reboot Device"), - size=AlertSize.mid, - status=AlertStatus.normal, -) - - -class AlertRenderer(Widget): - def __init__(self): - super().__init__() - self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL) - self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD) - - # font size is set dynamically - self._full_text1_label = Label("", font_size=0, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP) - self._full_text2_label = Label("", font_size=ALERT_FONT_BIG, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP) - - def get_alert(self, sm: messaging.SubMaster) -> Alert | None: - """Generate the current alert based on selfdrive state.""" - ss = sm['selfdriveState'] - - # Check if selfdriveState messages have stopped arriving - recv_frame = sm.recv_frame['selfdriveState'] - if not sm.updated['selfdriveState']: - time_since_onroad = time.monotonic() - ui_state.started_time - - # 1. Never received selfdriveState since going onroad - waiting_for_startup = recv_frame < ui_state.started_frame - if waiting_for_startup and time_since_onroad > 5: - return ALERT_STARTUP_PENDING - - # 2. Lost communication with selfdriveState after receiving it - if TICI and not waiting_for_startup: - ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] - if ss_missing > SELFDRIVE_STATE_TIMEOUT: - if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT: - return ALERT_CRITICAL_TIMEOUT - return ALERT_CRITICAL_REBOOT - - # No alert if size is none - if ss.alertSize == 0: - return None - - # Don't get old alert - if recv_frame < ui_state.started_frame: - return None - - # Return current alert - return Alert(text1=ss.alertText1, text2=ss.alertText2, size=ss.alertSize.raw, status=ss.alertStatus.raw) - - def _render(self, rect: rl.Rectangle): - alert = self.get_alert(ui_state.sm) - if not alert: - return - - alert_rect = self._get_alert_rect(rect, alert.size) - self._draw_background(alert_rect, alert) - - text_rect = rl.Rectangle( - alert_rect.x + ALERT_PADDING, - alert_rect.y + ALERT_PADDING, - alert_rect.width - 2 * ALERT_PADDING, - alert_rect.height - 2 * ALERT_PADDING - ) - self._draw_text(text_rect, alert) - - def _get_alert_rect(self, rect: rl.Rectangle, size: int) -> rl.Rectangle: - if size == AlertSize.full: - return rect - - h = ALERT_HEIGHTS.get(size, rect.height) - return rl.Rectangle(rect.x + ALERT_MARGIN, rect.y + rect.height - h + ALERT_MARGIN, - rect.width - ALERT_MARGIN * 2, h - ALERT_MARGIN * 2) - - def _draw_background(self, rect: rl.Rectangle, alert: Alert) -> None: - color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal]) - - if alert.size != AlertSize.full: - roundness = ALERT_BORDER_RADIUS / (min(rect.width, rect.height) / 2) - rl.draw_rectangle_rounded(rect, roundness, 10, color) - else: - rl.draw_rectangle_rec(rect, color) - - def _draw_text(self, rect: rl.Rectangle, alert: Alert) -> None: - if alert.size == AlertSize.small: - self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_MEDIUM) - - elif alert.size == AlertSize.mid: - self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_BIG, center_y=False) - rect.y += ALERT_FONT_BIG + ALERT_LINE_SPACING - self._draw_centered(alert.text2, rect, self.font_regular, ALERT_FONT_SMALL, center_y=False) - - else: - is_long = len(alert.text1) > 15 - font_size1 = 132 if is_long else 177 - - top_offset = 200 if is_long or '\n' in alert.text1 else 270 - title_rect = rl.Rectangle(rect.x, rect.y + top_offset, rect.width, 600) - self._full_text1_label.set_font_size(font_size1) - self._full_text1_label.set_text(alert.text1) - self._full_text1_label.render(title_rect) - - bottom_offset = 361 if is_long else 420 - subtitle_rect = rl.Rectangle(rect.x, rect.y + rect.height - bottom_offset, rect.width, 300) - self._full_text2_label.set_text(alert.text2) - self._full_text2_label.render(subtitle_rect) - - def _draw_centered(self, text, rect, font, font_size, center_y=True, color=rl.WHITE) -> None: - text_size = measure_text_cached(font, text, font_size) - x = rect.x + (rect.width - text_size.x) / 2 - y = rect.y + ((rect.height - text_size.y) / 2 if center_y else 0) - rl.draw_text_ex(font, text, rl.Vector2(x, y), font_size, 0, color) diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py deleted file mode 100644 index 1f202141c3806b..00000000000000 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ /dev/null @@ -1,234 +0,0 @@ -import time -import numpy as np -import pyray as rl -from cereal import log, messaging -from msgq.visionipc import VisionStreamType -from openpilot.selfdrive.ui import UI_BORDER_SIZE -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.selfdrive.ui.onroad.alert_renderer import AlertRenderer -from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer -from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer -from openpilot.selfdrive.ui.onroad.model_renderer import ModelRenderer -from openpilot.selfdrive.ui.onroad.cameraview import CameraView -from openpilot.system.ui.lib.application import gui_app -from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame -from openpilot.common.transformations.orientation import rot_from_euler - -OpState = log.SelfdriveState.OpenpilotState -CALIBRATED = log.LiveCalibrationData.Status.calibrated -ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD -WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD -DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] - -BORDER_COLORS = { - UIStatus.DISENGAGED: rl.Color(0x12, 0x28, 0x39, 0xFF), # Blue for disengaged state - UIStatus.OVERRIDE: rl.Color(0x89, 0x92, 0x8D, 0xFF), # Gray for override state - UIStatus.ENGAGED: rl.Color(0x16, 0x7F, 0x40, 0xFF), # Green for engaged state -} - -WIDE_CAM_MAX_SPEED = 10.0 # m/s (22 mph) -ROAD_CAM_MIN_SPEED = 15.0 # m/s (34 mph) -INF_POINT = np.array([1000.0, 0.0, 0.0]) - - -class AugmentedRoadView(CameraView): - def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD): - super().__init__("camerad", stream_type) - self._set_placeholder_color(BORDER_COLORS[UIStatus.DISENGAGED]) - - self.device_camera: DeviceCameraConfig | None = None - self.view_from_calib = view_frame_from_device_frame.copy() - self.view_from_wide_calib = view_frame_from_device_frame.copy() - - self._matrix_cache_key = (0, 0.0, 0.0, stream_type) - self._cached_matrix: np.ndarray | None = None - self._content_rect = rl.Rectangle() - - self.model_renderer = ModelRenderer() - self._hud_renderer = HudRenderer() - self.alert_renderer = AlertRenderer() - self.driver_state_renderer = DriverStateRenderer() - - # debug - self._pm = messaging.PubMaster(['uiDebug']) - - def _render(self, rect): - # Only render when system is started to avoid invalid data access - start_draw = time.monotonic() - if not ui_state.started: - return - - self._switch_stream_if_needed(ui_state.sm) - - # Update calibration before rendering - self._update_calibration() - - # Create inner content area with border padding - self._content_rect = rl.Rectangle( - rect.x + UI_BORDER_SIZE, - rect.y + UI_BORDER_SIZE, - rect.width - 2 * UI_BORDER_SIZE, - rect.height - 2 * UI_BORDER_SIZE, - ) - - # Enable scissor mode to clip all rendering within content rectangle boundaries - # This creates a rendering viewport that prevents graphics from drawing outside the border - rl.begin_scissor_mode( - int(self._content_rect.x), - int(self._content_rect.y), - int(self._content_rect.width), - int(self._content_rect.height) - ) - - # Render the base camera view - super()._render(rect) - - # Draw all UI overlays - self.model_renderer.render(self._content_rect) - self._hud_renderer.render(self._content_rect) - self.alert_renderer.render(self._content_rect) - self.driver_state_renderer.render(self._content_rect) - - # Custom UI extension point - add custom overlays here - # Use self._content_rect for positioning within camera bounds - - # End clipping region - rl.end_scissor_mode() - - # Draw colored border based on driving state - self._draw_border(rect) - - # publish uiDebug - msg = messaging.new_message('uiDebug') - msg.uiDebug.drawTimeMillis = (time.monotonic() - start_draw) * 1000 - self._pm.send('uiDebug', msg) - - def _handle_mouse_press(self, _): - if not self._hud_renderer.user_interacting() and self._click_callback is not None: - self._click_callback() - - def _handle_mouse_release(self, _): - # We only call click callback on press if not interacting with HUD - pass - - def _draw_border(self, rect: rl.Rectangle): - rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, rl.BLACK) - border_roundness = 0.12 - border_color = BORDER_COLORS.get(ui_state.status, BORDER_COLORS[UIStatus.DISENGAGED]) - border_rect = rl.Rectangle(rect.x + UI_BORDER_SIZE, rect.y + UI_BORDER_SIZE, - rect.width - 2 * UI_BORDER_SIZE, rect.height - 2 * UI_BORDER_SIZE) - rl.draw_rectangle_rounded_lines_ex(border_rect, border_roundness, 10, UI_BORDER_SIZE, border_color) - - def _switch_stream_if_needed(self, sm): - if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: - v_ego = sm['carState'].vEgo - if v_ego < WIDE_CAM_MAX_SPEED: - target = WIDE_CAM - elif v_ego > ROAD_CAM_MIN_SPEED: - target = ROAD_CAM - else: - # Hysteresis zone - keep current stream - target = self.stream_type - else: - target = ROAD_CAM - - if self.stream_type != target: - self.switch_stream(target) - - def _update_calibration(self): - # Update device camera if not already set - sm = ui_state.sm - if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']: - self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] - - # Check if live calibration data is available and valid - if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): - return - - calib = sm['liveCalibration'] - if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: - return - - # Update view_from_calib matrix - device_from_calib = rot_from_euler(calib.rpyCalib) - self.view_from_calib = view_frame_from_device_frame @ device_from_calib - - # Update wide calibration if available - if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3: - wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) - self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - # Check if we can use cached matrix - cache_key = ( - ui_state.sm.recv_frame['liveCalibration'], - self._content_rect.width, - self._content_rect.height, - self.stream_type - ) - if cache_key == self._matrix_cache_key and self._cached_matrix is not None: - return self._cached_matrix - - # Get camera configuration - device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA - is_wide_camera = self.stream_type == WIDE_CAM - intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics - calibration = self.view_from_wide_calib if is_wide_camera else self.view_from_calib - zoom = 2.0 if is_wide_camera else 1.1 - - # Calculate transforms for vanishing point - calib_transform = intrinsic @ calibration - kep = calib_transform @ INF_POINT - - # Calculate center points and dimensions - x, y = self._content_rect.x, self._content_rect.y - w, h = self._content_rect.width, self._content_rect.height - cx, cy = intrinsic[0, 2], intrinsic[1, 2] - - # Calculate max allowed offsets with margins - margin = 5 - max_x_offset = cx * zoom - w / 2 - margin - max_y_offset = cy * zoom - h / 2 - margin - - # Calculate and clamp offsets to prevent out-of-bounds issues - try: - if abs(kep[2]) > 1e-6: - x_offset = np.clip((kep[0] / kep[2] - cx) * zoom, -max_x_offset, max_x_offset) - y_offset = np.clip((kep[1] / kep[2] - cy) * zoom, -max_y_offset, max_y_offset) - else: - x_offset, y_offset = 0, 0 - except (ZeroDivisionError, OverflowError): - x_offset, y_offset = 0, 0 - - # Cache the computed transformation matrix to avoid recalculations - self._matrix_cache_key = cache_key - self._cached_matrix = np.array([ - [zoom * 2 * cx / w, 0, -x_offset / w * 2], - [0, zoom * 2 * cy / h, -y_offset / h * 2], - [0, 0, 1.0] - ]) - - video_transform = np.array([ - [zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)], - [0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)], - [0.0, 0.0, 1.0] - ]) - self.model_renderer.set_transform(video_transform @ calib_transform) - - return self._cached_matrix - - -if __name__ == "__main__": - gui_app.init_window("OnRoad Camera View") - road_camera_view = AugmentedRoadView(ROAD_CAM) - print("***press space to switch camera view***") - try: - for _ in gui_app.render(): - ui_state.update() - if rl.is_key_released(rl.KeyboardKey.KEY_SPACE): - if WIDE_CAM in road_camera_view.available_streams: - stream = ROAD_CAM if road_camera_view.stream_type == WIDE_CAM else WIDE_CAM - road_camera_view.switch_stream(stream) - road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - road_camera_view.close() diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py deleted file mode 100644 index 544394846595de..00000000000000 --- a/selfdrive/ui/onroad/cameraview.py +++ /dev/null @@ -1,366 +0,0 @@ -import platform -import numpy as np -import pyray as rl - -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state - -CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts - -VERSION = """ -#version 300 es -precision mediump float; -""" -if platform.system() == "Darwin": - VERSION = """ - #version 330 core - """ - - -VERTEX_SHADER = VERSION + """ -in vec3 vertexPosition; -in vec2 vertexTexCoord; -in vec3 vertexNormal; -in vec4 vertexColor; -uniform mat4 mvp; -out vec2 fragTexCoord; -out vec4 fragColor; -void main() { - fragTexCoord = vertexTexCoord; - fragColor = vertexColor; - gl_Position = mvp * vec4(vertexPosition, 1.0); -} -""" - -# Choose fragment shader based on platform capabilities -if TICI: - FRAME_FRAGMENT_SHADER = """ - #version 300 es - #extension GL_OES_EGL_image_external_essl3 : enable - precision mediump float; - in vec2 fragTexCoord; - uniform samplerExternalOES texture0; - out vec4 fragColor; - void main() { - vec4 color = texture(texture0, fragTexCoord); - fragColor = vec4(pow(color.rgb, vec3(1.0/1.28)), color.a); - } - """ -else: - FRAME_FRAGMENT_SHADER = VERSION + """ - in vec2 fragTexCoord; - uniform sampler2D texture0; - uniform sampler2D texture1; - out vec4 fragColor; - void main() { - float y = texture(texture0, fragTexCoord).r; - vec2 uv = texture(texture1, fragTexCoord).ra - 0.5; - fragColor = vec4(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x, 1.0); - } - """ - - -class CameraView(Widget): - def __init__(self, name: str, stream_type: VisionStreamType): - super().__init__() - self._name = name - # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) - self._stream_type = stream_type - self.available_streams: list[VisionStreamType] = [] - - # Target stream for switching - self._target_client: VisionIpcClient | None = None - self._target_stream_type: VisionStreamType | None = None - self._switching: bool = False - - self._texture_needs_update = True - self.last_connection_attempt: float = 0.0 - self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) - self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1 - - self.frame: VisionBuf | None = None - self.texture_y: rl.Texture | None = None - self.texture_uv: rl.Texture | None = None - - # EGL resources - self.egl_images: dict[int, EGLImage] = {} - self.egl_texture: rl.Texture | None = None - - self._placeholder_color: rl.Color | None = None - - # Initialize EGL for zero-copy rendering on TICI - if TICI: - if not init_egl(): - raise RuntimeError("Failed to initialize EGL") - - # Create a 1x1 pixel placeholder texture for EGL image binding - temp_image = rl.gen_image_color(1, 1, rl.BLACK) - self.egl_texture = rl.load_texture_from_image(temp_image) - rl.unload_image(temp_image) - - ui_state.add_offroad_transition_callback(self._offroad_transition) - - def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.frame = None - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) - - def _set_placeholder_color(self, color: rl.Color): - """Set a placeholder color to be drawn when no frame is available.""" - self._placeholder_color = color - - def switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: - return - - if self._switching and self._target_stream_type == stream_type: - return - - cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}') - - if self._target_client: - del self._target_client - - self._target_stream_type = stream_type - self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) - self._switching = True - - @property - def stream_type(self) -> VisionStreamType: - return self._stream_type - - def close(self) -> None: - self._clear_textures() - - # Clean up EGL texture - if TICI and self.egl_texture: - rl.unload_texture(self.egl_texture) - self.egl_texture = None - - # Clean up shader - if self.shader and self.shader.id: - rl.unload_shader(self.shader) - - self.frame = None - self.available_streams.clear() - self.client = None - - def __del__(self): - self.close() - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - if not self.frame: - return np.eye(3) - - # Calculate aspect ratios - widget_aspect_ratio = rect.width / rect.height - frame_aspect_ratio = self.frame.width / self.frame.height - - # Calculate scaling factors to maintain aspect ratio - zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0) - zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0) - - return np.array([ - [zx, 0.0, 0.0], - [0.0, zy, 0.0], - [0.0, 0.0, 1.0] - ]) - - def _render(self, rect: rl.Rectangle): - if self._switching: - self._handle_switch() - - if not self._ensure_connection(): - self._draw_placeholder(rect) - return - - # Try to get a new buffer without blocking - buffer = self.client.recv(timeout_ms=0) - if buffer: - self._texture_needs_update = True - self.frame = buffer - elif not self.client.is_connected(): - # ensure we clear the displayed frame when the connection is lost - self.frame = None - - if not self.frame: - self._draw_placeholder(rect) - return - - transform = self._calc_frame_matrix(rect) - src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height)) - # Flip driver camera horizontally - if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER: - src_rect.width = -src_rect.width - - # Calculate scale - scale_x = rect.width * transform[0, 0] # zx - scale_y = rect.height * transform[1, 1] # zy - - # Calculate base position (centered) - x_offset = rect.x + (rect.width - scale_x) / 2 - y_offset = rect.y + (rect.height - scale_y) / 2 - - x_offset += transform[0, 2] * rect.width / 2 - y_offset += transform[1, 2] * rect.height / 2 - - dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y) - - # Render with appropriate method - if TICI: - self._render_egl(src_rect, dst_rect) - else: - self._render_textures(src_rect, dst_rect) - - def _draw_placeholder(self, rect: rl.Rectangle): - if self._placeholder_color: - rl.draw_rectangle_rec(rect, self._placeholder_color) - - def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None: - return - - idx = self.frame.idx - egl_image = self.egl_images.get(idx) - - # Create EGL image if needed - if egl_image is None: - egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset) - if egl_image: - self.egl_images[idx] = egl_image - else: - return - - # Update texture dimensions to match current frame - self.egl_texture.width = self.frame.width - self.egl_texture.height = self.frame.height - - # Bind the EGL image to our texture - bind_egl_image_to_texture(self.egl_texture.id, egl_image) - - # Render with shader - rl.begin_shader_mode(self.shader) - rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using texture copies""" - if not self.texture_y or not self.texture_uv or self.frame is None: - return - - # Update textures with new frame data - if self._texture_needs_update: - y_data = self.frame.data[: self.frame.uv_offset] - uv_data = self.frame.data[self.frame.uv_offset:] - - rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) - self._texture_needs_update = False - - # Render with shader - rl.begin_shader_mode(self.shader) - rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv) - rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - self.available_streams.clear() - - # Throttle connection attempts - current_time = rl.get_time() - if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: - return False - self.last_connection_attempt = current_time - - if not self.client.connect(False) or not self.client.num_buffers: - return False - - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) - - return True - - def _handle_switch(self) -> None: - """Check if target stream is ready and switch immediately.""" - if not self._target_client or not self._switching: - return - - # Try to connect target if needed - if not self._target_client.is_connected(): - if not self._target_client.connect(False) or not self._target_client.num_buffers: - return - - cloudlog.debug(f"Target stream connected: {self._target_stream_type}") - - # Check if target has frames ready - target_frame = self._target_client.recv(timeout_ms=0) - if target_frame: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() - - def _complete_switch(self) -> None: - """Instantly switch to target stream.""" - cloudlog.debug(f"Switching to {self._target_stream_type}") - # Clean up current resources - if self.client: - del self.client - - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - self._texture_needs_update = True - - # Reset state - self._target_client = None - self._target_stream_type = None - self._switching = False - - # Initialize textures for new stream - self._initialize_textures() - - def _initialize_textures(self): - self._clear_textures() - if not TICI: - self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride), - int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE)) - self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2), - int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA)) - - def _clear_textures(self): - if self.texture_y and self.texture_y.id: - rl.unload_texture(self.texture_y) - self.texture_y = None - - if self.texture_uv and self.texture_uv.id: - rl.unload_texture(self.texture_uv) - self.texture_uv = None - - # Clean up EGL resources - if TICI: - for data in self.egl_images.values(): - destroy_egl_image(data) - self.egl_images = {} - - -if __name__ == "__main__": - gui_app.init_window("camera view") - road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) - for _ in gui_app.render(): - road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) diff --git a/selfdrive/ui/onroad/driver_camera_dialog.py b/selfdrive/ui/onroad/driver_camera_dialog.py deleted file mode 100644 index f69ad8c49cf209..00000000000000 --- a/selfdrive/ui/onroad/driver_camera_dialog.py +++ /dev/null @@ -1,111 +0,0 @@ -import numpy as np -import pyray as rl -from msgq.visionipc import VisionStreamType -from openpilot.selfdrive.ui.onroad.cameraview import CameraView -from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer -from openpilot.selfdrive.ui.ui_state import ui_state, device -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets.label import gui_label - - -class DriverCameraDialog(CameraView): - def __init__(self): - super().__init__("camerad", VisionStreamType.VISION_STREAM_DRIVER) - self.driver_state_renderer = DriverStateRenderer() - # TODO: this can grow unbounded, should be given some thought - device.add_interactive_timeout_callback(lambda: gui_app.set_modal_overlay(None)) - ui_state.params.put_bool("IsDriverViewEnabled", True) - - def hide_event(self): - super().hide_event() - ui_state.params.put_bool("IsDriverViewEnabled", False) - self.close() - - def _handle_mouse_release(self, _): - super()._handle_mouse_release(_) - gui_app.set_modal_overlay(None) - - def __del__(self): - self.close() - - def _render(self, rect): - super()._render(rect) - - if not self.frame: - gui_label( - rect, - tr("camera starting"), - font_size=100, - font_weight=FontWeight.BOLD, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - ) - return -1 - - self._draw_face_detection(rect) - self.driver_state_renderer.render(rect) - - return -1 - - def _draw_face_detection(self, rect: rl.Rectangle) -> None: - driver_state = ui_state.sm["driverStateV2"] - is_rhd = driver_state.wheelOnRightProb > 0.5 - driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData - face_detect = driver_data.faceProb > 0.7 - if not face_detect: - return - - # Get face position and orientation - face_x, face_y = driver_data.facePosition - face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) - alpha = 0.7 - if face_std > 0.15: - alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) - - # use approx instead of distort_points - # TODO: replace with distort_points - fbox_x = int(1080.0 - 1714.0 * face_x) - fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y) - box_size = 220 - - line_color = rl.Color(255, 255, 255, int(alpha * 255)) - rl.draw_rectangle_rounded_lines_ex( - rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), - 35.0 / box_size / 2, - 10, - 10, - line_color, - ) - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - driver_view_ratio = 2.0 - - # Get stream dimensions - if self.frame: - stream_width = self.frame.width - stream_height = self.frame.height - else: - # Default values if frame not available - stream_width = 1928 - stream_height = 1208 - - yscale = stream_height * driver_view_ratio / stream_width - xscale = yscale * rect.height / rect.width * stream_width / stream_height - - return np.array([ - [xscale, 0.0, 0.0], - [0.0, yscale, 0.0], - [0.0, 0.0, 1.0] - ]) - - -if __name__ == "__main__": - gui_app.init_window("Driver Camera View") - - driver_camera_view = DriverCameraDialog() - try: - for _ in gui_app.render(): - ui_state.update() - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - driver_camera_view.close() diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py deleted file mode 100644 index 7b3181d1ac50cb..00000000000000 --- a/selfdrive/ui/onroad/driver_state.py +++ /dev/null @@ -1,231 +0,0 @@ -import numpy as np -import pyray as rl -from cereal import log -from dataclasses import dataclass -from openpilot.selfdrive.ui import UI_BORDER_SIZE -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import Widget - -AlertSize = log.SelfdriveState.AlertSize - -# Default 3D coordinates for face keypoints as a NumPy array -DEFAULT_FACE_KPTS_3D = np.array([ - [-5.98, -51.20, 8.00], [-17.64, -49.14, 8.00], [-23.81, -46.40, 8.00], [-29.98, -40.91, 8.00], - [-32.04, -37.49, 8.00], [-34.10, -32.00, 8.00], [-36.16, -21.03, 8.00], [-36.16, 6.40, 8.00], - [-35.47, 10.51, 8.00], [-32.73, 19.43, 8.00], [-29.30, 26.29, 8.00], [-24.50, 33.83, 8.00], - [-19.01, 41.37, 8.00], [-14.21, 46.17, 8.00], [-12.16, 47.54, 8.00], [-4.61, 49.60, 8.00], - [4.99, 49.60, 8.00], [12.53, 47.54, 8.00], [14.59, 46.17, 8.00], [19.39, 41.37, 8.00], - [24.87, 33.83, 8.00], [29.67, 26.29, 8.00], [33.10, 19.43, 8.00], [35.84, 10.51, 8.00], - [36.53, 6.40, 8.00], [36.53, -21.03, 8.00], [34.47, -32.00, 8.00], [32.42, -37.49, 8.00], - [30.36, -40.91, 8.00], [24.19, -46.40, 8.00], [18.02, -49.14, 8.00], [6.36, -51.20, 8.00], - [-5.98, -51.20, 8.00], -], dtype=np.float32) - -# UI constants -BTN_SIZE = 192 -IMG_SIZE = 144 -ARC_LENGTH = 133 -ARC_THICKNESS_DEFAULT = 6.7 -ARC_THICKNESS_EXTEND = 12.0 - -SCALES_POS = np.array([0.9, 0.4, 0.4], dtype=np.float32) -SCALES_NEG = np.array([0.7, 0.4, 0.4], dtype=np.float32) - -ARC_POINT_COUNT = 37 # Number of points in the arc -ARC_ANGLES = np.linspace(0.0, np.pi, ARC_POINT_COUNT, dtype=np.float32) - - -@dataclass -class ArcData: - """Data structure for arc rendering parameters.""" - x: float - y: float - width: float - height: float - thickness: float - - -class DriverStateRenderer(Widget): - def __init__(self): - super().__init__() - # Initial state with NumPy arrays - self.face_kpts_draw = DEFAULT_FACE_KPTS_3D.copy() - self.is_active = False - self.is_rhd = False - self.dm_fade_state = 0.0 - self.driver_pose_vals = np.zeros(3, dtype=np.float32) - self.driver_pose_diff = np.zeros(3, dtype=np.float32) - self.driver_pose_sins = np.zeros(3, dtype=np.float32) - self.driver_pose_coss = np.zeros(3, dtype=np.float32) - self.face_keypoints_transformed = np.zeros((DEFAULT_FACE_KPTS_3D.shape[0], 2), dtype=np.float32) - self.position_x: float = 0.0 - self.position_y: float = 0.0 - self.h_arc_data = None - self.v_arc_data = None - - # Pre-allocate drawing arrays - self.face_lines = [rl.Vector2(0, 0) for _ in range(len(DEFAULT_FACE_KPTS_3D))] - self.h_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)] - self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)] - - # Load the driver face icon - self.dm_img = gui_app.texture("icons/driver_face.png", IMG_SIZE, IMG_SIZE) - - # Colors - self.white_color = rl.Color(255, 255, 255, 255) - self.arc_color = rl.Color(26, 242, 66, 255) - self.engaged_color = rl.Color(26, 242, 66, 255) - self.disengaged_color = rl.Color(139, 139, 139, 255) - - self.set_visible(lambda: (ui_state.sm["selfdriveState"].alertSize == AlertSize.none and - ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame)) - - def _render(self, rect): - # Set opacity based on active state - opacity = 0.65 if self.is_active else 0.2 - - # Draw background circle - rl.draw_circle(int(self.position_x), int(self.position_y), BTN_SIZE // 2, rl.Color(0, 0, 0, 70)) - - # Draw face icon - icon_pos = rl.Vector2(self.position_x - self.dm_img.width // 2, self.position_y - self.dm_img.height // 2) - rl.draw_texture_v(self.dm_img, icon_pos, rl.Color(255, 255, 255, int(255 * opacity))) - - # Draw face outline - self.white_color.a = int(255 * opacity) - rl.draw_spline_linear(self.face_lines, len(self.face_lines), 5.2, self.white_color) - - # Set arc color based on engaged state - self.arc_color = self.engaged_color if ui_state.engaged else self.disengaged_color - self.arc_color.a = int(0.4 * 255 * (1.0 - self.dm_fade_state)) # Fade out when inactive - - # Draw arcs - if self.h_arc_data: - rl.draw_spline_linear(self.h_arc_lines, len(self.h_arc_lines), self.h_arc_data.thickness, self.arc_color) - if self.v_arc_data: - rl.draw_spline_linear(self.v_arc_lines, len(self.v_arc_lines), self.v_arc_data.thickness, self.arc_color) - - def _update_state(self): - """Update the driver monitoring state based on model data""" - sm = ui_state.sm - if not self.is_visible: - return - - # Get monitoring state - dm_state = sm["driverMonitoringState"] - self.is_active = dm_state.isActiveMode - self.is_rhd = dm_state.isRHD - - # Update fade state (smoother transition between active/inactive) - fade_target = 0.0 if self.is_active else 0.5 - self.dm_fade_state = np.clip(self.dm_fade_state + 0.2 * (fade_target - self.dm_fade_state), 0.0, 1.0) - - # Get driver orientation data from appropriate camera - driverstate = sm["driverStateV2"] - driver_data = driverstate.rightDriverData if self.is_rhd else driverstate.leftDriverData - driver_orient = driver_data.faceOrientation - - # Update pose values with scaling and smoothing - driver_orient = np.array(driver_orient) - scales = np.where(driver_orient < 0, SCALES_NEG, SCALES_POS) - v_this = driver_orient * scales - self.driver_pose_diff = np.abs(self.driver_pose_vals - v_this) - self.driver_pose_vals = 0.8 * v_this + 0.2 * self.driver_pose_vals # Smooth changes - - # Apply fade to rotation and compute sin/cos - rotation_amount = self.driver_pose_vals * (1.0 - self.dm_fade_state) - self.driver_pose_sins = np.sin(rotation_amount) - self.driver_pose_coss = np.cos(rotation_amount) - - # Create rotation matrix for 3D face model - sin_y, sin_x, sin_z = self.driver_pose_sins - cos_y, cos_x, cos_z = self.driver_pose_coss - r_xyz = np.array( - [ - [cos_x * cos_z, cos_x * sin_z, -sin_x], - [-sin_y * sin_x * cos_z - cos_y * sin_z, -sin_y * sin_x * sin_z + cos_y * cos_z, -sin_y * cos_x], - [cos_y * sin_x * cos_z - sin_y * sin_z, cos_y * sin_x * sin_z + sin_y * cos_z, cos_y * cos_x], - ] - ) - - # Transform face keypoints using vectorized matrix multiplication - self.face_kpts_draw = DEFAULT_FACE_KPTS_3D @ r_xyz.T - self.face_kpts_draw[:, 2] = self.face_kpts_draw[:, 2] * (1.0 - self.dm_fade_state) + 8 * self.dm_fade_state - - # Pre-calculate the transformed keypoints - kp_depth = (self.face_kpts_draw[:, 2] - 8) / 120.0 + 1.0 - self.face_keypoints_transformed = self.face_kpts_draw[:, :2] * kp_depth[:, None] - - # Pre-calculate all drawing elements - self._pre_calculate_drawing_elements() - - def _pre_calculate_drawing_elements(self): - """Pre-calculate all drawing elements based on the current rectangle""" - # Calculate icon position (bottom-left or bottom-right) - width, height = self._rect.width, self._rect.height - offset = UI_BORDER_SIZE + BTN_SIZE // 2 - self.position_x = self._rect.x + (width - offset if self.is_rhd else offset) - self.position_y = self._rect.y + height - offset - - # Pre-calculate the face lines positions - positioned_keypoints = self.face_keypoints_transformed + np.array([self.position_x, self.position_y]) - for i in range(len(positioned_keypoints)): - self.face_lines[i].x = positioned_keypoints[i][0] - self.face_lines[i].y = positioned_keypoints[i][1] - - # Calculate arc dimensions based on head rotation - delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0 # Horizontal movement - delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0 # Vertical movement - - # Horizontal arc - h_width = abs(delta_x) - self.h_arc_data = self._calculate_arc_data( - delta_x, h_width, self.position_x, self.position_y - ARC_LENGTH / 2, - self.driver_pose_sins[1], self.driver_pose_diff[1], is_horizontal=True - ) - - # Vertical arc - v_height = abs(delta_y) - self.v_arc_data = self._calculate_arc_data( - delta_y, v_height, self.position_x - ARC_LENGTH / 2, self.position_y, - self.driver_pose_sins[0], self.driver_pose_diff[0], is_horizontal=False - ) - - def _calculate_arc_data( - self, delta: float, size: float, x: float, y: float, sin_val: float, diff_val: float, is_horizontal: bool - ): - """Calculate arc data and pre-compute arc points.""" - if size <= 0: - return None - - thickness = ARC_THICKNESS_DEFAULT + ARC_THICKNESS_EXTEND * min(1.0, diff_val * 5.0) - start_angle = (90 if sin_val > 0 else -90) if is_horizontal else (0 if sin_val > 0 else 180) - x = min(x + delta, x) if is_horizontal else x - y = y if is_horizontal else min(y + delta, y) - - arc_data = ArcData( - x=x, - y=y, - width=size if is_horizontal else ARC_LENGTH, - height=ARC_LENGTH if is_horizontal else size, - thickness=thickness, - ) - - # Pre-calculate arc points - angles = ARC_ANGLES + np.deg2rad(start_angle) - - center_x = x + arc_data.width / 2 - center_y = y + arc_data.height / 2 - radius_x = arc_data.width / 2 - radius_y = arc_data.height / 2 - - x_coords = center_x + np.cos(angles) * radius_x - y_coords = center_y - np.sin(angles) * radius_y - - arc_lines = self.h_arc_lines if is_horizontal else self.v_arc_lines - for i, (x_coord, y_coord) in enumerate(zip(x_coords, y_coords, strict=True)): - arc_lines[i].x = x_coord - arc_lines[i].y = y_coord - - return arc_data diff --git a/selfdrive/ui/onroad/exp_button.py b/selfdrive/ui/onroad/exp_button.py deleted file mode 100644 index e5d81714130d67..00000000000000 --- a/selfdrive/ui/onroad/exp_button.py +++ /dev/null @@ -1,70 +0,0 @@ -import time -import pyray as rl -from openpilot.common.params import Params -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.widgets import Widget - - -class ExpButton(Widget): - def __init__(self, button_size: int, icon_size: int): - super().__init__() - self._params = Params() - self._experimental_mode: bool = False - self._engageable: bool = False - - # State hold mechanism - self._hold_duration = 2.0 # seconds - self._held_mode: bool | None = None - self._hold_end_time: float | None = None - - self._white_color: rl.Color = rl.Color(255, 255, 255, 255) - self._black_bg: rl.Color = rl.Color(0, 0, 0, 166) - self._txt_wheel: rl.Texture = gui_app.texture('icons/chffr_wheel.png', icon_size, icon_size) - self._txt_exp: rl.Texture = gui_app.texture('icons/experimental.png', icon_size, icon_size) - self._rect = rl.Rectangle(0, 0, button_size, button_size) - - def set_rect(self, rect: rl.Rectangle) -> None: - self._rect.x, self._rect.y = rect.x, rect.y - - def _update_state(self) -> None: - selfdrive_state = ui_state.sm["selfdriveState"] - self._experimental_mode = selfdrive_state.experimentalMode - self._engageable = selfdrive_state.engageable or selfdrive_state.enabled - - def _handle_mouse_release(self, _): - super()._handle_mouse_release(_) - if self._is_toggle_allowed(): - new_mode = not self._experimental_mode - self._params.put_bool("ExperimentalMode", new_mode) - - # Hold new state temporarily - self._held_mode = new_mode - self._hold_end_time = time.monotonic() + self._hold_duration - - def _render(self, rect: rl.Rectangle) -> None: - center_x = int(self._rect.x + self._rect.width // 2) - center_y = int(self._rect.y + self._rect.height // 2) - - self._white_color.a = 180 if self.is_pressed or not self._engageable else 255 - - texture = self._txt_exp if self._held_or_actual_mode() else self._txt_wheel - rl.draw_circle(center_x, center_y, self._rect.width / 2, self._black_bg) - rl.draw_texture(texture, center_x - texture.width // 2, center_y - texture.height // 2, self._white_color) - - def _held_or_actual_mode(self): - now = time.monotonic() - if self._hold_end_time and now < self._hold_end_time: - return self._held_mode - - if self._hold_end_time and now >= self._hold_end_time: - self._hold_end_time = self._held_mode = None - - return self._experimental_mode - - def _is_toggle_allowed(self): - if not self._params.get_bool("ExperimentalModeConfirmed"): - return False - - # Mirror exp mode toggle using persistent car params - return ui_state.has_longitudinal_control diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py deleted file mode 100644 index 79f150deea03d2..00000000000000 --- a/selfdrive/ui/onroad/hud_renderer.py +++ /dev/null @@ -1,180 +0,0 @@ -import pyray as rl -from dataclasses import dataclass -from openpilot.common.constants import CV -from openpilot.selfdrive.ui.onroad.exp_button import ExpButton -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget - -# Constants -SET_SPEED_NA = 255 -KM_TO_MILE = 0.621371 -CRUISE_DISABLED_CHAR = '–' - - -@dataclass(frozen=True) -class UIConfig: - header_height: int = 300 - border_size: int = 30 - button_size: int = 192 - set_speed_width_metric: int = 200 - set_speed_width_imperial: int = 172 - set_speed_height: int = 204 - wheel_icon_size: int = 144 - - -@dataclass(frozen=True) -class FontSizes: - current_speed: int = 176 - speed_unit: int = 66 - max_speed: int = 40 - set_speed: int = 90 - - -@dataclass(frozen=True) -class Colors: - WHITE = rl.WHITE - DISENGAGED = rl.Color(145, 155, 149, 255) - OVERRIDE = rl.Color(145, 155, 149, 255) # Added - ENGAGED = rl.Color(128, 216, 166, 255) - DISENGAGED_BG = rl.Color(0, 0, 0, 153) - OVERRIDE_BG = rl.Color(145, 155, 149, 204) - ENGAGED_BG = rl.Color(128, 216, 166, 204) - GREY = rl.Color(166, 166, 166, 255) - DARK_GREY = rl.Color(114, 114, 114, 255) - BLACK_TRANSLUCENT = rl.Color(0, 0, 0, 166) - WHITE_TRANSLUCENT = rl.Color(255, 255, 255, 200) - BORDER_TRANSLUCENT = rl.Color(255, 255, 255, 75) - HEADER_GRADIENT_START = rl.Color(0, 0, 0, 114) - HEADER_GRADIENT_END = rl.BLANK - - -UI_CONFIG = UIConfig() -FONT_SIZES = FontSizes() -COLORS = Colors() - - -class HudRenderer(Widget): - def __init__(self): - super().__init__() - """Initialize the HUD renderer.""" - self.is_cruise_set: bool = False - self.is_cruise_available: bool = True - self.set_speed: float = SET_SPEED_NA - self.speed: float = 0.0 - self.v_ego_cluster_seen: bool = False - - self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD) - self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD) - self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM) - - self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size) - - def _update_state(self) -> None: - """Update HUD state based on car state and controls state.""" - sm = ui_state.sm - if sm.recv_frame["carState"] < ui_state.started_frame: - self.is_cruise_set = False - self.set_speed = SET_SPEED_NA - self.speed = 0.0 - return - - controls_state = sm['controlsState'] - car_state = sm['carState'] - - v_cruise_cluster = car_state.vCruiseCluster - self.set_speed = ( - controls_state.vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster - ) - self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA - self.is_cruise_available = self.set_speed != -1 - - if self.is_cruise_set and not ui_state.is_metric: - self.set_speed *= KM_TO_MILE - - v_ego_cluster = car_state.vEgoCluster - self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0 - v_ego = v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo - speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH - self.speed = max(0.0, v_ego * speed_conversion) - - def _render(self, rect: rl.Rectangle) -> None: - """Render HUD elements to the screen.""" - # Draw the header background - rl.draw_rectangle_gradient_v( - int(rect.x), - int(rect.y), - int(rect.width), - UI_CONFIG.header_height, - COLORS.HEADER_GRADIENT_START, - COLORS.HEADER_GRADIENT_END, - ) - - if self.is_cruise_available: - self._draw_set_speed(rect) - - self._draw_current_speed(rect) - - button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size - button_y = rect.y + UI_CONFIG.border_size - self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size)) - - def user_interacting(self) -> bool: - return self._exp_button.is_pressed - - def _draw_set_speed(self, rect: rl.Rectangle) -> None: - """Draw the MAX speed indicator box.""" - set_speed_width = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial - x = rect.x + 60 + (UI_CONFIG.set_speed_width_imperial - set_speed_width) // 2 - y = rect.y + 45 - - set_speed_rect = rl.Rectangle(x, y, set_speed_width, UI_CONFIG.set_speed_height) - rl.draw_rectangle_rounded(set_speed_rect, 0.35, 10, COLORS.BLACK_TRANSLUCENT) - rl.draw_rectangle_rounded_lines_ex(set_speed_rect, 0.35, 10, 6, COLORS.BORDER_TRANSLUCENT) - - max_color = COLORS.GREY - set_speed_color = COLORS.DARK_GREY - if self.is_cruise_set: - set_speed_color = COLORS.WHITE - if ui_state.status == UIStatus.ENGAGED: - max_color = COLORS.ENGAGED - elif ui_state.status == UIStatus.DISENGAGED: - max_color = COLORS.DISENGAGED - elif ui_state.status == UIStatus.OVERRIDE: - max_color = COLORS.OVERRIDE - - max_text = tr("MAX") - max_text_width = measure_text_cached(self._font_semi_bold, max_text, FONT_SIZES.max_speed).x - rl.draw_text_ex( - self._font_semi_bold, - max_text, - rl.Vector2(x + (set_speed_width - max_text_width) / 2, y + 27), - FONT_SIZES.max_speed, - 0, - max_color, - ) - - set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(self.set_speed)) - speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x - rl.draw_text_ex( - self._font_bold, - set_speed_text, - rl.Vector2(x + (set_speed_width - speed_text_width) / 2, y + 77), - FONT_SIZES.set_speed, - 0, - set_speed_color, - ) - - def _draw_current_speed(self, rect: rl.Rectangle) -> None: - """Draw the current vehicle speed and unit.""" - speed_text = str(round(self.speed)) - speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed) - speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2) - rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.WHITE) - - unit_text = tr("km/h") if ui_state.is_metric else tr("mph") - unit_text_size = measure_text_cached(self._font_medium, unit_text, FONT_SIZES.speed_unit) - unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2) - rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.WHITE_TRANSLUCENT) diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py deleted file mode 100644 index b9f601f8fb2b33..00000000000000 --- a/selfdrive/ui/onroad/model_renderer.py +++ /dev/null @@ -1,435 +0,0 @@ -import colorsys -import numpy as np -import pyray as rl -from cereal import messaging, car -from dataclasses import dataclass, field -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient -from openpilot.system.ui.widgets import Widget - -CLIP_MARGIN = 500 -MIN_DRAW_DISTANCE = 10.0 -MAX_DRAW_DISTANCE = 100.0 - -THROTTLE_COLORS = [ - rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) - rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35) - rl.Color(114, 255, 92, 0), # HSLF(112/360, 1.0, 0.68, 0.0) -] - -NO_THROTTLE_COLORS = [ - rl.Color(242, 242, 242, 102), # HSLF(148/360, 0.0, 0.95, 0.4) - rl.Color(242, 242, 242, 89), # HSLF(112/360, 0.0, 0.95, 0.35) - rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0) -] - - -@dataclass -class ModelPoints: - raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32)) - projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32)) - - -@dataclass -class LeadVehicle: - glow: list[float] = field(default_factory=list) - chevron: list[float] = field(default_factory=list) - fill_alpha: int = 0 - - -class ModelRenderer(Widget): - def __init__(self): - super().__init__() - self._longitudinal_control = False - self._experimental_mode = False - self._blend_filter = FirstOrderFilter(1.0, 0.25, 1 / gui_app.target_fps) - self._prev_allow_throttle = True - self._lane_line_probs = np.zeros(4, dtype=np.float32) - self._road_edge_stds = np.zeros(2, dtype=np.float32) - self._lead_vehicles = [LeadVehicle(), LeadVehicle()] - self._path_offset_z = HEIGHT_INIT[0] - - # Initialize ModelPoints objects - self._path = ModelPoints() - self._lane_lines = [ModelPoints() for _ in range(4)] - self._road_edges = [ModelPoints() for _ in range(2)] - self._acceleration_x = np.empty((0,), dtype=np.float32) - - # Transform matrix (3x3 for car space to screen space) - self._car_space_transform = np.zeros((3, 3), dtype=np.float32) - self._transform_dirty = True - self._clip_region = None - - self._exp_gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=[], - stops=[], - ) - - # Get longitudinal control setting from car parameters - if car_params := Params().get("CarParams"): - cp = messaging.log_from_bytes(car_params, car.CarParams) - self._longitudinal_control = cp.openpilotLongitudinalControl - - def set_transform(self, transform: np.ndarray): - self._car_space_transform = transform.astype(np.float32) - self._transform_dirty = True - - def _render(self, rect: rl.Rectangle): - sm = ui_state.sm - - # Check if data is up-to-date - if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or - sm.recv_frame["modelV2"] < ui_state.started_frame): - return - - # Set up clipping region - self._clip_region = rl.Rectangle( - rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN - ) - - # Update state - self._experimental_mode = sm['selfdriveState'].experimentalMode - - live_calib = sm['liveCalibration'] - self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0] - - if sm.updated['carParams']: - self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl - - model = sm['modelV2'] - radar_state = sm['radarState'] if sm.valid['radarState'] else None - lead_one = radar_state.leadOne if radar_state else None - render_lead_indicator = self._longitudinal_control and radar_state is not None - - # Update model data when needed - model_updated = sm.updated['modelV2'] - if model_updated or sm.updated['radarState'] or self._transform_dirty: - if model_updated: - self._update_raw_points(model) - - path_x_array = self._path.raw_points[:, 0] - if path_x_array.size == 0: - return - - self._update_model(lead_one, path_x_array) - if render_lead_indicator: - self._update_leads(radar_state, path_x_array) - self._transform_dirty = False - - # Draw elements - self._draw_lane_lines() - self._draw_path(sm) - - if render_lead_indicator and radar_state: - self._draw_lead_indicator() - - def _update_raw_points(self, model): - """Update raw 3D points from model data""" - self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T - - for i, lane_line in enumerate(model.laneLines): - self._lane_lines[i].raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T - - for i, road_edge in enumerate(model.roadEdges): - self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T - - self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32) - self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32) - self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32) - - def _update_leads(self, radar_state, path_x_array): - """Update positions of lead vehicles""" - self._lead_vehicles = [LeadVehicle(), LeadVehicle()] - leads = [radar_state.leadOne, radar_state.leadTwo] - - for i, lead_data in enumerate(leads): - if lead_data and lead_data.status: - d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel - idx = self._get_path_length_idx(path_x_array, d_rel) - - # Get z-coordinate from path at the lead vehicle position - z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 - point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) - if point: - self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) - - def _update_model(self, lead, path_x_array): - """Update model visualization data based on model message""" - max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) - max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) - - # Update lane lines using raw points - for i, lane_line in enumerate(self._lane_lines): - lane_line.projected_points = self._map_line_to_polygon( - lane_line.raw_points, 0.025 * self._lane_line_probs[i], 0.0, max_idx, max_distance - ) - - # Update road edges using raw points - for road_edge in self._road_edges: - road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, 0.025, 0.0, max_idx, max_distance) - - # Update path using raw points - if lead and lead.status: - lead_d = lead.dRel * 2.0 - max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) - - max_idx = self._get_path_length_idx(path_x_array, max_distance) - self._path.projected_points = self._map_line_to_polygon( - self._path.raw_points, 0.9, self._path_offset_z, max_idx, max_distance, allow_invert=False - ) - - self._update_experimental_gradient() - - def _update_experimental_gradient(self): - """Pre-calculate experimental mode gradient colors""" - if not self._experimental_mode: - return - - max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) - - segment_colors = [] - gradient_stops = [] - - i = 0 - while i < max_len: - # Some points (screen space) are out of frame (rect space) - track_y = self._path.projected_points[i][1] - if track_y < self._rect.y or track_y > (self._rect.y + self._rect.height): - i += 1 - continue - - # Calculate color based on acceleration (0 is bottom, 1 is top) - lin_grad_point = 1 - (track_y - self._rect.y) / self._rect.height - - # speed up: 120, slow down: 0 - path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120) - - saturation = min(abs(self._acceleration_x[i] * 1.5), 1) - lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62]) - alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0]) - - # Use HSL to RGB conversion - color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) - - gradient_stops.append(lin_grad_point) - segment_colors.append(color) - - # Skip a point, unless next is last - i += 1 + (1 if (i + 2) < max_len else 0) - - # Store the gradient in the path object - self._exp_gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=segment_colors, - stops=gradient_stops, - ) - - def _update_lead_vehicle(self, d_rel, v_rel, point, rect): - speed_buff, lead_buff = 10.0, 40.0 - - # Calculate fill alpha - fill_alpha = 0 - if d_rel < lead_buff: - fill_alpha = 255 * (1.0 - (d_rel / lead_buff)) - if v_rel < 0: - fill_alpha += 255 * (-1 * (v_rel / speed_buff)) - fill_alpha = min(fill_alpha, 255) - - # Calculate size and position - sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35 - x = np.clip(point[0], 0.0, rect.width - sz / 2) - y = min(point[1], rect.height - sz * 0.6) - - g_xo = sz / 5 - g_yo = sz / 10 - - glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)] - chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)] - - return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha)) - - def _draw_lane_lines(self): - """Draw lane lines and road edges""" - for i, lane_line in enumerate(self._lane_lines): - if lane_line.projected_points.size == 0: - continue - - alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) - color = rl.Color(255, 255, 255, int(alpha * 255)) - draw_polygon(self._rect, lane_line.projected_points, color) - - for i, road_edge in enumerate(self._road_edges): - if road_edge.projected_points.size == 0: - continue - - alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) - color = rl.Color(255, 0, 0, int(alpha * 255)) - draw_polygon(self._rect, road_edge.projected_points, color) - - def _draw_path(self, sm): - """Draw path with dynamic coloring based on mode and throttle state.""" - if not self._path.projected_points.size: - return - - allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control - self._blend_filter.update(int(allow_throttle)) - - if self._experimental_mode: - # Draw with acceleration coloring - if len(self._exp_gradient.colors) > 1: - draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) - else: - draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) - else: - # Blend throttle/no throttle colors based on transition - blend_factor = round(self._blend_filter.x * 100) / 100 - blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) - gradient = Gradient( - start=(0.0, 1.0), # Bottom of path - end=(0.0, 0.0), # Top of path - colors=blended_colors, - stops=[0.0, 0.5, 1.0], - ) - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - - def _draw_lead_indicator(self): - # Draw lead vehicles if available - for lead in self._lead_vehicles: - if not lead.glow or not lead.chevron: - continue - - rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) - rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha)) - - @staticmethod - def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int: - """Get the index corresponding to the given path distance""" - if len(pos_x_array) == 0: - return 0 - indices = np.where(pos_x_array <= path_distance)[0] - return indices[-1] if indices.size > 0 else 0 - - def _map_to_screen(self, in_x, in_y, in_z): - """Project a point in car space to screen space""" - input_pt = np.array([in_x, in_y, in_z]) - pt = self._car_space_transform @ input_pt - - if abs(pt[2]) < 1e-6: - return None - - x, y = pt[0] / pt[2], pt[1] / pt[2] - - clip = self._clip_region - if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height): - return None - - return (x, y) - - def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, max_distance: float, allow_invert: bool = True) -> np.ndarray: - """Convert 3D line to 2D polygon for rendering.""" - if line.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) - - # Slice points and filter non-negative x-coordinates - points = line[:max_idx + 1] - - # Interpolate around max_idx so path end is smooth (max_distance is always >= p0.x) - if 0 < max_idx < line.shape[0] - 1: - p0 = line[max_idx] - p1 = line[max_idx + 1] - x0, x1 = p0[0], p1[0] - interp_y = np.interp(max_distance, [x0, x1], [p0[1], p1[1]]) - interp_z = np.interp(max_distance, [x0, x1], [p0[2], p1[2]]) - interp_point = np.array([max_distance, interp_y, interp_z], dtype=points.dtype) - points = np.concatenate((points, interp_point[None, :]), axis=0) - - points = points[points[:, 0] >= 0] - if points.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) - - N = points.shape[0] - # Generate left and right 3D points in one array using broadcasting - offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32) - points_3d = points[None, :, :] + offsets[:, None, :] # Shape: 2xNx3 - points_3d = points_3d.reshape(2 * N, 3) # Shape: (2*N)x3 - - # Transform all points to projected space in one operation - proj = self._car_space_transform @ points_3d.T # Shape: 3x(2*N) - proj = proj.reshape(3, 2, N) - left_proj = proj[:, 0, :] - right_proj = proj[:, 1, :] - - # Filter points where z is sufficiently large - valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6) - if not np.any(valid_proj): - return np.empty((0, 2), dtype=np.float32) - - # Compute screen coordinates - left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :] - right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :] - - # Define clip region bounds - clip = self._clip_region - x_min, x_max = clip.x, clip.x + clip.width - y_min, y_max = clip.y, clip.y + clip.height - - # Filter points within clip region - left_in_clip = ( - (left_screen[0] >= x_min) & (left_screen[0] <= x_max) & - (left_screen[1] >= y_min) & (left_screen[1] <= y_max) - ) - right_in_clip = ( - (right_screen[0] >= x_min) & (right_screen[0] <= x_max) & - (right_screen[1] >= y_min) & (right_screen[1] <= y_max) - ) - both_in_clip = left_in_clip & right_in_clip - - if not np.any(both_in_clip): - return np.empty((0, 2), dtype=np.float32) - - # Select valid and clipped points - left_screen = left_screen[:, both_in_clip] - right_screen = right_screen[:, both_in_clip] - - # Handle Y-coordinate inversion on hills - if not allow_invert and left_screen.shape[1] > 1: - y = left_screen[1, :] # y-coordinates - keep = y == np.minimum.accumulate(y) - if not np.any(keep): - return np.empty((0, 2), dtype=np.float32) - left_screen = left_screen[:, keep] - right_screen = right_screen[:, keep] - - return np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32) - - @staticmethod - def _hsla_to_color(h, s, l, a): - rgb = colorsys.hls_to_rgb(h, l, s) - return rl.Color( - int(rgb[0] * 255), - int(rgb[1] * 255), - int(rgb[2] * 255), - int(a * 255) - ) - - @staticmethod - def _blend_colors(begin_colors, end_colors, t): - if t >= 1.0: - return end_colors - if t <= 0.0: - return begin_colors - - inv_t = 1.0 - t - return [rl.Color( - int(inv_t * start.r + t * end.r), - int(inv_t * start.g + t * end.g), - int(inv_t * start.b + t * end.b), - int(inv_t * start.a + t * end.a) - ) for start, end in zip(begin_colors, end_colors, strict=True)] diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc new file mode 100644 index 00000000000000..84e1a4032e88c2 --- /dev/null +++ b/selfdrive/ui/qt/api.cc @@ -0,0 +1,140 @@ +#include "selfdrive/ui/qt/api.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "common/util.h" +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/util.h" + +namespace CommaApi { + +QByteArray rsa_sign(const QByteArray &data) { + static std::string key = util::read_file(Path::rsa_file()); + if (key.empty()) { + qDebug() << "No RSA private key found, please run manager.py or registration.py"; + return {}; + } + + BIO* mem = BIO_new_mem_buf(key.data(), key.size()); + assert(mem); + RSA* rsa_private = PEM_read_bio_RSAPrivateKey(mem, NULL, NULL, NULL); + assert(rsa_private); + auto sig = QByteArray(); + sig.resize(RSA_size(rsa_private)); + unsigned int sig_len; + int ret = RSA_sign(NID_sha256, (unsigned char*)data.data(), data.size(), (unsigned char*)sig.data(), &sig_len, rsa_private); + assert(ret == 1); + assert(sig_len == sig.size()); + BIO_free(mem); + RSA_free(rsa_private); + return sig; +} + +QString create_jwt(const QJsonObject &payloads, int expiry) { + QJsonObject header = {{"alg", "RS256"}}; + + auto t = QDateTime::currentSecsSinceEpoch(); + QJsonObject payload = {{"identity", getDongleId().value_or("")}, {"nbf", t}, {"iat", t}, {"exp", t + expiry}}; + for (auto it = payloads.begin(); it != payloads.end(); ++it) { + payload.insert(it.key(), it.value()); + } + + auto b64_opts = QByteArray::Base64UrlEncoding | QByteArray::OmitTrailingEquals; + QString jwt = QJsonDocument(header).toJson(QJsonDocument::Compact).toBase64(b64_opts) + '.' + + QJsonDocument(payload).toJson(QJsonDocument::Compact).toBase64(b64_opts); + + auto hash = QCryptographicHash::hash(jwt.toUtf8(), QCryptographicHash::Sha256); + auto sig = rsa_sign(hash); + jwt += '.' + sig.toBase64(b64_opts); + return jwt; +} + +} // namespace CommaApi + +HttpRequest::HttpRequest(QObject *parent, bool create_jwt, int timeout) : create_jwt(create_jwt), QObject(parent) { + networkTimer = new QTimer(this); + networkTimer->setSingleShot(true); + networkTimer->setInterval(timeout); + connect(networkTimer, &QTimer::timeout, this, &HttpRequest::requestTimeout); +} + +bool HttpRequest::active() const { + return reply != nullptr; +} + +bool HttpRequest::timeout() const { + return reply && reply->error() == QNetworkReply::OperationCanceledError; +} + +void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Method method) { + if (active()) { + qDebug() << "HttpRequest is active"; + return; + } + QString token; + if(create_jwt) { + token = CommaApi::create_jwt(); + } else { + QString token_json = QString::fromStdString(util::read_file(util::getenv("HOME") + "/.comma/auth.json")); + QJsonDocument json_d = QJsonDocument::fromJson(token_json.toUtf8()); + token = json_d["access_token"].toString(); + } + + QNetworkRequest request; + request.setUrl(QUrl(requestURL)); + request.setRawHeader("User-Agent", getUserAgent().toUtf8()); + + if (!token.isEmpty()) { + request.setRawHeader(QByteArray("Authorization"), ("JWT " + token).toUtf8()); + } + + if (method == HttpRequest::Method::GET) { + reply = nam()->get(request); + } else if (method == HttpRequest::Method::DELETE) { + reply = nam()->deleteResource(request); + } + + networkTimer->start(); + connect(reply, &QNetworkReply::finished, this, &HttpRequest::requestFinished); +} + +void HttpRequest::requestTimeout() { + reply->abort(); +} + +void HttpRequest::requestFinished() { + networkTimer->stop(); + + if (reply->error() == QNetworkReply::NoError) { + emit requestDone(reply->readAll(), true, reply->error()); + } else { + QString error; + if (reply->error() == QNetworkReply::OperationCanceledError) { + nam()->clearAccessCache(); + nam()->clearConnectionCache(); + error = "Request timed out"; + } else { + error = reply->errorString(); + } + emit requestDone(error, false, reply->error()); + } + + reply->deleteLater(); + reply = nullptr; +} + +QNetworkAccessManager *HttpRequest::nam() { + static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp); + return networkAccessManager; +} diff --git a/tools/cabana/utils/api.h b/selfdrive/ui/qt/api.h similarity index 100% rename from tools/cabana/utils/api.h rename to selfdrive/ui/qt/api.h diff --git a/selfdrive/ui/qt/body.cc b/selfdrive/ui/qt/body.cc new file mode 100644 index 00000000000000..f2628d304f36b5 --- /dev/null +++ b/selfdrive/ui/qt/body.cc @@ -0,0 +1,161 @@ +#include "selfdrive/ui/qt/body.h" + +#include +#include + +#include +#include + +#include "common/params.h" +#include "common/timing.h" + +RecordButton::RecordButton(QWidget *parent) : QPushButton(parent) { + setCheckable(true); + setChecked(false); + setFixedSize(148, 148); + + QObject::connect(this, &QPushButton::toggled, [=]() { + setEnabled(false); + }); +} + +void RecordButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + QPoint center(width() / 2, height() / 2); + + QColor bg(isChecked() ? "#FFFFFF" : "#737373"); + QColor accent(isChecked() ? "#FF0000" : "#FFFFFF"); + if (!isEnabled()) { + bg = QColor("#404040"); + accent = QColor("#FFFFFF"); + } + + if (isDown()) { + accent.setAlphaF(0.7); + } + + p.setPen(Qt::NoPen); + p.setBrush(bg); + p.drawEllipse(center, 74, 74); + + p.setPen(QPen(accent, 6)); + p.setBrush(Qt::NoBrush); + p.drawEllipse(center, 42, 42); + + p.setPen(Qt::NoPen); + p.setBrush(accent); + p.drawEllipse(center, 22, 22); +} + + +BodyWindow::BodyWindow(QWidget *parent) : fuel_filter(1.0, 5., 1. / UI_FREQ), QWidget(parent) { + QStackedLayout *layout = new QStackedLayout(this); + layout->setStackingMode(QStackedLayout::StackAll); + + QWidget *w = new QWidget; + QVBoxLayout *vlayout = new QVBoxLayout(w); + vlayout->setMargin(45); + layout->addWidget(w); + + // face + face = new QLabel(); + face->setAlignment(Qt::AlignCenter); + layout->addWidget(face); + awake = new QMovie("../assets/body/awake.gif"); + awake->setCacheMode(QMovie::CacheAll); + sleep = new QMovie("../assets/body/sleep.gif"); + sleep->setCacheMode(QMovie::CacheAll); + + // record button + btn = new RecordButton(this); + vlayout->addWidget(btn, 0, Qt::AlignBottom | Qt::AlignRight); + QObject::connect(btn, &QPushButton::clicked, [=](bool checked) { + btn->setEnabled(false); + Params().putBool("DisableLogging", !checked); + last_button = nanos_since_boot(); + }); + w->raise(); + + QObject::connect(uiState(), &UIState::uiUpdate, this, &BodyWindow::updateState); +} + +void BodyWindow::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + p.fillRect(rect(), QColor(0, 0, 0)); + + // battery outline + detail + p.translate(width() - 136, 16); + const QColor gray = QColor("#737373"); + p.setBrush(Qt::NoBrush); + p.setPen(QPen(gray, 4, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + p.drawRoundedRect(2, 2, 78, 36, 8, 8); + + p.setPen(Qt::NoPen); + p.setBrush(gray); + p.drawRoundedRect(84, 12, 6, 16, 4, 4); + p.drawRect(84, 12, 3, 16); + + // battery level + double fuel = std::clamp(fuel_filter.x(), 0.2f, 1.0f); + const int m = 5; // manual margin since we can't do an inner border + p.setPen(Qt::NoPen); + p.setBrush(fuel > 0.25 ? QColor("#32D74B") : QColor("#FF453A")); + p.drawRoundedRect(2 + m, 2 + m, (78 - 2*m)*fuel, 36 - 2*m, 4, 4); + + // charging status + if (charging) { + p.setPen(Qt::NoPen); + p.setBrush(Qt::white); + const QPolygonF charger({ + QPointF(12.31, 0), + QPointF(12.31, 16.92), + QPointF(18.46, 16.92), + QPointF(6.15, 40), + QPointF(6.15, 23.08), + QPointF(0, 23.08), + }); + p.drawPolygon(charger.translated(98, 0)); + } +} + +void BodyWindow::offroadTransition(bool offroad) { + btn->setChecked(true); + btn->setEnabled(true); + fuel_filter.reset(1.0); +} + +void BodyWindow::updateState(const UIState &s) { + if (!isVisible()) { + return; + } + + const SubMaster &sm = *(s.sm); + auto cs = sm["carState"].getCarState(); + + charging = cs.getCharging(); + fuel_filter.update(cs.getFuelGauge()); + + // TODO: use carState.standstill when that's fixed + const bool standstill = std::abs(cs.getVEgo()) < 0.01; + QMovie *m = standstill ? sleep : awake; + if (m != face->movie()) { + face->setMovie(m); + face->movie()->start(); + } + + // update record button state + if (sm.updated("managerState") && (sm.rcv_time("managerState") - last_button)*1e-9 > 0.5) { + for (auto proc : sm["managerState"].getManagerState().getProcesses()) { + if (proc.getName() == "loggerd") { + btn->setEnabled(true); + btn->setChecked(proc.getRunning()); + } + } + } + + update(); +} diff --git a/selfdrive/ui/qt/body.h b/selfdrive/ui/qt/body.h new file mode 100644 index 00000000000000..567a54d49befd5 --- /dev/null +++ b/selfdrive/ui/qt/body.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +#include "common/util.h" +#include "selfdrive/ui/ui.h" + +class RecordButton : public QPushButton { + Q_OBJECT + +public: + RecordButton(QWidget* parent = 0); + +private: + void paintEvent(QPaintEvent*) override; +}; + +class BodyWindow : public QWidget { + Q_OBJECT + +public: + BodyWindow(QWidget* parent = 0); + +private: + bool charging = false; + uint64_t last_button = 0; + FirstOrderFilter fuel_filter; + QLabel *face; + QMovie *awake, *sleep; + RecordButton *btn; + void paintEvent(QPaintEvent*) override; + +private slots: + void updateState(const UIState &s); + void offroadTransition(bool onroad); +}; diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc new file mode 100644 index 00000000000000..0edeb252b7e5cc --- /dev/null +++ b/selfdrive/ui/qt/home.cc @@ -0,0 +1,207 @@ +#include "selfdrive/ui/qt/home.h" + +#include +#include +#include +#include + +#include "common/params.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/widgets/drive_stats.h" +#include "selfdrive/ui/qt/widgets/prime.h" + +// HomeWindow: the container for the offroad and onroad UIs + +HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { + QHBoxLayout *main_layout = new QHBoxLayout(this); + main_layout->setMargin(0); + main_layout->setSpacing(0); + + sidebar = new Sidebar(this); + main_layout->addWidget(sidebar); + QObject::connect(sidebar, &Sidebar::openSettings, this, &HomeWindow::openSettings); + + slayout = new QStackedLayout(); + main_layout->addLayout(slayout); + + home = new OffroadHome(); + slayout->addWidget(home); + + onroad = new OnroadWindow(this); + slayout->addWidget(onroad); + + body = new BodyWindow(this); + slayout->addWidget(body); + + driver_view = new DriverViewWindow(this); + connect(driver_view, &DriverViewWindow::done, [=] { + showDriverView(false); + }); + slayout->addWidget(driver_view); + setAttribute(Qt::WA_NoSystemBackground); + QObject::connect(uiState(), &UIState::uiUpdate, this, &HomeWindow::updateState); + QObject::connect(uiState(), &UIState::offroadTransition, this, &HomeWindow::offroadTransition); +} + +void HomeWindow::showSidebar(bool show) { + sidebar->setVisible(show); +} + +void HomeWindow::updateState(const UIState &s) { + const SubMaster &sm = *(s.sm); + + // switch to the generic robot UI + if (onroad->isVisible() && !body->isEnabled() && sm["carParams"].getCarParams().getNotCar()) { + body->setEnabled(true); + slayout->setCurrentWidget(body); + } +} + +void HomeWindow::offroadTransition(bool offroad) { + body->setEnabled(false); + sidebar->setVisible(offroad); + if (offroad) { + slayout->setCurrentWidget(home); + } else { + slayout->setCurrentWidget(onroad); + } +} + +void HomeWindow::showDriverView(bool show) { + if (show) { + emit closeSettings(); + slayout->setCurrentWidget(driver_view); + } else { + slayout->setCurrentWidget(home); + } + sidebar->setVisible(show == false); +} + +void HomeWindow::mousePressEvent(QMouseEvent* e) { + // Handle sidebar collapsing + if ((onroad->isVisible() || body->isVisible()) && (!sidebar->isVisible() || e->x() > sidebar->width())) { + sidebar->setVisible(!sidebar->isVisible() && !onroad->isMapVisible()); + } +} + +void HomeWindow::mouseDoubleClickEvent(QMouseEvent* e) { + HomeWindow::mousePressEvent(e); + const SubMaster &sm = *(uiState()->sm); + if (sm["carParams"].getCarParams().getNotCar()) { + if (onroad->isVisible()) { + slayout->setCurrentWidget(body); + } else if (body->isVisible()) { + slayout->setCurrentWidget(onroad); + } + showSidebar(false); + } +} + +// OffroadHome: the offroad home page + +OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) { + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(40, 40, 40, 45); + + // top header + QHBoxLayout* header_layout = new QHBoxLayout(); + header_layout->setContentsMargins(15, 15, 15, 0); + header_layout->setSpacing(16); + + date = new QLabel(); + header_layout->addWidget(date, 1, Qt::AlignHCenter | Qt::AlignLeft); + + update_notif = new QPushButton(tr("UPDATE")); + update_notif->setVisible(false); + update_notif->setStyleSheet("background-color: #364DEF;"); + QObject::connect(update_notif, &QPushButton::clicked, [=]() { center_layout->setCurrentIndex(1); }); + header_layout->addWidget(update_notif, 0, Qt::AlignHCenter | Qt::AlignRight); + + alert_notif = new QPushButton(); + alert_notif->setVisible(false); + alert_notif->setStyleSheet("background-color: #E22C2C;"); + QObject::connect(alert_notif, &QPushButton::clicked, [=] { center_layout->setCurrentIndex(2); }); + header_layout->addWidget(alert_notif, 0, Qt::AlignHCenter | Qt::AlignRight); + + header_layout->addWidget(new QLabel(getBrandVersion()), 0, Qt::AlignHCenter | Qt::AlignRight); + + main_layout->addLayout(header_layout); + + // main content + main_layout->addSpacing(25); + center_layout = new QStackedLayout(); + + QWidget* statsAndSetupWidget = new QWidget(this); + QHBoxLayout* statsAndSetup = new QHBoxLayout(statsAndSetupWidget); + statsAndSetup->setMargin(0); + statsAndSetup->setSpacing(30); + statsAndSetup->addWidget(new DriveStats, 1); + statsAndSetup->addWidget(new SetupWidget); + + center_layout->addWidget(statsAndSetupWidget); + + // add update & alerts widgets + update_widget = new UpdateAlert(); + QObject::connect(update_widget, &UpdateAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); }); + center_layout->addWidget(update_widget); + alerts_widget = new OffroadAlert(); + QObject::connect(alerts_widget, &OffroadAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); }); + center_layout->addWidget(alerts_widget); + + main_layout->addLayout(center_layout, 1); + + // set up refresh timer + timer = new QTimer(this); + timer->callOnTimeout(this, &OffroadHome::refresh); + + setStyleSheet(R"( + * { + color: white; + } + OffroadHome { + background-color: black; + } + OffroadHome > QPushButton { + padding: 15px 30px; + border-radius: 5px; + font-size: 40px; + font-weight: 500; + } + OffroadHome > QLabel { + font-size: 55px; + } + )"); +} + +void OffroadHome::showEvent(QShowEvent *event) { + refresh(); + timer->start(10 * 1000); +} + +void OffroadHome::hideEvent(QHideEvent *event) { + timer->stop(); +} + +void OffroadHome::refresh() { + date->setText(QDateTime::currentDateTime().toString("dddd, MMMM d")); + + bool updateAvailable = update_widget->refresh(); + int alerts = alerts_widget->refresh(); + + // pop-up new notification + int idx = center_layout->currentIndex(); + if (!updateAvailable && !alerts) { + idx = 0; + } else if (updateAvailable && (!update_notif->isVisible() || (!alerts && idx == 2))) { + idx = 1; + } else if (alerts && (!alert_notif->isVisible() || (!updateAvailable && idx == 1))) { + idx = 2; + } + center_layout->setCurrentIndex(idx); + + update_notif->setVisible(updateAvailable); + alert_notif->setVisible(alerts); + if (alerts) { + alert_notif->setText(QString::number(alerts) + (alerts > 1 ? tr(" ALERTS") : tr(" ALERT"))); + } +} diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h new file mode 100644 index 00000000000000..94f1330109d053 --- /dev/null +++ b/selfdrive/ui/qt/home.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/offroad/driverview.h" +#include "selfdrive/ui/qt/body.h" +#include "selfdrive/ui/qt/onroad.h" +#include "selfdrive/ui/qt/sidebar.h" +#include "selfdrive/ui/qt/widgets/offroad_alerts.h" +#include "selfdrive/ui/ui.h" + +class OffroadHome : public QFrame { + Q_OBJECT + +public: + explicit OffroadHome(QWidget* parent = 0); + +private: + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + void refresh(); + + QTimer* timer; + QLabel* date; + QStackedLayout* center_layout; + UpdateAlert *update_widget; + OffroadAlert* alerts_widget; + QPushButton* alert_notif; + QPushButton* update_notif; +}; + +class HomeWindow : public QWidget { + Q_OBJECT + +public: + explicit HomeWindow(QWidget* parent = 0); + +signals: + void openSettings(); + void closeSettings(); + +public slots: + void offroadTransition(bool offroad); + void showDriverView(bool show); + void showSidebar(bool show); + +protected: + void mousePressEvent(QMouseEvent* e) override; + void mouseDoubleClickEvent(QMouseEvent* e) override; + +private: + Sidebar *sidebar; + OffroadHome *home; + OnroadWindow *onroad; + BodyWindow *body; + DriverViewWindow *driver_view; + QStackedLayout *slayout; + +private slots: + void updateState(const UIState &s); +}; diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc new file mode 100644 index 00000000000000..71706fd35e0d35 --- /dev/null +++ b/selfdrive/ui/qt/maps/map.cc @@ -0,0 +1,687 @@ +#include "selfdrive/ui/qt/maps/map.h" + +#include +#include + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/transformations/coordinates.hpp" +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/ui.h" + + +const int PAN_TIMEOUT = 100; +const float MANEUVER_TRANSITION_THRESHOLD = 10; + +const float MAX_ZOOM = 17; +const float MIN_ZOOM = 14; +const float MAX_PITCH = 50; +const float MIN_PITCH = 0; +const float MAP_SCALE = 2; + +const float VALID_POS_STD = 50.0; // m + +const QString ICON_SUFFIX = ".png"; + +MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { + QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState); + + // Instructions + map_instructions = new MapInstructions(this); + QObject::connect(this, &MapWindow::instructionsChanged, map_instructions, &MapInstructions::updateInstructions); + QObject::connect(this, &MapWindow::distanceChanged, map_instructions, &MapInstructions::updateDistance); + map_instructions->setFixedWidth(width()); + map_instructions->setVisible(false); + + map_eta = new MapETA(this); + QObject::connect(this, &MapWindow::ETAChanged, map_eta, &MapETA::updateETA); + + const int h = 120; + map_eta->setFixedHeight(h); + map_eta->move(25, 1080 - h - bdr_s*2); + map_eta->setVisible(false); + + auto last_gps_position = coordinate_from_param("LastGPSPosition"); + if (last_gps_position) { + last_position = *last_gps_position; + } + + grabGesture(Qt::GestureType::PinchGesture); + qDebug() << "MapWindow initialized"; +} + +MapWindow::~MapWindow() { + makeCurrent(); +} + +void MapWindow::initLayers() { + // This doesn't work from initializeGL + if (!m_map->layerExists("modelPathLayer")) { + qDebug() << "Initializing modelPathLayer"; + QVariantMap modelPath; + modelPath["id"] = "modelPathLayer"; + modelPath["type"] = "line"; + modelPath["source"] = "modelPathSource"; + m_map->addLayer(modelPath); + m_map->setPaintProperty("modelPathLayer", "line-color", QColor("red")); + m_map->setPaintProperty("modelPathLayer", "line-width", 5.0); + m_map->setLayoutProperty("modelPathLayer", "line-cap", "round"); + } + if (!m_map->layerExists("navLayer")) { + qDebug() << "Initializing navLayer"; + QVariantMap nav; + nav["id"] = "navLayer"; + nav["type"] = "line"; + nav["source"] = "navSource"; + m_map->addLayer(nav, "road-intersection"); + m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee")); + m_map->setPaintProperty("navLayer", "line-width", 7.5); + m_map->setLayoutProperty("navLayer", "line-cap", "round"); + } + if (!m_map->layerExists("carPosLayer")) { + qDebug() << "Initializing carPosLayer"; + m_map->addImage("label-arrow", QImage("../assets/images/triangle.svg")); + + QVariantMap carPos; + carPos["id"] = "carPosLayer"; + carPos["type"] = "symbol"; + carPos["source"] = "carPosSource"; + m_map->addLayer(carPos); + m_map->setLayoutProperty("carPosLayer", "icon-pitch-alignment", "map"); + m_map->setLayoutProperty("carPosLayer", "icon-image", "label-arrow"); + m_map->setLayoutProperty("carPosLayer", "icon-size", 0.5); + m_map->setLayoutProperty("carPosLayer", "icon-ignore-placement", true); + m_map->setLayoutProperty("carPosLayer", "icon-allow-overlap", true); + m_map->setLayoutProperty("carPosLayer", "symbol-sort-key", 0); + } +} + +void MapWindow::updateState(const UIState &s) { + if (!uiState()->scene.started) { + return; + } + const SubMaster &sm = *(s.sm); + update(); + + if (sm.updated("liveLocationKalman")) { + auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); + auto locationd_pos = locationd_location.getPositionGeodetic(); + auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); + auto locationd_velocity = locationd_location.getVelocityCalibrated(); + + locationd_valid = (locationd_location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && + locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid(); + + if (locationd_valid) { + last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]); + last_bearing = RAD2DEG(locationd_orientation.getValue()[2]); + velocity_filter.update(locationd_velocity.getValue()[0]); + } + } + + if (sm.updated("gnssMeasurements")) { + auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); + auto laikad_pos = laikad_location.getPositionECEF(); + auto laikad_pos_ecef = laikad_pos.getValue(); + auto laikad_pos_std = laikad_pos.getStd(); + auto laikad_velocity_ecef = laikad_location.getVelocityECEF().getValue(); + + laikad_valid = laikad_pos.getValid() && Eigen::Vector3d(laikad_pos_std[0], laikad_pos_std[1], laikad_pos_std[2]).norm() < VALID_POS_STD; + + if (laikad_valid && !locationd_valid) { + ECEF ecef = {.x = laikad_pos_ecef[0], .y = laikad_pos_ecef[1], .z = laikad_pos_ecef[2]}; + Geodetic laikad_pos_geodetic = ecef2geodetic(ecef); + last_position = QMapbox::Coordinate(laikad_pos_geodetic.lat, laikad_pos_geodetic.lon); + + // Compute NED velocity + LocalCoord converter(ecef); + ECEF next_ecef = {.x = ecef.x + laikad_velocity_ecef[0], .y = ecef.y + laikad_velocity_ecef[1], .z = ecef.z + laikad_velocity_ecef[2]}; + Eigen::VectorXd ned_vel = converter.ecef2ned(next_ecef).to_vector() - converter.ecef2ned(ecef).to_vector(); + + float velocity = ned_vel.norm(); + velocity_filter.update(velocity); + + // Convert NED velocity to angle + if (velocity > 1.0) { + float new_bearing = fmod(RAD2DEG(atan2(ned_vel[1], ned_vel[0])) + 360.0, 360.0); + if (last_bearing) { + float delta = 0.1 * angle_difference(*last_bearing, new_bearing); // Smooth heading + last_bearing = fmod(*last_bearing + delta + 360.0, 360.0); + } else { + last_bearing = new_bearing; + } + } + } + } + + if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) { + qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; + + // Only open the map on setting destination the first time + if (allow_open) { + setVisible(true); // Show map on destination set/change + allow_open = false; + } + } + + if (m_map.isNull()) { + return; + } + + loaded_once = loaded_once || m_map->isFullyLoaded(); + if (!loaded_once) { + map_instructions->showError(tr("Map Loading")); + return; + } + + initLayers(); + + if (locationd_valid || laikad_valid) { + map_instructions->noError(); + + // Update current location marker + auto point = coordinate_to_collection(*last_position); + QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {}); + QVariantMap carPosSource; + carPosSource["type"] = "geojson"; + carPosSource["data"] = QVariant::fromValue(feature1); + m_map->updateSource("carPosSource", carPosSource); + } else { + map_instructions->showError(tr("Waiting for GPS")); + } + + if (pan_counter == 0) { + if (last_position) m_map->setCoordinate(*last_position); + if (last_bearing) m_map->setBearing(*last_bearing); + } else { + pan_counter--; + } + + if (zoom_counter == 0) { + m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); + } else { + zoom_counter--; + } + + if (sm.updated("navInstruction")) { + if (sm.valid("navInstruction")) { + auto i = sm["navInstruction"].getNavInstruction(); + emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); + + if (locationd_valid || laikad_valid) { + m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance + emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged + emit instructionsChanged(i); + } + } else { + m_map->setPitch(MIN_PITCH); + clearRoute(); + } + } + + if (sm.rcv_frame("navRoute") != route_rcv_frame) { + qWarning() << "Updating navLayer with new route"; + auto route = sm["navRoute"].getNavRoute(); + auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates()); + QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); + QVariantMap navSource; + navSource["type"] = "geojson"; + navSource["data"] = QVariant::fromValue(feature); + m_map->updateSource("navSource", navSource); + m_map->setLayoutProperty("navLayer", "visibility", "visible"); + + route_rcv_frame = sm.rcv_frame("navRoute"); + } +} + +void MapWindow::resizeGL(int w, int h) { + m_map->resize(size() / MAP_SCALE); + map_instructions->setFixedWidth(width()); +} + +void MapWindow::initializeGL() { + m_map.reset(new QMapboxGL(this, m_settings, size(), 1)); + + if (last_position) { + m_map->setCoordinateZoom(*last_position, MAX_ZOOM); + } else { + m_map->setCoordinateZoom(QMapbox::Coordinate(64.31990695292795, -149.79038934046247), MIN_ZOOM); + } + + m_map->setMargins({0, 350, 0, 50}); + m_map->setPitch(MIN_PITCH); + m_map->setStyleUrl("mapbox://styles/commaai/ckr64tlwp0azb17nqvr9fj13s"); + + QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) { + if (change == QMapboxGL::MapChange::MapChangeDidFinishLoadingMap) { + loaded_once = true; + } + }); +} + +void MapWindow::paintGL() { + if (!isVisible() || m_map.isNull()) return; + m_map->render(); +} + +void MapWindow::clearRoute() { + if (!m_map.isNull()) { + m_map->setLayoutProperty("navLayer", "visibility", "none"); + m_map->setPitch(MIN_PITCH); + } + + map_instructions->hideIfNoError(); + map_eta->setVisible(false); + allow_open = true; +} + +void MapWindow::mousePressEvent(QMouseEvent *ev) { + m_lastPos = ev->localPos(); + ev->accept(); +} + +void MapWindow::mouseDoubleClickEvent(QMouseEvent *ev) { + if (last_position) m_map->setCoordinate(*last_position); + if (last_bearing) m_map->setBearing(*last_bearing); + m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); + update(); + + pan_counter = 0; + zoom_counter = 0; +} + +void MapWindow::mouseMoveEvent(QMouseEvent *ev) { + QPointF delta = ev->localPos() - m_lastPos; + + if (!delta.isNull()) { + pan_counter = PAN_TIMEOUT; + m_map->moveBy(delta / MAP_SCALE); + update(); + } + + m_lastPos = ev->localPos(); + ev->accept(); +} + +void MapWindow::wheelEvent(QWheelEvent *ev) { + if (ev->orientation() == Qt::Horizontal) { + return; + } + + float factor = ev->delta() / 1200.; + if (ev->delta() < 0) { + factor = factor > -1 ? factor : 1 / factor; + } + + m_map->scaleBy(1 + factor, ev->pos() / MAP_SCALE); + update(); + + zoom_counter = PAN_TIMEOUT; + ev->accept(); +} + +bool MapWindow::event(QEvent *event) { + if (event->type() == QEvent::Gesture) { + return gestureEvent(static_cast(event)); + } + + return QWidget::event(event); +} + +bool MapWindow::gestureEvent(QGestureEvent *event) { + if (QGesture *pinch = event->gesture(Qt::PinchGesture)) { + pinchTriggered(static_cast(pinch)); + } + return true; +} + +void MapWindow::pinchTriggered(QPinchGesture *gesture) { + QPinchGesture::ChangeFlags changeFlags = gesture->changeFlags(); + if (changeFlags & QPinchGesture::ScaleFactorChanged) { + // TODO: figure out why gesture centerPoint doesn't work + m_map->scaleBy(gesture->scaleFactor(), {width() / 2.0 / MAP_SCALE, height() / 2.0 / MAP_SCALE}); + update(); + zoom_counter = PAN_TIMEOUT; + } +} + +void MapWindow::offroadTransition(bool offroad) { + if (offroad) { + clearRoute(); + } else { + auto dest = coordinate_from_param("NavDestination"); + setVisible(dest.has_value()); + } + last_bearing = {}; +} + +MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) { + is_rhd = Params().getBool("IsRhdDetected"); + QHBoxLayout *main_layout = new QHBoxLayout(this); + main_layout->setContentsMargins(11, 50, 11, 11); + { + QVBoxLayout *layout = new QVBoxLayout; + icon_01 = new QLabel; + layout->addWidget(icon_01); + layout->addStretch(); + main_layout->addLayout(layout); + } + + { + QVBoxLayout *layout = new QVBoxLayout; + + distance = new QLabel; + distance->setStyleSheet(R"(font-size: 90px;)"); + layout->addWidget(distance); + + primary = new QLabel; + primary->setStyleSheet(R"(font-size: 60px;)"); + primary->setWordWrap(true); + layout->addWidget(primary); + + secondary = new QLabel; + secondary->setStyleSheet(R"(font-size: 50px;)"); + secondary->setWordWrap(true); + layout->addWidget(secondary); + + lane_widget = new QWidget; + lane_widget->setFixedHeight(125); + + lane_layout = new QHBoxLayout(lane_widget); + layout->addWidget(lane_widget); + + main_layout->addLayout(layout); + } + + setStyleSheet(R"( + * { + color: white; + font-family: "Inter"; + } + )"); + + QPalette pal = palette(); + pal.setColor(QPalette::Background, QColor(0, 0, 0, 150)); + setAutoFillBackground(true); + setPalette(pal); +} + +void MapInstructions::updateDistance(float d) { + d = std::max(d, 0.0f); + QString distance_str; + + if (uiState()->scene.is_metric) { + if (d > 500) { + distance_str.setNum(d / 1000, 'f', 1); + distance_str += tr(" km"); + } else { + distance_str.setNum(50 * int(d / 50)); + distance_str += tr(" m"); + } + } else { + float miles = d * METER_TO_MILE; + float feet = d * METER_TO_FOOT; + + if (feet > 500) { + distance_str.setNum(miles, 'f', 1); + distance_str += tr(" mi"); + } else { + distance_str.setNum(50 * int(feet / 50)); + distance_str += tr(" ft"); + } + } + + distance->setAlignment(Qt::AlignLeft); + distance->setText(distance_str); +} + +void MapInstructions::showError(QString error_text) { + primary->setText(""); + distance->setText(error_text); + distance->setAlignment(Qt::AlignCenter); + + secondary->setVisible(false); + icon_01->setVisible(false); + + this->error = true; + lane_widget->setVisible(false); + + setVisible(true); +} + +void MapInstructions::noError() { + error = false; +} + +void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) { + // Word wrap widgets need fixed width + primary->setFixedWidth(width() - 250); + secondary->setFixedWidth(width() - 250); + + + // Show instruction text + QString primary_str = QString::fromStdString(instruction.getManeuverPrimaryText()); + QString secondary_str = QString::fromStdString(instruction.getManeuverSecondaryText()); + + primary->setText(primary_str); + secondary->setVisible(secondary_str.length() > 0); + secondary->setText(secondary_str); + + // Show arrow with direction + QString type = QString::fromStdString(instruction.getManeuverType()); + QString modifier = QString::fromStdString(instruction.getManeuverModifier()); + if (!type.isEmpty()) { + QString fn = "../assets/navigation/direction_" + type; + if (!modifier.isEmpty()) { + fn += "_" + modifier; + } + fn += ICON_SUFFIX; + fn = fn.replace(' ', '_'); + + // for rhd, reflect direction and then flip + if (is_rhd) { + if (fn.contains("left")) { + fn.replace("left", "right"); + } else if (fn.contains("right")) { + fn.replace("right", "left"); + } + } + + QPixmap pix(fn); + if (is_rhd) { + pix = pix.transformed(QTransform().scale(-1, 1)); + } + icon_01->setPixmap(pix.scaledToWidth(200, Qt::SmoothTransformation)); + icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + icon_01->setVisible(true); + } + + // Show lanes + bool has_lanes = false; + clearLayout(lane_layout); + for (auto const &lane: instruction.getLanes()) { + has_lanes = true; + bool active = lane.getActive(); + + // TODO: only use active direction if active + bool left = false, straight = false, right = false; + for (auto const &direction: lane.getDirections()) { + left |= direction == cereal::NavInstruction::Direction::LEFT; + right |= direction == cereal::NavInstruction::Direction::RIGHT; + straight |= direction == cereal::NavInstruction::Direction::STRAIGHT; + } + + // TODO: Make more images based on active direction and combined directions + QString fn = "../assets/navigation/direction_"; + if (left) { + fn += "turn_left"; + } else if (right) { + fn += "turn_right"; + } else if (straight) { + fn += "turn_straight"; + } + + if (!active) { + fn += "_inactive"; + } + + auto icon = new QLabel; + icon->setPixmap(loadPixmap(fn + ICON_SUFFIX, {125, 125}, Qt::IgnoreAspectRatio)); + icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + lane_layout->addWidget(icon); + } + lane_widget->setVisible(has_lanes); + + show(); + resize(sizeHint()); +} + + +void MapInstructions::hideIfNoError() { + if (!error) { + hide(); + } +} + +MapETA::MapETA(QWidget * parent) : QWidget(parent) { + QHBoxLayout *main_layout = new QHBoxLayout(this); + main_layout->setContentsMargins(40, 25, 40, 25); + + { + QHBoxLayout *layout = new QHBoxLayout; + eta = new QLabel; + eta->setAlignment(Qt::AlignCenter); + eta->setStyleSheet("font-weight:600"); + + eta_unit = new QLabel; + eta_unit->setAlignment(Qt::AlignCenter); + + layout->addWidget(eta); + layout->addWidget(eta_unit); + main_layout->addLayout(layout); + } + main_layout->addSpacing(40); + { + QHBoxLayout *layout = new QHBoxLayout; + time = new QLabel; + time->setAlignment(Qt::AlignCenter); + + time_unit = new QLabel; + time_unit->setAlignment(Qt::AlignCenter); + + layout->addWidget(time); + layout->addWidget(time_unit); + main_layout->addLayout(layout); + } + main_layout->addSpacing(40); + { + QHBoxLayout *layout = new QHBoxLayout; + distance = new QLabel; + distance->setAlignment(Qt::AlignCenter); + distance->setStyleSheet("font-weight:600"); + + distance_unit = new QLabel; + distance_unit->setAlignment(Qt::AlignCenter); + + layout->addWidget(distance); + layout->addWidget(distance_unit); + main_layout->addLayout(layout); + } + + setStyleSheet(R"( + * { + color: white; + font-family: "Inter"; + font-size: 70px; + } + )"); + + QPalette pal = palette(); + pal.setColor(QPalette::Background, QColor(0, 0, 0, 150)); + setAutoFillBackground(true); + setPalette(pal); +} + + +void MapETA::updateETA(float s, float s_typical, float d) { + if (d < MANEUVER_TRANSITION_THRESHOLD) { + hide(); + return; + } + + // ETA + auto eta_time = QDateTime::currentDateTime().addSecs(s).time(); + if (params.getBool("NavSettingTime24h")) { + eta->setText(eta_time.toString("HH:mm")); + eta_unit->setText(tr("eta")); + } else { + auto t = eta_time.toString("h:mm a").split(' '); + eta->setText(t[0]); + eta_unit->setText(t[1]); + } + + // Remaining time + if (s < 3600) { + time->setText(QString::number(int(s / 60))); + time_unit->setText(tr("min")); + } else { + int hours = int(s) / 3600; + time->setText(QString::number(hours) + ":" + QString::number(int((s - hours * 3600) / 60)).rightJustified(2, '0')); + time_unit->setText(tr("hr")); + } + + QString color; + if (s / s_typical > 1.5) { + color = "#DA3025"; + } else if (s / s_typical > 1.2) { + color = "#DAA725"; + } else { + color = "#25DA6E"; + } + + time->setStyleSheet(QString(R"(color: %1; font-weight:600;)").arg(color)); + time_unit->setStyleSheet(QString(R"(color: %1;)").arg(color)); + + // Distance + QString distance_str; + float num = 0; + if (uiState()->scene.is_metric) { + num = d / 1000.0; + distance_unit->setText(tr("km")); + } else { + num = d * METER_TO_MILE; + distance_unit->setText(tr("mi")); + } + + distance_str.setNum(num, 'f', num < 100 ? 1 : 0); + distance->setText(distance_str); + + show(); + adjustSize(); + repaint(); + adjustSize(); + + // Rounded corners + const int radius = 25; + const auto r = rect(); + + // Top corners rounded + QPainterPath path; + path.setFillRule(Qt::WindingFill); + path.addRoundedRect(r, radius, radius); + + // Bottom corners not rounded + path.addRect(r.marginsRemoved(QMargins(0, radius, 0, 0))); + + // Set clipping mask + QRegion mask = QRegion(path.simplified().toFillPolygon().toPolygon()); + setMask(mask); + + // Center + move(static_cast(parent())->width() / 2 - width() / 2, 1080 - height() - bdr_s*2); +} diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h new file mode 100644 index 00000000000000..ecba867edb250c --- /dev/null +++ b/selfdrive/ui/qt/maps/map.h @@ -0,0 +1,127 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/params.h" +#include "common/util.h" +#include "selfdrive/ui/ui.h" + +class MapInstructions : public QWidget { + Q_OBJECT + +private: + QLabel *distance; + QLabel *primary; + QLabel *secondary; + QLabel *icon_01; + QWidget *lane_widget; + QHBoxLayout *lane_layout; + bool error = false; + bool is_rhd = false; + +public: + MapInstructions(QWidget * parent=nullptr); + void showError(QString error); + void noError(); + void hideIfNoError(); + +public slots: + void updateDistance(float d); + void updateInstructions(cereal::NavInstruction::Reader instruction); +}; + +class MapETA : public QWidget { + Q_OBJECT + +private: + QLabel *eta; + QLabel *eta_unit; + QLabel *time; + QLabel *time_unit; + QLabel *distance; + QLabel *distance_unit; + Params params; + +public: + MapETA(QWidget * parent=nullptr); + +public slots: + void updateETA(float seconds, float seconds_typical, float distance); +}; + +class MapWindow : public QOpenGLWidget { + Q_OBJECT + +public: + MapWindow(const QMapboxGLSettings &); + ~MapWindow(); + +private: + void initializeGL() final; + void paintGL() final; + void resizeGL(int w, int h) override; + + QMapboxGLSettings m_settings; + QScopedPointer m_map; + + void initLayers(); + + void mousePressEvent(QMouseEvent *ev) final; + void mouseDoubleClickEvent(QMouseEvent *ev) final; + void mouseMoveEvent(QMouseEvent *ev) final; + void wheelEvent(QWheelEvent *ev) final; + bool event(QEvent *event) final; + bool gestureEvent(QGestureEvent *event); + void pinchTriggered(QPinchGesture *gesture); + + bool m_sourceAdded = false; + + bool loaded_once = false; + bool allow_open = true; + + // Panning + QPointF m_lastPos; + int pan_counter = 0; + int zoom_counter = 0; + + // Position + std::optional last_position; + std::optional last_bearing; + FirstOrderFilter velocity_filter; + bool laikad_valid = false; + bool locationd_valid = false; + + MapInstructions* map_instructions; + MapETA* map_eta; + + void clearRoute(); + uint64_t route_rcv_frame = 0; + +private slots: + void updateState(const UIState &s); + +public slots: + void offroadTransition(bool offroad); + +signals: + void distanceChanged(float distance); + void instructionsChanged(cereal::NavInstruction::Reader instruction); + void ETAChanged(float seconds, float seconds_typical, float distance); +}; + diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc new file mode 100644 index 00000000000000..8d5d4e1715816d --- /dev/null +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -0,0 +1,135 @@ +#include "selfdrive/ui/qt/maps/map_helpers.h" + +#include +#include + +#include "common/params.h" +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/api.h" + +QString get_mapbox_token() { + // Valid for 4 weeks since we can't swap tokens on the fly + return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN; +} + +QMapboxGLSettings get_mapbox_settings() { + QMapboxGLSettings settings; + + if (!Hardware::PC()) { + settings.setCacheDatabasePath(MAPS_CACHE_PATH); + } + settings.setApiBaseUrl(MAPS_HOST); + settings.setAccessToken(get_mapbox_token()); + + return settings; +} + +QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) { + return QGeoCoordinate(in.first, in.second); +} + +QMapbox::CoordinatesCollections model_to_collection( + const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, + const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, + const cereal::ModelDataV2::XYZTData::Reader &line){ + + Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]); + Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]); + Eigen::Matrix3d ecef_from_local = euler2rot(orient); + + QMapbox::Coordinates coordinates; + auto x = line.getX(); + auto y = line.getY(); + auto z = line.getZ(); + for (int i = 0; i < x.size(); i++) { + Eigen::Vector3d point_ecef = ecef_from_local * Eigen::Vector3d(x[i], y[i], z[i]) + ecef; + Geodetic point_geodetic = ecef2geodetic((ECEF){.x = point_ecef[0], .y = point_ecef[1], .z = point_ecef[2]}); + coordinates.push_back({point_geodetic.lat, point_geodetic.lon}); + } + + return {QMapbox::CoordinatesCollection{coordinates}}; +} + +QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c) { + QMapbox::Coordinates coordinates{c}; + return {QMapbox::CoordinatesCollection{coordinates}}; +} + +QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader& coordinate_list) { + QMapbox::Coordinates coordinates; + for (auto const &c: coordinate_list) { + coordinates.push_back({c.getLatitude(), c.getLongitude()}); + } + return {QMapbox::CoordinatesCollection{coordinates}}; +} + +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list) { + QMapbox::Coordinates coordinates; + for (auto &c : coordinate_list) { + coordinates.push_back({c.latitude(), c.longitude()}); + } + return {QMapbox::CoordinatesCollection{coordinates}}; +} + +QList polyline_to_coordinate_list(const QString &polylineString) { + QList path; + if (polylineString.isEmpty()) + return path; + + QByteArray data = polylineString.toLatin1(); + + bool parsingLatitude = true; + + int shift = 0; + int value = 0; + + QGeoCoordinate coord(0, 0); + + for (int i = 0; i < data.length(); ++i) { + unsigned char c = data.at(i) - 63; + + value |= (c & 0x1f) << shift; + shift += 5; + + // another chunk + if (c & 0x20) + continue; + + int diff = (value & 1) ? ~(value >> 1) : (value >> 1); + + if (parsingLatitude) { + coord.setLatitude(coord.latitude() + (double)diff/1e6); + } else { + coord.setLongitude(coord.longitude() + (double)diff/1e6); + path.append(coord); + } + + parsingLatitude = !parsingLatitude; + + value = 0; + shift = 0; + } + + return path; +} + +std::optional coordinate_from_param(const std::string ¶m) { + QString json_str = QString::fromStdString(Params().get(param)); + if (json_str.isEmpty()) return {}; + + QJsonDocument doc = QJsonDocument::fromJson(json_str.toUtf8()); + if (doc.isNull()) return {}; + + QJsonObject json = doc.object(); + if (json["latitude"].isDouble() && json["longitude"].isDouble()) { + QMapbox::Coordinate coord(json["latitude"].toDouble(), json["longitude"].toDouble()); + return coord; + } else { + return {}; + } +} + +double angle_difference(double angle1, double angle2) { + double diff = fmod(angle2 - angle1 + 180.0, 360.0) - 180.0; + return diff < -180.0 ? diff + 360.0 : diff; +} diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h new file mode 100644 index 00000000000000..6bd5b0f067f638 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include + +#include "common/util.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "cereal/messaging/messaging.h" + +const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str(); +const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str(); +const QString MAPS_CACHE_PATH = "/data/mbgl-cache-navd.db"; + +QString get_mapbox_token(); +QMapboxGLSettings get_mapbox_settings(); +QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in); +QMapbox::CoordinatesCollections model_to_collection( + const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, + const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, + const cereal::ModelDataV2::XYZTData::Reader &line); +QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c); +QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list); +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list); +QList polyline_to_coordinate_list(const QString &polylineString); + +std::optional coordinate_from_param(const std::string ¶m); +double angle_difference(double angle1, double angle2); diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc new file mode 100644 index 00000000000000..d143b44e70e373 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_settings.cc @@ -0,0 +1,294 @@ +#include "map_settings.h" + +#include + +#include "common/util.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + +static QString shorten(const QString &str, int max_len) { + return str.size() > max_len ? str.left(max_len).trimmed() + "…" : str; +} + +MapPanel::MapPanel(QWidget* parent) : QWidget(parent) { + stack = new QStackedWidget; + + QWidget * main_widget = new QWidget; + QVBoxLayout *main_layout = new QVBoxLayout(main_widget); + const int icon_size = 200; + + // Home + QHBoxLayout *home_layout = new QHBoxLayout; + home_button = new QPushButton; + home_button->setIconSize(QSize(icon_size, icon_size)); + home_layout->addWidget(home_button); + + home_address = new QLabel; + home_address->setWordWrap(true); + home_layout->addSpacing(30); + home_layout->addWidget(home_address); + home_layout->addStretch(); + + // Work + QHBoxLayout *work_layout = new QHBoxLayout; + work_button = new QPushButton; + work_button->setIconSize(QSize(icon_size, icon_size)); + work_layout->addWidget(work_button); + + work_address = new QLabel; + work_address->setWordWrap(true); + work_layout->addSpacing(30); + work_layout->addWidget(work_address); + work_layout->addStretch(); + + // Home & Work layout + QHBoxLayout *home_work_layout = new QHBoxLayout; + home_work_layout->addLayout(home_layout, 1); + home_work_layout->addSpacing(50); + home_work_layout->addLayout(work_layout, 1); + + main_layout->addLayout(home_work_layout); + main_layout->addSpacing(20); + main_layout->addWidget(horizontal_line()); + main_layout->addSpacing(20); + + // Current route + { + current_widget = new QWidget(this); + QVBoxLayout *current_layout = new QVBoxLayout(current_widget); + + QLabel *title = new QLabel(tr("Current Destination")); + title->setStyleSheet("font-size: 55px"); + current_layout->addWidget(title); + + current_route = new ButtonControl("", tr("CLEAR")); + current_route->setStyleSheet("padding-left: 40px;"); + current_layout->addWidget(current_route); + QObject::connect(current_route, &ButtonControl::clicked, [=]() { + params.remove("NavDestination"); + updateCurrentRoute(); + }); + + current_layout->addSpacing(10); + current_layout->addWidget(horizontal_line()); + current_layout->addSpacing(20); + } + main_layout->addWidget(current_widget); + + // Recents + QLabel *recents_title = new QLabel(tr("Recent Destinations")); + recents_title->setStyleSheet("font-size: 55px"); + main_layout->addWidget(recents_title); + main_layout->addSpacing(20); + + recent_layout = new QVBoxLayout; + QWidget *recent_widget = new LayoutWidget(recent_layout, this); + ScrollView *recent_scroller = new ScrollView(recent_widget, this); + main_layout->addWidget(recent_scroller); + + // No prime upsell + QWidget * no_prime_widget = new QWidget; + { + QVBoxLayout *no_prime_layout = new QVBoxLayout(no_prime_widget); + QLabel *signup_header = new QLabel(tr("Try the Navigation Beta")); + signup_header->setStyleSheet(R"(font-size: 75px; color: white; font-weight:600;)"); + signup_header->setAlignment(Qt::AlignCenter); + + no_prime_layout->addWidget(signup_header); + no_prime_layout->addSpacing(50); + + QLabel *screenshot = new QLabel; + QPixmap pm = QPixmap("../assets/navigation/screenshot.png"); + screenshot->setPixmap(pm.scaledToWidth(1080, Qt::SmoothTransformation)); + no_prime_layout->addWidget(screenshot, 0, Qt::AlignHCenter); + + QLabel *signup = new QLabel(tr("Get turn-by-turn directions displayed and more with a comma\nprime subscription. Sign up now: https://connect.comma.ai")); + signup->setStyleSheet(R"(font-size: 45px; color: white; font-weight:300;)"); + signup->setAlignment(Qt::AlignCenter); + + no_prime_layout->addSpacing(20); + no_prime_layout->addWidget(signup); + no_prime_layout->addStretch(); + } + + stack->addWidget(main_widget); + stack->addWidget(no_prime_widget); + stack->setCurrentIndex(uiState()->prime_type ? 0 : 1); + + QVBoxLayout *wrapper = new QVBoxLayout(this); + wrapper->addWidget(stack); + + + clear(); + + if (auto dongle_id = getDongleId()) { + // Fetch favorite and recent locations + { + QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/locations"; + RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_NavDestinations", 30, true); + QObject::connect(repeater, &RequestRepeater::requestDone, this, &MapPanel::parseResponse); + } + + // Destination set while offline + { + QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/next"; + RequestRepeater* repeater = new RequestRepeater(this, url, "", 10, true); + HttpRequest* deleter = new HttpRequest(this); + + QObject::connect(repeater, &RequestRepeater::requestDone, [=](const QString &resp, bool success) { + if (success && resp != "null") { + if (params.get("NavDestination").empty()) { + qWarning() << "Setting NavDestination from /next" << resp; + params.put("NavDestination", resp.toStdString()); + } else { + qWarning() << "Got location from /next, but NavDestination already set"; + } + + // Send DELETE to clear destination server side + deleter->sendRequest(url, HttpRequest::Method::DELETE); + } + }); + } + } +} + +void MapPanel::showEvent(QShowEvent *event) { + updateCurrentRoute(); +} + +void MapPanel::clear() { + home_button->setIcon(QPixmap("../assets/navigation/home_inactive.png")); + home_address->setStyleSheet(R"(font-size: 50px; color: grey;)"); + home_address->setText(tr("No home\nlocation set")); + home_button->disconnect(); + + work_button->setIcon(QPixmap("../assets/navigation/work_inactive.png")); + work_address->setStyleSheet(R"(font-size: 50px; color: grey;)"); + work_address->setText(tr("No work\nlocation set")); + work_button->disconnect(); + + clearLayout(recent_layout); +} + +void MapPanel::updateCurrentRoute() { + auto dest = QString::fromStdString(params.get("NavDestination")); + QJsonDocument doc = QJsonDocument::fromJson(dest.trimmed().toUtf8()); + if (dest.size() && !doc.isNull()) { + auto name = doc["place_name"].toString(); + auto details = doc["place_details"].toString(); + current_route->setTitle(shorten(name + " " + details, 42)); + } + current_widget->setVisible(dest.size() && !doc.isNull()); +} + +void MapPanel::parseResponse(const QString &response, bool success) { + stack->setCurrentIndex((uiState()->prime_type || success) ? 0 : 1); + + if (!success) { + return; + } + + QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on navigation locations"; + return; + } + + clear(); + + bool has_recents = false; + for (auto &save_type: {"favorite", "recent"}) { + for (auto location : doc.array()) { + auto obj = location.toObject(); + + auto type = obj["save_type"].toString(); + auto label = obj["label"].toString(); + auto name = obj["place_name"].toString(); + auto details = obj["place_details"].toString(); + + if (type != save_type) continue; + + if (type == "favorite" && label == "home") { + home_address->setText(name); + home_address->setStyleSheet(R"(font-size: 50px; color: white;)"); + home_button->setIcon(QPixmap("../assets/navigation/home.png")); + QObject::connect(home_button, &QPushButton::clicked, [=]() { + navigateTo(obj); + emit closeSettings(); + }); + } else if (type == "favorite" && label == "work") { + work_address->setText(name); + work_address->setStyleSheet(R"(font-size: 50px; color: white;)"); + work_button->setIcon(QPixmap("../assets/navigation/work.png")); + QObject::connect(work_button, &QPushButton::clicked, [=]() { + navigateTo(obj); + emit closeSettings(); + }); + } else { + ClickableWidget *widget = new ClickableWidget; + QHBoxLayout *layout = new QHBoxLayout(widget); + layout->setContentsMargins(15, 14, 40, 14); + + QLabel *star = new QLabel("★"); + auto sp = star->sizePolicy(); + sp.setRetainSizeWhenHidden(true); + star->setSizePolicy(sp); + + star->setVisible(type == "favorite"); + star->setStyleSheet(R"(font-size: 60px;)"); + layout->addWidget(star); + layout->addSpacing(10); + + + QLabel *recent_label = new QLabel(shorten(name + " " + details, 45)); + recent_label->setStyleSheet(R"(font-size: 50px;)"); + + layout->addWidget(recent_label); + layout->addStretch(); + + QLabel *arrow = new QLabel("→"); + arrow->setStyleSheet(R"(font-size: 60px;)"); + layout->addWidget(arrow); + + widget->setStyleSheet(R"( + .ClickableWidget { + border-radius: 10px; + border-width: 1px; + border-style: solid; + border-color: gray; + } + QWidget { + background-color: #393939; + color: #9c9c9c; + } + )"); + + QObject::connect(widget, &ClickableWidget::clicked, [=]() { + navigateTo(obj); + emit closeSettings(); + }); + + recent_layout->addWidget(widget); + recent_layout->addSpacing(10); + has_recents = true; + } + } + + } + + if (!has_recents) { + QLabel *no_recents = new QLabel(tr("no recent destinations")); + no_recents->setStyleSheet(R"(font-size: 50px; color: #9c9c9c)"); + recent_layout->addWidget(no_recents); + } + + recent_layout->addStretch(); + repaint(); +} + +void MapPanel::navigateTo(const QJsonObject &place) { + QJsonDocument doc(place); + params.put("NavDestination", doc.toJson().toStdString()); +} diff --git a/selfdrive/ui/qt/maps/map_settings.h b/selfdrive/ui/qt/maps/map_settings.h new file mode 100644 index 00000000000000..962d12767942c1 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_settings.h @@ -0,0 +1,37 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "selfdrive/ui/qt/widgets/controls.h" + +class MapPanel : public QWidget { + Q_OBJECT +public: + explicit MapPanel(QWidget* parent = nullptr); + + void navigateTo(const QJsonObject &place); + void parseResponse(const QString &response, bool success); + void updateCurrentRoute(); + void clear(); + +private: + void showEvent(QShowEvent *event) override; + + Params params; + QStackedWidget *stack; + QPushButton *home_button, *work_button; + QLabel *home_address, *work_address; + QVBoxLayout *recent_layout; + QWidget *current_widget; + ButtonControl *current_route; + +signals: + void closeSettings(); +}; diff --git a/selfdrive/ui/qt/maps/set_destination.py b/selfdrive/ui/qt/maps/set_destination.py new file mode 100755 index 00000000000000..b9721171cc967a --- /dev/null +++ b/selfdrive/ui/qt/maps/set_destination.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python3 +import json +import sys + +from common.params import Params + +if __name__ == "__main__": + coords = sys.argv[1].split("/@")[-1].split("/")[0].split(",") + dest = {"latitude": float(coords[0]), "longitude": float(coords[1])} + Params().put("NavDestination", json.dumps(dest)) diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc new file mode 100644 index 00000000000000..be8b84d45e9910 --- /dev/null +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -0,0 +1,91 @@ +#include "selfdrive/ui/qt/offroad/driverview.h" + +#include + +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/util.h" + +const int FACE_IMG_SIZE = 130; + +DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + layout = new QStackedLayout(this); + layout->setStackingMode(QStackedLayout::StackAll); + + cameraView = new CameraViewWidget("camerad", VISION_STREAM_DRIVER, true, this); + layout->addWidget(cameraView); + + scene = new DriverViewScene(this); + connect(cameraView, &CameraViewWidget::vipcThreadFrameReceived, scene, &DriverViewScene::frameUpdated); + layout->addWidget(scene); + layout->setCurrentWidget(scene); +} + +void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { + emit done(); +} + +DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) { + face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); +} + +void DriverViewScene::showEvent(QShowEvent* event) { + frame_updated = false; + params.putBool("IsDriverViewEnabled", true); +} + +void DriverViewScene::hideEvent(QHideEvent* event) { + params.putBool("IsDriverViewEnabled", false); +} + +void DriverViewScene::frameUpdated() { + frame_updated = true; + sm.update(0); + update(); +} + +void DriverViewScene::paintEvent(QPaintEvent* event) { + QPainter p(this); + + // startup msg + if (!frame_updated) { + p.setPen(Qt::white); + p.setRenderHint(QPainter::TextAntialiasing); + configFont(p, "Inter", 100, "Bold"); + p.drawText(geometry(), Qt::AlignCenter, tr("camera starting")); + return; + } + + cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2(); + cereal::DriverStateV2::DriverData::Reader driver_data; + + is_rhd = driver_state.getWheelOnRightProb() > 0.5; + driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData(); + + bool face_detected = driver_data.getFaceProb() > 0.7; + if (face_detected) { + auto fxy_list = driver_data.getFacePosition(); + auto std_list = driver_data.getFaceOrientationStd(); + float face_x = fxy_list[0]; + float face_y = fxy_list[1]; + float face_std = std::max(std_list[0], std_list[1]); + + float alpha = 0.7; + if (face_std > 0.15) { + alpha = std::max(0.7 - (face_std-0.15)*3.5, 0.0); + } + const int box_size = 220; + // use approx instead of distort_points + int fbox_x = 1080.0 - 1714.0 * face_x; + int fbox_y = -135.0 + (504.0 + std::abs(face_x)*112.0) + (1205.0 - std::abs(face_x)*724.0) * face_y; + p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10)); + p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0); + } + + // icon + const int img_offset = 60; + const int img_x = is_rhd ? rect().right() - FACE_IMG_SIZE - img_offset : rect().left() + img_offset; + const int img_y = rect().bottom() - FACE_IMG_SIZE - img_offset; + p.setOpacity(face_detected ? 1.0 : 0.2); + p.drawPixmap(img_x, img_y, face_img); +} diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h new file mode 100644 index 00000000000000..5d090ad7725715 --- /dev/null +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include + +#include "common/util.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + +class DriverViewScene : public QWidget { + Q_OBJECT + +public: + explicit DriverViewScene(QWidget *parent); + +public slots: + void frameUpdated(); + +protected: + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + void paintEvent(QPaintEvent *event) override; + +private: + Params params; + SubMaster sm; + QPixmap face_img; + bool is_rhd = false; + bool frame_updated = false; +}; + +class DriverViewWindow : public QWidget { + Q_OBJECT + +public: + explicit DriverViewWindow(QWidget *parent); + +signals: + void done(); + +protected: + void mouseReleaseEvent(QMouseEvent* e) override; + +private: + CameraViewWidget *cameraView; + DriverViewScene *scene; + QStackedLayout *layout; +}; diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc new file mode 100644 index 00000000000000..c7341d1987c886 --- /dev/null +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -0,0 +1,332 @@ +#include "selfdrive/ui/qt/offroad/networking.h" + +#include + +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + + +// Networking functions + +Networking::Networking(QWidget* parent, bool show_advanced) : QFrame(parent) { + main_layout = new QStackedLayout(this); + + wifi = new WifiManager(this); + connect(wifi, &WifiManager::refreshSignal, this, &Networking::refresh); + connect(wifi, &WifiManager::wrongPassword, this, &Networking::wrongPassword); + + wifiScreen = new QWidget(this); + QVBoxLayout* vlayout = new QVBoxLayout(wifiScreen); + vlayout->setContentsMargins(20, 20, 20, 20); + if (show_advanced) { + QPushButton* advancedSettings = new QPushButton(tr("Advanced")); + advancedSettings->setObjectName("advanced_btn"); + advancedSettings->setStyleSheet("margin-right: 30px;"); + advancedSettings->setFixedSize(400, 100); + connect(advancedSettings, &QPushButton::clicked, [=]() { main_layout->setCurrentWidget(an); }); + vlayout->addSpacing(10); + vlayout->addWidget(advancedSettings, 0, Qt::AlignRight); + vlayout->addSpacing(10); + } + + wifiWidget = new WifiUI(this, wifi); + wifiWidget->setObjectName("wifiWidget"); + connect(wifiWidget, &WifiUI::connectToNetwork, this, &Networking::connectToNetwork); + + ScrollView *wifiScroller = new ScrollView(wifiWidget, this); + wifiScroller->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + vlayout->addWidget(wifiScroller, 1); + main_layout->addWidget(wifiScreen); + + an = new AdvancedNetworking(this, wifi); + connect(an, &AdvancedNetworking::backPress, [=]() { main_layout->setCurrentWidget(wifiScreen); }); + main_layout->addWidget(an); + + QPalette pal = palette(); + pal.setColor(QPalette::Window, QColor(0x29, 0x29, 0x29)); + setAutoFillBackground(true); + setPalette(pal); + + setStyleSheet(R"( + #wifiWidget > QPushButton, #back_btn, #advanced_btn { + font-size: 50px; + margin: 0px; + padding: 15px; + border-width: 0; + border-radius: 30px; + color: #dddddd; + background-color: #393939; + } + #back_btn:pressed, #advanced_btn:pressed { + background-color: #4a4a4a; + } + )"); + main_layout->setCurrentWidget(wifiScreen); +} + +void Networking::refresh() { + wifiWidget->refresh(); + an->refresh(); +} + +void Networking::connectToNetwork(const Network &n) { + if (wifi->isKnownConnection(n.ssid)) { + wifi->activateWifiConnection(n.ssid); + wifiWidget->refresh(); + } else if (n.security_type == SecurityType::OPEN) { + wifi->connect(n); + } else if (n.security_type == SecurityType::WPA) { + QString pass = InputDialog::getText(tr("Enter password"), this, tr("for \"%1\"").arg(QString::fromUtf8(n.ssid)), true, 8); + if (!pass.isEmpty()) { + wifi->connect(n, pass); + } + } +} + +void Networking::wrongPassword(const QString &ssid) { + if (wifi->seenNetworks.contains(ssid)) { + const Network &n = wifi->seenNetworks.value(ssid); + QString pass = InputDialog::getText(tr("Wrong password"), this, tr("for \"%1\"").arg(QString::fromUtf8(n.ssid)), true, 8); + if (!pass.isEmpty()) { + wifi->connect(n, pass); + } + } +} + +void Networking::showEvent(QShowEvent *event) { + wifi->start(); +} + +void Networking::hideEvent(QHideEvent *event) { + wifi->stop(); +} + +// AdvancedNetworking functions + +AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWidget(parent), wifi(wifi) { + + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setMargin(40); + main_layout->setSpacing(20); + + // Back button + QPushButton* back = new QPushButton(tr("Back")); + back->setObjectName("back_btn"); + back->setFixedSize(400, 100); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + main_layout->addWidget(back, 0, Qt::AlignLeft); + + ListWidget *list = new ListWidget(this); + // Enable tethering layout + tetheringToggle = new ToggleControl(tr("Enable Tethering"), "", "", wifi->isTetheringEnabled()); + list->addItem(tetheringToggle); + QObject::connect(tetheringToggle, &ToggleControl::toggleFlipped, this, &AdvancedNetworking::toggleTethering); + + // Change tethering password + ButtonControl *editPasswordButton = new ButtonControl(tr("Tethering Password"), tr("EDIT")); + connect(editPasswordButton, &ButtonControl::clicked, [=]() { + QString pass = InputDialog::getText(tr("Enter new tethering password"), this, "", true, 8, wifi->getTetheringPassword()); + if (!pass.isEmpty()) { + wifi->changeTetheringPassword(pass); + } + }); + list->addItem(editPasswordButton); + + // IP address + ipLabel = new LabelControl(tr("IP Address"), wifi->ipv4_address); + list->addItem(ipLabel); + + // SSH keys + list->addItem(new SshToggle()); + list->addItem(new SshControl()); + + // Roaming toggle + const bool roamingEnabled = params.getBool("GsmRoaming"); + ToggleControl *roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); + QObject::connect(roamingToggle, &SshToggle::toggleFlipped, [=](bool state) { + params.putBool("GsmRoaming", state); + wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn"))); + }); + list->addItem(roamingToggle); + + // APN settings + ButtonControl *editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); + connect(editApnButton, &ButtonControl::clicked, [=]() { + const bool roamingEnabled = params.getBool("GsmRoaming"); + const QString cur_apn = QString::fromStdString(params.get("GsmApn")); + QString apn = InputDialog::getText(tr("Enter APN"), this, tr("leave blank for automatic configuration"), false, -1, cur_apn).trimmed(); + + if (apn.isEmpty()) { + params.remove("GsmApn"); + } else { + params.put("GsmApn", apn.toStdString()); + } + wifi->updateGsmSettings(roamingEnabled, apn); + }); + list->addItem(editApnButton); + + // Set initial config + wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn"))); + + main_layout->addWidget(new ScrollView(list, this)); + main_layout->addStretch(1); +} + +void AdvancedNetworking::refresh() { + ipLabel->setText(wifi->ipv4_address); + tetheringToggle->setEnabled(true); + update(); +} + +void AdvancedNetworking::toggleTethering(bool enabled) { + wifi->setTetheringEnabled(enabled); + tetheringToggle->setEnabled(false); +} + +// WifiUI functions + +WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) { + main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + + // load imgs + for (const auto &s : {"low", "medium", "high", "full"}) { + QPixmap pix(ASSET_PATH + "/offroad/icon_wifi_strength_" + s + ".svg"); + strengths.push_back(pix.scaledToHeight(68, Qt::SmoothTransformation)); + } + lock = QPixmap(ASSET_PATH + "offroad/icon_lock_closed.svg").scaledToWidth(49, Qt::SmoothTransformation); + checkmark = QPixmap(ASSET_PATH + "offroad/icon_checkmark.svg").scaledToWidth(49, Qt::SmoothTransformation); + circled_slash = QPixmap(ASSET_PATH + "img_circled_slash.svg").scaledToWidth(49, Qt::SmoothTransformation); + + QLabel *scanning = new QLabel(tr("Scanning for networks...")); + scanning->setStyleSheet("font-size: 65px;"); + main_layout->addWidget(scanning, 0, Qt::AlignCenter); + + setStyleSheet(R"( + QScrollBar::handle:vertical { + min-height: 0px; + border-radius: 4px; + background-color: #8A8A8A; + } + #forgetBtn { + font-size: 32px; + font-weight: 600; + color: #292929; + background-color: #BDBDBD; + border-width: 1px solid #828282; + border-radius: 5px; + padding: 40px; + padding-bottom: 16px; + padding-top: 16px; + } + #connecting { + font-size: 32px; + font-weight: 600; + color: white; + border-radius: 0; + padding: 27px; + padding-left: 43px; + padding-right: 43px; + background-color: black; + } + #ssidLabel { + font-size: 55px; + font-weight: 300; + text-align: left; + border: none; + padding-top: 50px; + padding-bottom: 50px; + } + #ssidLabel[disconnected=false] { + font-weight: 500; + } + #ssidLabel:disabled { + color: #696969; + } + )"); +} + +void WifiUI::refresh() { + // TODO: don't rebuild this every time + clearLayout(main_layout); + + if (wifi->seenNetworks.size() == 0) { + QLabel *scanning = new QLabel(tr("Scanning for networks...")); + scanning->setStyleSheet("font-size: 65px;"); + main_layout->addWidget(scanning, 0, Qt::AlignCenter); + return; + } + QList sortedNetworks = wifi->seenNetworks.values(); + std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength); + + // add networks + ListWidget *list = new ListWidget(this); + for (Network &network : sortedNetworks) { + QHBoxLayout *hlayout = new QHBoxLayout; + hlayout->setContentsMargins(44, 0, 73, 0); + hlayout->setSpacing(50); + + // Clickable SSID label + ElidedLabel *ssidLabel = new ElidedLabel(network.ssid); + ssidLabel->setObjectName("ssidLabel"); + ssidLabel->setEnabled(network.security_type != SecurityType::UNSUPPORTED); + ssidLabel->setProperty("disconnected", network.connected == ConnectedType::DISCONNECTED); + if (network.connected == ConnectedType::DISCONNECTED) { + QObject::connect(ssidLabel, &ElidedLabel::clicked, this, [=]() { emit connectToNetwork(network); }); + } + hlayout->addWidget(ssidLabel, network.connected == ConnectedType::CONNECTING ? 0 : 1); + + if (network.connected == ConnectedType::CONNECTING) { + QPushButton *connecting = new QPushButton(tr("CONNECTING...")); + connecting->setObjectName("connecting"); + hlayout->addWidget(connecting, 2, Qt::AlignLeft); + } + + // Forget button + if (wifi->isKnownConnection(network.ssid) && !wifi->isTetheringEnabled()) { + QPushButton *forgetBtn = new QPushButton(tr("FORGET")); + forgetBtn->setObjectName("forgetBtn"); + QObject::connect(forgetBtn, &QPushButton::clicked, [=]() { + if (ConfirmationDialog::confirm(tr("Forget Wi-Fi Network \"%1\"?").arg(QString::fromUtf8(network.ssid)), this)) { + wifi->forgetConnection(network.ssid); + } + }); + hlayout->addWidget(forgetBtn, 0, Qt::AlignRight); + } + + // Status icon + if (network.connected == ConnectedType::CONNECTED) { + QLabel *connectIcon = new QLabel(); + connectIcon->setPixmap(checkmark); + hlayout->addWidget(connectIcon, 0, Qt::AlignRight); + } else if (network.security_type == SecurityType::UNSUPPORTED) { + QLabel *unsupportedIcon = new QLabel(); + unsupportedIcon->setPixmap(circled_slash); + hlayout->addWidget(unsupportedIcon, 0, Qt::AlignRight); + } else if (network.security_type == SecurityType::WPA) { + QLabel *lockIcon = new QLabel(); + lockIcon->setPixmap(lock); + hlayout->addWidget(lockIcon, 0, Qt::AlignRight); + } else { + hlayout->addSpacing(lock.width() + hlayout->spacing()); + } + + // Strength indicator + QLabel *strength = new QLabel(); + strength->setPixmap(strengths[std::clamp((int)round(network.strength / 33.), 0, 3)]); + hlayout->addWidget(strength, 0, Qt::AlignRight); + + list->addItem(hlayout); + } + main_layout->addWidget(list); + main_layout->addStretch(1); +} diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h new file mode 100644 index 00000000000000..e78d65ef0f1b48 --- /dev/null +++ b/selfdrive/ui/qt/offroad/networking.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include + +#include "selfdrive/ui/qt/offroad/wifiManager.h" +#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/widgets/ssh_keys.h" +#include "selfdrive/ui/qt/widgets/toggle.h" + +class WifiUI : public QWidget { + Q_OBJECT + +public: + explicit WifiUI(QWidget *parent = 0, WifiManager* wifi = 0); + +private: + WifiManager *wifi = nullptr; + QVBoxLayout* main_layout; + QPixmap lock; + QPixmap checkmark; + QPixmap circled_slash; + QVector strengths; + +signals: + void connectToNetwork(const Network &n); + +public slots: + void refresh(); +}; + +class AdvancedNetworking : public QWidget { + Q_OBJECT +public: + explicit AdvancedNetworking(QWidget* parent = 0, WifiManager* wifi = 0); + +private: + LabelControl* ipLabel; + ToggleControl* tetheringToggle; + WifiManager* wifi = nullptr; + Params params; + +signals: + void backPress(); + +public slots: + void toggleTethering(bool enabled); + void refresh(); +}; + +class Networking : public QFrame { + Q_OBJECT + +public: + explicit Networking(QWidget* parent = 0, bool show_advanced = true); + WifiManager* wifi = nullptr; + +private: + QStackedLayout* main_layout = nullptr; + QWidget* wifiScreen = nullptr; + AdvancedNetworking* an = nullptr; + + WifiUI* wifiWidget; + +protected: + void showEvent(QShowEvent* event) override; + void hideEvent(QHideEvent* event) override; + +public slots: + void refresh(); + +private slots: + void connectToNetwork(const Network &n); + void wrongPassword(const QString &ssid); +}; diff --git a/selfdrive/ui/qt/offroad/networkmanager.h b/selfdrive/ui/qt/offroad/networkmanager.h new file mode 100644 index 00000000000000..52d85c16afffb6 --- /dev/null +++ b/selfdrive/ui/qt/offroad/networkmanager.h @@ -0,0 +1,38 @@ +/** + * We are using a NetworkManager DBUS API : https://developer.gnome.org/NetworkManager/1.26/spec.html + * */ + +// https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApFlags +const int NM_802_11_AP_FLAGS_NONE = 0x00000000; +const int NM_802_11_AP_FLAGS_PRIVACY = 0x00000001; +const int NM_802_11_AP_FLAGS_WPS = 0x00000002; + +// https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApSecurityFlags +const int NM_802_11_AP_SEC_PAIR_WEP40 = 0x00000001; +const int NM_802_11_AP_SEC_PAIR_WEP104 = 0x00000002; +const int NM_802_11_AP_SEC_GROUP_WEP40 = 0x00000010; +const int NM_802_11_AP_SEC_GROUP_WEP104 = 0x00000020; +const int NM_802_11_AP_SEC_KEY_MGMT_PSK = 0x00000100; +const int NM_802_11_AP_SEC_KEY_MGMT_802_1X = 0x00000200; + +const QString NM_DBUS_PATH = "/org/freedesktop/NetworkManager"; +const QString NM_DBUS_PATH_SETTINGS = "/org/freedesktop/NetworkManager/Settings"; + +const QString NM_DBUS_INTERFACE = "org.freedesktop.NetworkManager"; +const QString NM_DBUS_INTERFACE_PROPERTIES = "org.freedesktop.DBus.Properties"; +const QString NM_DBUS_INTERFACE_SETTINGS = "org.freedesktop.NetworkManager.Settings"; +const QString NM_DBUS_INTERFACE_SETTINGS_CONNECTION = "org.freedesktop.NetworkManager.Settings.Connection"; +const QString NM_DBUS_INTERFACE_DEVICE = "org.freedesktop.NetworkManager.Device"; +const QString NM_DBUS_INTERFACE_DEVICE_WIRELESS = "org.freedesktop.NetworkManager.Device.Wireless"; +const QString NM_DBUS_INTERFACE_ACCESS_POINT = "org.freedesktop.NetworkManager.AccessPoint"; +const QString NM_DBUS_INTERFACE_ACTIVE_CONNECTION = "org.freedesktop.NetworkManager.Connection.Active"; +const QString NM_DBUS_INTERFACE_IP4_CONFIG = "org.freedesktop.NetworkManager.IP4Config"; + +const QString NM_DBUS_SERVICE = "org.freedesktop.NetworkManager"; + +const int NM_DEVICE_STATE_ACTIVATED = 100; +const int NM_DEVICE_STATE_NEED_AUTH = 60; +const int NM_DEVICE_TYPE_WIFI = 2; +const int NM_DEVICE_TYPE_MODEM = 8; +const int NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT = 8; +const int DBUS_TIMEOUT = 100; diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc new file mode 100644 index 00000000000000..f3e50b572baf72 --- /dev/null +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -0,0 +1,213 @@ +#include "selfdrive/ui/qt/offroad/onboarding.h" + +#include +#include +#include +#include +#include + +#include "common/util.h" +#include "common/params.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/widgets/input.h" + +TrainingGuide::TrainingGuide(QWidget *parent) : QFrame(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); +} + +void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) { + if (click_timer.elapsed() < 250) { + return; + } + click_timer.restart(); + + if (boundingRect[currentIndex].contains(e->x(), e->y())) { + if (currentIndex == 9) { + const QRect yes = QRect(707, 804, 531, 164); + Params().putBool("RecordFront", yes.contains(e->x(), e->y())); + } + currentIndex += 1; + } else if (currentIndex == (boundingRect.size() - 2) && boundingRect.last().contains(e->x(), e->y())) { + currentIndex = 0; + } + + if (currentIndex >= (boundingRect.size() - 1)) { + emit completedTraining(); + } else { + image.load(img_path + "step" + QString::number(currentIndex) + ".png"); + update(); + } +} + +void TrainingGuide::showEvent(QShowEvent *event) { + img_path = width() == WIDE_WIDTH ? "../assets/training_wide/" : "../assets/training/"; + boundingRect = width() == WIDE_WIDTH ? boundingRectWide : boundingRectStandard; + + currentIndex = 0; + image.load(img_path + "step0.png"); + click_timer.start(); +} + +void TrainingGuide::paintEvent(QPaintEvent *event) { + QPainter painter(this); + + QRect bg(0, 0, painter.device()->width(), painter.device()->height()); + painter.fillRect(bg, QColor("#000000")); + + QRect rect(image.rect()); + rect.moveCenter(bg.center()); + painter.drawImage(rect.topLeft(), image); + + // progress bar + if (currentIndex > 0 && currentIndex < (boundingRect.size() - 2)) { + const int h = 20; + const int w = (currentIndex / (float)(boundingRect.size() - 2)) * width(); + painter.fillRect(QRect(0, height() - h, w, h), QColor("#465BEA")); + } +} + +void TermsPage::showEvent(QShowEvent *event) { + // late init, building QML widget takes 200ms + if (layout()) { + return; + } + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(45, 35, 45, 45); + main_layout->setSpacing(0); + + QLabel *title = new QLabel(tr("Terms & Conditions")); + title->setStyleSheet("font-size: 90px; font-weight: 600;"); + main_layout->addWidget(title); + + main_layout->addSpacing(30); + + QQuickWidget *text = new QQuickWidget(this); + text->setResizeMode(QQuickWidget::SizeRootObjectToView); + text->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + text->setAttribute(Qt::WA_AlwaysStackOnTop); + text->setClearColor(QColor("#1B1B1B")); + + QString text_view = util::read_file("../assets/offroad/tc.html").c_str(); + text->rootContext()->setContextProperty("text_view", text_view); + + text->setSource(QUrl::fromLocalFile("qt/offroad/text_view.qml")); + + main_layout->addWidget(text, 1); + main_layout->addSpacing(50); + + QObject *obj = (QObject*)text->rootObject(); + QObject::connect(obj, SIGNAL(scroll()), SLOT(enableAccept())); + + QHBoxLayout* buttons = new QHBoxLayout; + buttons->setMargin(0); + buttons->setSpacing(45); + main_layout->addLayout(buttons); + + QPushButton *decline_btn = new QPushButton(tr("Decline")); + buttons->addWidget(decline_btn); + QObject::connect(decline_btn, &QPushButton::clicked, this, &TermsPage::declinedTerms); + + accept_btn = new QPushButton(tr("Scroll to accept")); + accept_btn->setEnabled(false); + accept_btn->setStyleSheet(R"( + QPushButton { + background-color: #465BEA; + } + QPushButton:disabled { + background-color: #4F4F4F; + } + )"); + buttons->addWidget(accept_btn); + QObject::connect(accept_btn, &QPushButton::clicked, this, &TermsPage::acceptedTerms); +} + +void TermsPage::enableAccept() { + accept_btn->setText(tr("Agree")); + accept_btn->setEnabled(true); +} + +void DeclinePage::showEvent(QShowEvent *event) { + if (layout()) { + return; + } + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(45); + main_layout->setSpacing(40); + + QLabel *text = new QLabel(this); + text->setText(tr("You must accept the Terms and Conditions in order to use openpilot.")); + text->setStyleSheet(R"(font-size: 80px; font-weight: 300; margin: 200px;)"); + text->setWordWrap(true); + main_layout->addWidget(text, 0, Qt::AlignCenter); + + QHBoxLayout* buttons = new QHBoxLayout; + buttons->setSpacing(45); + main_layout->addLayout(buttons); + + QPushButton *back_btn = new QPushButton(tr("Back")); + buttons->addWidget(back_btn); + + QObject::connect(back_btn, &QPushButton::clicked, this, &DeclinePage::getBack); + + QPushButton *uninstall_btn = new QPushButton(tr("Decline, uninstall %1").arg(getBrand())); + uninstall_btn->setStyleSheet("background-color: #B73D3D"); + buttons->addWidget(uninstall_btn); + QObject::connect(uninstall_btn, &QPushButton::clicked, [=]() { + Params().putBool("DoUninstall", true); + }); +} + +void OnboardingWindow::updateActiveScreen() { + if (!accepted_terms) { + setCurrentIndex(0); + } else if (!training_done && !params.getBool("Passive")) { + setCurrentIndex(1); + } else { + emit onboardingDone(); + } +} + +OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) { + std::string current_terms_version = params.get("TermsVersion"); + std::string current_training_version = params.get("TrainingVersion"); + accepted_terms = params.get("HasAcceptedTerms") == current_terms_version; + training_done = params.get("CompletedTrainingVersion") == current_training_version; + + TermsPage* terms = new TermsPage(this); + addWidget(terms); + connect(terms, &TermsPage::acceptedTerms, [=]() { + Params().put("HasAcceptedTerms", current_terms_version); + accepted_terms = true; + updateActiveScreen(); + }); + connect(terms, &TermsPage::declinedTerms, [=]() { setCurrentIndex(2); }); + + TrainingGuide* tr = new TrainingGuide(this); + addWidget(tr); + connect(tr, &TrainingGuide::completedTraining, [=]() { + training_done = true; + Params().put("CompletedTrainingVersion", current_training_version); + updateActiveScreen(); + }); + + DeclinePage* declinePage = new DeclinePage(this); + addWidget(declinePage); + connect(declinePage, &DeclinePage::getBack, [=]() { updateActiveScreen(); }); + + setStyleSheet(R"( + * { + color: white; + background-color: black; + } + QPushButton { + height: 160px; + font-size: 55px; + font-weight: 400; + border-radius: 10px; + background-color: #4F4F4F; + } + )"); + updateActiveScreen(); +} diff --git a/selfdrive/ui/qt/offroad/onboarding.h b/selfdrive/ui/qt/offroad/onboarding.h new file mode 100644 index 00000000000000..48f409489949bd --- /dev/null +++ b/selfdrive/ui/qt/offroad/onboarding.h @@ -0,0 +1,133 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "selfdrive/ui/qt/qt_window.h" + +class TrainingGuide : public QFrame { + Q_OBJECT + +public: + explicit TrainingGuide(QWidget *parent = 0); + +private: + void showEvent(QShowEvent *event) override; + void paintEvent(QPaintEvent *event) override; + void mouseReleaseEvent(QMouseEvent* e) override; + + QImage image; + int currentIndex = 0; + + // Bounding boxes for each training guide step + const QRect continueBtnStandard = {1620, 0, 300, 1080}; + QVector boundingRectStandard { + QRect(112, 804, 619, 166), + continueBtnStandard, + continueBtnStandard, + QRect(1476, 565, 253, 308), + QRect(1501, 529, 184, 108), + continueBtnStandard, + QRect(1613, 665, 178, 153), + QRect(1220, 0, 420, 730), + QRect(1335, 499, 440, 147), + QRect(112, 820, 996, 148), + QRect(1412, 199, 316, 333), + continueBtnStandard, + QRect(1237, 63, 683, 1017), + continueBtnStandard, + QRect(1455, 110, 313, 860), + QRect(1253, 519, 383, 228), + continueBtnStandard, + continueBtnStandard, + QRect(630, 804, 626, 164), + QRect(108, 804, 426, 164), + }; + + const QRect continueBtnWide = {1840, 0, 320, 1080}; + QVector boundingRectWide { + QRect(112, 804, 618, 164), + continueBtnWide, + continueBtnWide, + QRect(1641, 558, 210, 313), + QRect(1662, 528, 184, 108), + continueBtnWide, + QRect(1814, 621, 211, 170), + QRect(1350, 0, 497, 755), + QRect(1553, 516, 406, 112), + QRect(112, 804, 1126, 164), + QRect(1598, 199, 316, 333), + continueBtnWide, + QRect(1364, 90, 796, 990), + continueBtnWide, + QRect(1593, 114, 318, 853), + QRect(1379, 511, 391, 243), + continueBtnWide, + continueBtnWide, + QRect(630, 804, 626, 164), + QRect(108, 804, 426, 164), + }; + + QString img_path; + QVector boundingRect; + QElapsedTimer click_timer; + +signals: + void completedTraining(); +}; + + +class TermsPage : public QFrame { + Q_OBJECT + +public: + explicit TermsPage(QWidget *parent = 0) : QFrame(parent) {}; + +public slots: + void enableAccept(); + +private: + void showEvent(QShowEvent *event) override; + + QPushButton *accept_btn; + +signals: + void acceptedTerms(); + void declinedTerms(); +}; + +class DeclinePage : public QFrame { + Q_OBJECT + +public: + explicit DeclinePage(QWidget *parent = 0) : QFrame(parent) {}; + +private: + void showEvent(QShowEvent *event) override; + +signals: + void getBack(); +}; + +class OnboardingWindow : public QStackedWidget { + Q_OBJECT + +public: + explicit OnboardingWindow(QWidget *parent = 0); + inline void showTrainingGuide() { setCurrentIndex(1); } + inline bool completed() const { return accepted_terms && training_done; } + +private: + void updateActiveScreen(); + + Params params; + bool accepted_terms = false, training_done = false; + +signals: + void onboardingDone(); +}; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc new file mode 100644 index 00000000000000..e5e56345452629 --- /dev/null +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -0,0 +1,478 @@ +#include "selfdrive/ui/qt/offroad/settings.h" + +#include +#include +#include + +#include + +#include "selfdrive/ui/qt/offroad/networking.h" + +#ifdef ENABLE_MAPS +#include "selfdrive/ui/qt/maps/map_settings.h" +#endif + +#include "common/params.h" +#include "common/watchdog.h" +#include "common/util.h" +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" +#include "selfdrive/ui/qt/widgets/ssh_keys.h" +#include "selfdrive/ui/qt/widgets/toggle.h" +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/input.h" + +TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { + // param, title, desc, icon + std::vector> toggle_defs{ + { + "OpenpilotEnabledToggle", + tr("Enable openpilot"), + tr("Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off."), + "../assets/offroad/icon_openpilot.png", + }, + { + "IsLdwEnabled", + tr("Enable Lane Departure Warnings"), + tr("Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h)."), + "../assets/offroad/icon_warning.png", + }, + { + "IsMetric", + tr("Use Metric System"), + tr("Display speed in km/h instead of mph."), + "../assets/offroad/icon_metric.png", + }, + { + "RecordFront", + tr("Record and Upload Driver Camera"), + tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), + "../assets/offroad/icon_monitoring.png", + }, + { + "DisengageOnAccelerator", + tr("Disengage On Accelerator Pedal"), + tr("When enabled, pressing the accelerator pedal will disengage openpilot."), + "../assets/offroad/icon_disengage_on_accelerator.svg", + }, + { + "EndToEndLong", + tr("🌮 End-to-end longitudinal (extremely alpha) 🌮"), + "", + "../assets/offroad/icon_road.png", + }, + { + "ExperimentalLongitudinalEnabled", + tr("Experimental openpilot longitudinal control"), + tr("WARNING: openpilot longitudinal control is experimental for this car and will disable AEB."), + "../assets/offroad/icon_speed_limit.png", + }, +#ifdef ENABLE_MAPS + { + "NavSettingTime24h", + tr("Show ETA in 24h Format"), + tr("Use 24h format instead of am/pm"), + "../assets/offroad/icon_metric.png", + }, + { + "NavSettingLeftSide", + tr("Show Map on Left Side of UI"), + tr("Show map on left side when in split screen view."), + "../assets/offroad/icon_road.png", + }, +#endif + + }; + + for (auto &[param, title, desc, icon] : toggle_defs) { + auto toggle = new ParamControl(param, title, desc, icon, this); + + bool locked = params.getBool((param + "Lock").toStdString()); + toggle->setEnabled(!locked); + + addItem(toggle); + toggles[param.toStdString()] = toggle; + } + + connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { + updateToggles(); + }); +} + +void TogglesPanel::showEvent(QShowEvent *event) { + updateToggles(); +} + +void TogglesPanel::updateToggles() { + auto e2e_toggle = toggles["EndToEndLong"]; + auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"]; + const QString e2e_description = tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental."); + + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + if (!CP.getExperimentalLongitudinalAvailable()) { + params.remove("ExperimentalLongitudinalEnabled"); + } + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); + + const bool op_long = CP.getOpenpilotLongitudinalControl() && !CP.getExperimentalLongitudinalAvailable(); + const bool exp_long_enabled = CP.getExperimentalLongitudinalAvailable() && params.getBool("ExperimentalLongitudinalEnabled"); + if (op_long || exp_long_enabled) { + // normal description and toggle + e2e_toggle->setEnabled(true); + e2e_toggle->setDescription(e2e_description); + } else { + // no long for now + e2e_toggle->setEnabled(false); + params.remove("EndToEndLong"); + + const QString no_long = "openpilot longitudinal control is not currently available for this car."; + const QString exp_long = "Enable experimental longitudinal control to enable this."; + e2e_toggle->setDescription("" + (CP.getExperimentalLongitudinalAvailable() ? exp_long : no_long) + "

    " + e2e_description); + } + + e2e_toggle->refresh(); + } else { + e2e_toggle->setDescription(e2e_description); + op_long_toggle->setVisible(false); + } +} + +DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { + setSpacing(50); + addItem(new LabelControl(tr("Dongle ID"), getDongleId().value_or(tr("N/A")))); + addItem(new LabelControl(tr("Serial"), params.get("HardwareSerial").c_str())); + + // offroad-only buttons + + auto dcamBtn = new ButtonControl(tr("Driver Camera"), tr("PREVIEW"), + tr("Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)")); + connect(dcamBtn, &ButtonControl::clicked, [=]() { emit showDriverView(); }); + addItem(dcamBtn); + + auto resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), ""); + connect(resetCalibBtn, &ButtonControl::showDescription, this, &DevicePanel::updateCalibDescription); + connect(resetCalibBtn, &ButtonControl::clicked, [&]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), this)) { + params.remove("CalibrationParams"); + } + }); + addItem(resetCalibBtn); + + if (!params.getBool("Passive")) { + auto retrainingBtn = new ButtonControl(tr("Review Training Guide"), tr("REVIEW"), tr("Review the rules, features, and limitations of openpilot")); + connect(retrainingBtn, &ButtonControl::clicked, [=]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to review the training guide?"), this)) { + emit reviewTrainingGuide(); + } + }); + addItem(retrainingBtn); + } + + if (Hardware::TICI()) { + auto regulatoryBtn = new ButtonControl(tr("Regulatory"), tr("VIEW"), ""); + connect(regulatoryBtn, &ButtonControl::clicked, [=]() { + const std::string txt = util::read_file("../assets/offroad/fcc.html"); + RichTextDialog::alert(QString::fromStdString(txt), this); + }); + addItem(regulatoryBtn); + } + + auto translateBtn = new ButtonControl(tr("Change Language"), tr("CHANGE"), ""); + connect(translateBtn, &ButtonControl::clicked, [=]() { + QMap langs = getSupportedLanguages(); + QString currentLang = QString::fromStdString(Params().get("LanguageSetting")); + QString selection = MultiOptionDialog::getSelection(tr("Select a language"), langs.keys(), langs.key(currentLang), this); + if (!selection.isEmpty()) { + // put language setting, exit Qt UI, and trigger fast restart + Params().put("LanguageSetting", langs[selection].toStdString()); + qApp->exit(18); + watchdog_kick(0); + } + }); + addItem(translateBtn); + + QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + for (auto btn : findChildren()) { + btn->setEnabled(offroad); + } + }); + + // power buttons + QHBoxLayout *power_layout = new QHBoxLayout(); + power_layout->setSpacing(30); + + QPushButton *reboot_btn = new QPushButton(tr("Reboot")); + reboot_btn->setObjectName("reboot_btn"); + power_layout->addWidget(reboot_btn); + QObject::connect(reboot_btn, &QPushButton::clicked, this, &DevicePanel::reboot); + + QPushButton *poweroff_btn = new QPushButton(tr("Power Off")); + poweroff_btn->setObjectName("poweroff_btn"); + power_layout->addWidget(poweroff_btn); + QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff); + + if (!Hardware::PC()) { + connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible); + } + + setStyleSheet(R"( + #reboot_btn { height: 120px; border-radius: 15px; background-color: #393939; } + #reboot_btn:pressed { background-color: #4a4a4a; } + #poweroff_btn { height: 120px; border-radius: 15px; background-color: #E22C2C; } + #poweroff_btn:pressed { background-color: #FF2424; } + )"); + addItem(power_layout); +} + +void DevicePanel::updateCalibDescription() { + QString desc = + tr("openpilot requires the device to be mounted within 4° left or right and " + "within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required."); + std::string calib_bytes = Params().get("CalibrationParams"); + if (!calib_bytes.empty()) { + try { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size())); + auto calib = cmsg.getRoot().getLiveCalibration(); + if (calib.getCalStatus() != 0) { + double pitch = calib.getRpyCalib()[1] * (180 / M_PI); + double yaw = calib.getRpyCalib()[2] * (180 / M_PI); + desc += tr(" Your device is pointed %1° %2 and %3° %4.") + .arg(QString::number(std::abs(pitch), 'g', 1), pitch > 0 ? tr("down") : tr("up"), + QString::number(std::abs(yaw), 'g', 1), yaw > 0 ? tr("left") : tr("right")); + } + } catch (kj::Exception) { + qInfo() << "invalid CalibrationParams"; + } + } + qobject_cast(sender())->setDescription(desc); +} + +void DevicePanel::reboot() { + if (!uiState()->engaged()) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot?"), this)) { + // Check engaged again in case it changed while the dialog was open + if (!uiState()->engaged()) { + Params().putBool("DoReboot", true); + } + } + } else { + ConfirmationDialog::alert(tr("Disengage to Reboot"), this); + } +} + +void DevicePanel::poweroff() { + if (!uiState()->engaged()) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to power off?"), this)) { + // Check engaged again in case it changed while the dialog was open + if (!uiState()->engaged()) { + Params().putBool("DoShutdown", true); + } + } + } else { + ConfirmationDialog::alert(tr("Disengage to Power Off"), this); + } +} + +SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { + gitBranchLbl = new LabelControl(tr("Git Branch")); + gitCommitLbl = new LabelControl(tr("Git Commit")); + osVersionLbl = new LabelControl(tr("OS Version")); + versionLbl = new LabelControl(tr("Version"), "", QString::fromStdString(params.get("ReleaseNotes")).trimmed()); + lastUpdateLbl = new LabelControl(tr("Last Update Check"), "", tr("The last time openpilot successfully checked for an update. The updater only runs while the car is off.")); + updateBtn = new ButtonControl(tr("Check for Update"), ""); + connect(updateBtn, &ButtonControl::clicked, [=]() { + if (params.getBool("IsOffroad")) { + fs_watch->addPath(QString::fromStdString(params.getParamPath("LastUpdateTime"))); + fs_watch->addPath(QString::fromStdString(params.getParamPath("UpdateFailedCount"))); + updateBtn->setText(tr("CHECKING")); + updateBtn->setEnabled(false); + } + std::system("pkill -1 -f selfdrive.updated"); + }); + connect(uiState(), &UIState::offroadTransition, updateBtn, &QPushButton::setEnabled); + + branchSwitcherBtn = new ButtonControl(tr("Switch Branch"), tr("ENTER"), tr("The new branch will be pulled the next time the updater runs.")); + connect(branchSwitcherBtn, &ButtonControl::clicked, [=]() { + QString branch = InputDialog::getText(tr("Enter branch name"), this, tr("The new branch will be pulled the next time the updater runs."), + false, -1, QString::fromStdString(params.get("SwitchToBranch"))); + if (branch.isEmpty()) { + params.remove("SwitchToBranch"); + } else { + params.put("SwitchToBranch", branch.toStdString()); + } + std::system("pkill -1 -f selfdrive.updated"); + }); + connect(uiState(), &UIState::offroadTransition, branchSwitcherBtn, &QPushButton::setEnabled); + + auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL")); + connect(uninstallBtn, &ButtonControl::clicked, [&]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to uninstall?"), this)) { + params.putBool("DoUninstall", true); + } + }); + connect(uiState(), &UIState::offroadTransition, uninstallBtn, &QPushButton::setEnabled); + + QWidget *widgets[] = {versionLbl, lastUpdateLbl, updateBtn, branchSwitcherBtn, gitBranchLbl, gitCommitLbl, osVersionLbl, uninstallBtn}; + for (QWidget* w : widgets) { + if (w == branchSwitcherBtn && params.getBool("IsTestedBranch")) { + continue; + } + addItem(w); + } + + fs_watch = new QFileSystemWatcher(this); + QObject::connect(fs_watch, &QFileSystemWatcher::fileChanged, [=](const QString path) { + if (path.contains("UpdateFailedCount") && std::atoi(params.get("UpdateFailedCount").c_str()) > 0) { + lastUpdateLbl->setText(tr("failed to fetch update")); + updateBtn->setText(tr("CHECK")); + updateBtn->setEnabled(true); + } else if (path.contains("LastUpdateTime")) { + updateLabels(); + } + }); +} + +void SoftwarePanel::showEvent(QShowEvent *event) { + updateLabels(); +} + +void SoftwarePanel::updateLabels() { + QString lastUpdate = ""; + auto tm = params.get("LastUpdateTime"); + if (!tm.empty()) { + lastUpdate = timeAgo(QDateTime::fromString(QString::fromStdString(tm + "Z"), Qt::ISODate)); + } + + versionLbl->setText(getBrandVersion()); + lastUpdateLbl->setText(lastUpdate); + updateBtn->setText(tr("CHECK")); + updateBtn->setEnabled(true); + gitBranchLbl->setText(QString::fromStdString(params.get("GitBranch"))); + gitCommitLbl->setText(QString::fromStdString(params.get("GitCommit")).left(10)); + osVersionLbl->setText(QString::fromStdString(Hardware::get_os_version()).trimmed()); +} + +void SettingsWindow::showEvent(QShowEvent *event) { + panel_widget->setCurrentIndex(0); + nav_btns->buttons()[0]->setChecked(true); +} + +SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { + + // setup two main layouts + sidebar_widget = new QWidget; + QVBoxLayout *sidebar_layout = new QVBoxLayout(sidebar_widget); + sidebar_layout->setMargin(0); + panel_widget = new QStackedWidget(); + panel_widget->setStyleSheet(R"( + border-radius: 30px; + background-color: #292929; + )"); + + // close button + QPushButton *close_btn = new QPushButton(tr("×")); + close_btn->setStyleSheet(R"( + QPushButton { + font-size: 140px; + padding-bottom: 20px; + font-weight: bold; + border 1px grey solid; + border-radius: 100px; + background-color: #292929; + font-weight: 400; + } + QPushButton:pressed { + background-color: #3B3B3B; + } + )"); + close_btn->setFixedSize(200, 200); + sidebar_layout->addSpacing(45); + sidebar_layout->addWidget(close_btn, 0, Qt::AlignCenter); + QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindow::closeSettings); + + // setup panels + DevicePanel *device = new DevicePanel(this); + QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); + QObject::connect(device, &DevicePanel::showDriverView, this, &SettingsWindow::showDriverView); + + QList> panels = { + {tr("Device"), device}, + {tr("Network"), new Networking(this)}, + {tr("Toggles"), new TogglesPanel(this)}, + {tr("Software"), new SoftwarePanel(this)}, + }; + +#ifdef ENABLE_MAPS + auto map_panel = new MapPanel(this); + panels.push_back({tr("Navigation"), map_panel}); + QObject::connect(map_panel, &MapPanel::closeSettings, this, &SettingsWindow::closeSettings); +#endif + + const int padding = panels.size() > 3 ? 25 : 35; + + nav_btns = new QButtonGroup(this); + for (auto &[name, panel] : panels) { + QPushButton *btn = new QPushButton(name); + btn->setCheckable(true); + btn->setChecked(nav_btns->buttons().size() == 0); + btn->setStyleSheet(QString(R"( + QPushButton { + color: grey; + border: none; + background: none; + font-size: 65px; + font-weight: 500; + padding-top: %1px; + padding-bottom: %1px; + } + QPushButton:checked { + color: white; + } + QPushButton:pressed { + color: #ADADAD; + } + )").arg(padding)); + + nav_btns->addButton(btn); + sidebar_layout->addWidget(btn, 0, Qt::AlignRight); + + const int lr_margin = name != tr("Network") ? 50 : 0; // Network panel handles its own margins + panel->setContentsMargins(lr_margin, 25, lr_margin, 25); + + ScrollView *panel_frame = new ScrollView(panel, this); + panel_widget->addWidget(panel_frame); + + QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { + btn->setChecked(true); + panel_widget->setCurrentWidget(w); + }); + } + sidebar_layout->setContentsMargins(50, 50, 100, 50); + + // main settings layout, sidebar + main panel + QHBoxLayout *main_layout = new QHBoxLayout(this); + + sidebar_widget->setFixedWidth(500); + main_layout->addWidget(sidebar_widget); + main_layout->addWidget(panel_widget); + + setStyleSheet(R"( + * { + color: white; + font-size: 50px; + } + SettingsWindow { + background-color: black; + } + )"); +} diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h new file mode 100644 index 00000000000000..1f823851f10c11 --- /dev/null +++ b/selfdrive/ui/qt/offroad/settings.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +#include "selfdrive/ui/qt/widgets/controls.h" + +// ********** settings window + top-level panels ********** +class SettingsWindow : public QFrame { + Q_OBJECT + +public: + explicit SettingsWindow(QWidget *parent = 0); + +protected: + void showEvent(QShowEvent *event) override; + +signals: + void closeSettings(); + void reviewTrainingGuide(); + void showDriverView(); + +private: + QPushButton *sidebar_alert_widget; + QWidget *sidebar_widget; + QButtonGroup *nav_btns; + QStackedWidget *panel_widget; +}; + +class DevicePanel : public ListWidget { + Q_OBJECT +public: + explicit DevicePanel(SettingsWindow *parent); +signals: + void reviewTrainingGuide(); + void showDriverView(); + +private slots: + void poweroff(); + void reboot(); + void updateCalibDescription(); + +private: + Params params; +}; + +class TogglesPanel : public ListWidget { + Q_OBJECT +public: + explicit TogglesPanel(SettingsWindow *parent); + void showEvent(QShowEvent *event) override; + +private: + Params params; + std::map toggles; + + void updateToggles(); +}; + +class SoftwarePanel : public ListWidget { + Q_OBJECT +public: + explicit SoftwarePanel(QWidget* parent = nullptr); + +private: + void showEvent(QShowEvent *event) override; + void updateLabels(); + + LabelControl *gitBranchLbl; + LabelControl *gitCommitLbl; + LabelControl *osVersionLbl; + LabelControl *versionLbl; + LabelControl *lastUpdateLbl; + ButtonControl *updateBtn; + ButtonControl *branchSwitcherBtn; + + Params params; + QFileSystemWatcher *fs_watch; +}; diff --git a/selfdrive/ui/qt/offroad/text_view.qml b/selfdrive/ui/qt/offroad/text_view.qml new file mode 100644 index 00000000000000..10b423bacbd09b --- /dev/null +++ b/selfdrive/ui/qt/offroad/text_view.qml @@ -0,0 +1,47 @@ +import QtQuick 2.0 + +Item { + id: root + signal scroll() + + Flickable { + id: flickArea + objectName: "flickArea" + anchors.fill: parent + contentHeight: helpText.height + contentWidth: width - (leftMargin + rightMargin) + bottomMargin: 50 + topMargin: 50 + rightMargin: 50 + leftMargin: 50 + flickableDirection: Flickable.VerticalFlick + flickDeceleration: 7500.0 + maximumFlickVelocity: 10000.0 + pixelAligned: true + + onAtYEndChanged: root.scroll() + + Text { + id: helpText + width: flickArea.contentWidth + font.family: "Inter" + font.weight: "Light" + font.pixelSize: 50 + textFormat: Text.RichText + color: "#C9C9C9" + wrapMode: Text.Wrap + text: text_view + } + } + + Rectangle { + id: scrollbar + anchors.right: flickArea.right + anchors.rightMargin: 20 + y: flickArea.topMargin + flickArea.visibleArea.yPosition * (flickArea.height - flickArea.bottomMargin - flickArea.topMargin) + width: 12 + radius: 6 + height: flickArea.visibleArea.heightRatio * (flickArea.height - flickArea.bottomMargin - flickArea.topMargin) + color: "#808080" + } +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc new file mode 100644 index 00000000000000..fbb64b972ef171 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -0,0 +1,470 @@ +#include "selfdrive/ui/qt/offroad/wifiManager.h" + +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/widgets/prime.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "selfdrive/ui/qt/util.h" + +bool compare_by_strength(const Network &a, const Network &b) { + if (a.connected == ConnectedType::CONNECTED) return true; + if (b.connected == ConnectedType::CONNECTED) return false; + if (a.connected == ConnectedType::CONNECTING) return true; + if (b.connected == ConnectedType::CONNECTING) return false; + return a.strength > b.strength; +} + +template +T call(const QString &path, const QString &interface, const QString &method, Args &&...args) { + QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + nm.setTimeout(DBUS_TIMEOUT); + QDBusMessage response = nm.call(method, args...); + if constexpr (std::is_same_v) { + return response; + } else if (response.arguments().count() >= 1) { + QVariant vFirst = response.arguments().at(0).value().variant(); + if (vFirst.canConvert()) { + return vFirst.value(); + } + QDebug critical = qCritical(); + critical << "Variant unpacking failure :" << method << ','; + (critical << ... << args); + } + return T(); +} + +template +QDBusPendingCall asyncCall(const QString &path, const QString &interface, const QString &method, Args &&...args) { + QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + return nm.asyncCall(method, args...); +} + +WifiManager::WifiManager(QObject *parent) : QObject(parent) { + qDBusRegisterMetaType(); + qDBusRegisterMetaType(); + + // Set tethering ssid as "weedle" + first 4 characters of a dongle id + tethering_ssid = "weedle"; + if (auto dongle_id = getDongleId()) { + tethering_ssid += "-" + dongle_id->left(4); + } + + adapter = getAdapter(); + if (!adapter.isEmpty()) { + setup(); + } else { + QDBusConnection::systemBus().connect(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeviceAdded", this, SLOT(deviceAdded(QDBusObjectPath))); + } + + timer.callOnTimeout(this, &WifiManager::requestScan); +} + +void WifiManager::setup() { + auto bus = QDBusConnection::systemBus(); + bus.connect(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_DEVICE, "StateChanged", this, SLOT(stateChange(unsigned int, unsigned int, unsigned int))); + bus.connect(NM_DBUS_SERVICE, adapter, NM_DBUS_INTERFACE_PROPERTIES, "PropertiesChanged", this, SLOT(propertyChange(QString, QVariantMap, QStringList))); + + bus.connect(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "ConnectionRemoved", this, SLOT(connectionRemoved(QDBusObjectPath))); + bus.connect(NM_DBUS_SERVICE, NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "NewConnection", this, SLOT(newConnection(QDBusObjectPath))); + + raw_adapter_state = call(adapter, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE, "State"); + activeAp = call(adapter, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE_WIRELESS, "ActiveAccessPoint").path(); + + initConnections(); + requestScan(); +} + +void WifiManager::start() { + timer.start(5000); + refreshNetworks(); +} + +void WifiManager::stop() { + timer.stop(); +} + +void WifiManager::refreshNetworks() { + if (adapter.isEmpty() || !timer.isActive()) return; + + QDBusPendingCall pending_call = asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "GetAllAccessPoints"); + QDBusPendingCallWatcher *watcher = new QDBusPendingCallWatcher(pending_call); + QObject::connect(watcher, &QDBusPendingCallWatcher::finished, this, &WifiManager::refreshFinished); +} + +void WifiManager::refreshFinished(QDBusPendingCallWatcher *watcher) { + ipv4_address = getIp4Address(); + seenNetworks.clear(); + + const QDBusReply> wather_reply = *watcher; + for (const QDBusObjectPath &path : wather_reply.value()) { + QDBusReply replay = call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "GetAll", NM_DBUS_INTERFACE_ACCESS_POINT); + auto properties = replay.value(); + + const QByteArray ssid = properties["Ssid"].toByteArray(); + uint32_t strength = properties["Strength"].toUInt(); + if (ssid.isEmpty() || (seenNetworks.contains(ssid) && strength <= seenNetworks[ssid].strength)) continue; + + SecurityType security = getSecurityType(properties); + ConnectedType ctype = ConnectedType::DISCONNECTED; + if (path.path() == activeAp) { + ctype = (ssid == connecting_to_network) ? ConnectedType::CONNECTING : ConnectedType::CONNECTED; + } + seenNetworks[ssid] = {ssid, strength, ctype, security}; + } + + emit refreshSignal(); + watcher->deleteLater(); +} + +QString WifiManager::getIp4Address() { + if (raw_adapter_state != NM_DEVICE_STATE_ACTIVATED) return ""; + + for (const auto &p : getActiveConnections()) { + QString type = call(p.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); + if (type == "802-11-wireless") { + auto ip4config = call(p.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Ip4Config"); + const auto &arr = call(ip4config.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_IP4_CONFIG, "AddressData"); + QVariantMap path; + arr.beginArray(); + while (!arr.atEnd()) { + arr >> path; + arr.endArray(); + return path.value("address").value(); + } + arr.endArray(); + } + } + return ""; +} + +SecurityType WifiManager::getSecurityType(const QVariantMap &properties) { + int sflag = properties["Flags"].toUInt(); + int wpaflag = properties["WpaFlags"].toUInt(); + int rsnflag = properties["RsnFlags"].toUInt(); + int wpa_props = wpaflag | rsnflag; + + // obtained by looking at flags of networks in the office as reported by an Android phone + const int 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 ((sflag == NM_802_11_AP_FLAGS_NONE) || ((sflag & NM_802_11_AP_FLAGS_WPS) && !(wpa_props & supports_wpa))) { + return SecurityType::OPEN; + } else if ((sflag & NM_802_11_AP_FLAGS_PRIVACY) && (wpa_props & supports_wpa) && !(wpa_props & NM_802_11_AP_SEC_KEY_MGMT_802_1X)) { + return SecurityType::WPA; + } else { + LOGW("Unsupported network! sflag: %d, wpaflag: %d, rsnflag: %d", sflag, wpaflag, rsnflag); + return SecurityType::UNSUPPORTED; + } +} + +void WifiManager::connect(const Network &n, const QString &password, const QString &username) { + connecting_to_network = n.ssid; + seenNetworks[n.ssid].connected = ConnectedType::CONNECTING; + forgetConnection(n.ssid); // Clear all connections that may already exist to the network we are connecting + Connection connection; + connection["connection"]["type"] = "802-11-wireless"; + connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); + connection["connection"]["id"] = "openpilot connection " + QString::fromStdString(n.ssid.toStdString()); + connection["connection"]["autoconnect-retries"] = 0; + + connection["802-11-wireless"]["ssid"] = n.ssid; + connection["802-11-wireless"]["mode"] = "infrastructure"; + + if (n.security_type == SecurityType::WPA) { + connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; + connection["802-11-wireless-security"]["auth-alg"] = "open"; + connection["802-11-wireless-security"]["psk"] = password; + } + + connection["ipv4"]["method"] = "auto"; + connection["ipv4"]["dns-priority"] = 600; + connection["ipv6"]["method"] = "ignore"; + + call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); +} + +void WifiManager::deactivateConnectionBySsid(const QString &ssid) { + for (QDBusObjectPath active_connection : getActiveConnections()) { + auto pth = call(active_connection.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "SpecificObject"); + if (pth.path() != "" && pth.path() != "/") { + QString Ssid = get_property(pth.path(), "Ssid"); + if (Ssid == ssid) { + deactivateConnection(active_connection); + return; + } + } + } +} + +void WifiManager::deactivateConnection(const QDBusObjectPath &path) { + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeactivateConnection", QVariant::fromValue(path)); +} + +QVector WifiManager::getActiveConnections() { + QVector conns; + QDBusObjectPath path; + const QDBusArgument &arr = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "ActiveConnections"); + arr.beginArray(); + while (!arr.atEnd()) { + arr >> path; + conns.push_back(path); + } + arr.endArray(); + return conns; +} + +bool WifiManager::isKnownConnection(const QString &ssid) { + return !getConnectionPath(ssid).path().isEmpty(); +} + +void WifiManager::forgetConnection(const QString &ssid) { + const QDBusObjectPath &path = getConnectionPath(ssid); + if (!path.path().isEmpty()) { + call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "Delete"); + } +} + +uint WifiManager::getAdapterType(const QDBusObjectPath &path) { + return call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType"); +} + +void WifiManager::requestScan() { + if (!adapter.isEmpty()) { + asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "RequestScan", QVariantMap()); + } +} + +QByteArray WifiManager::get_property(const QString &network_path , const QString &property) { + return call(network_path, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACCESS_POINT, property); +} + +QString WifiManager::getAdapter(const uint adapter_type) { + QDBusReply> response = call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "GetDevices"); + for (const QDBusObjectPath &path : response.value()) { + if (getAdapterType(path) == adapter_type) { + return path.path(); + } + } + return ""; +} + +void WifiManager::stateChange(unsigned int new_state, unsigned int previous_state, unsigned int change_reason) { + raw_adapter_state = new_state; + if (new_state == NM_DEVICE_STATE_NEED_AUTH && change_reason == NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT && !connecting_to_network.isEmpty()) { + forgetConnection(connecting_to_network); + emit wrongPassword(connecting_to_network); + } else if (new_state == NM_DEVICE_STATE_ACTIVATED) { + connecting_to_network = ""; + refreshNetworks(); + } +} + +// https://developer.gnome.org/NetworkManager/stable/gdbus-org.freedesktop.NetworkManager.Device.Wireless.html +void WifiManager::propertyChange(const QString &interface, const QVariantMap &props, const QStringList &invalidated_props) { + if (interface == NM_DBUS_INTERFACE_DEVICE_WIRELESS && props.contains("LastScan")) { + refreshNetworks(); + } else if (interface == NM_DBUS_INTERFACE_DEVICE_WIRELESS && props.contains("ActiveAccessPoint")) { + activeAp = props.value("ActiveAccessPoint").value().path(); + } +} + +void WifiManager::deviceAdded(const QDBusObjectPath &path) { + if (getAdapterType(path) == NM_DEVICE_TYPE_WIFI && (adapter.isEmpty() || adapter == "/")) { + adapter = path.path(); + setup(); + } +} + +void WifiManager::connectionRemoved(const QDBusObjectPath &path) { + knownConnections.remove(path); +} + +void WifiManager::newConnection(const QDBusObjectPath &path) { + Connection settings = getConnectionSettings(path); + if (settings.value("connection").value("type") == "802-11-wireless") { + knownConnections[path] = settings.value("802-11-wireless").value("ssid").toString(); + if (knownConnections[path] != tethering_ssid) { + activateWifiConnection(knownConnections[path]); + } + } +} + +QDBusObjectPath WifiManager::getConnectionPath(const QString &ssid) { + return knownConnections.key(ssid); +} + +Connection WifiManager::getConnectionSettings(const QDBusObjectPath &path) { + return QDBusReply(call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "GetSettings")).value(); +} + +void WifiManager::initConnections() { + const QDBusReply> response = call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "ListConnections"); + for (const QDBusObjectPath &path : response.value()) { + const Connection settings = getConnectionSettings(path); + if (settings.value("connection").value("type") == "802-11-wireless") { + knownConnections[path] = settings.value("802-11-wireless").value("ssid").toString(); + } else if (settings.value("connection").value("id") == "lte") { + lteConnectionPath = path; + } + } +} + +std::optional WifiManager::activateWifiConnection(const QString &ssid) { + const QDBusObjectPath &path = getConnectionPath(ssid); + if (!path.path().isEmpty()) { + connecting_to_network = ssid; + return asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/"))); + } + return std::nullopt; +} + +void WifiManager::activateModemConnection(const QDBusObjectPath &path) { + QString modem = getAdapter(NM_DEVICE_TYPE_MODEM); + if (!path.path().isEmpty() && !modem.isEmpty()) { + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/"))); + } +} + +// function matches tici/hardware.py +NetworkType WifiManager::currentNetworkType() { + auto primary_conn = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "PrimaryConnection"); + auto primary_type = call(primary_conn.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); + + if (primary_type == "802-3-ethernet") { + return NetworkType::ETHERNET; + } else if (primary_type == "802-11-wireless" && !isTetheringEnabled()) { + return NetworkType::WIFI; + } else { + for (const QDBusObjectPath &conn : getActiveConnections()) { + auto type = call(conn.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); + if (type == "gsm") { + return NetworkType::CELL; + } + } + } + return NetworkType::NONE; +} + +void WifiManager::updateGsmSettings(bool roaming, QString apn) { + if (!lteConnectionPath.path().isEmpty()) { + bool changes = false; + bool auto_config = apn.isEmpty(); + Connection settings = getConnectionSettings(lteConnectionPath); + if (settings.value("gsm").value("auto-config").toBool() != auto_config) { + qWarning() << "Changing gsm.auto-config to" << auto_config; + settings["gsm"]["auto-config"] = auto_config; + changes = true; + } + + if (settings.value("gsm").value("apn").toString() != apn) { + qWarning() << "Changing gsm.apn to" << apn; + settings["gsm"]["apn"] = apn; + changes = true; + } + + if (settings.value("gsm").value("home-only").toBool() == roaming) { + qWarning() << "Changing gsm.home-only to" << !roaming; + settings["gsm"]["home-only"] = !roaming; + changes = true; + } + + if (changes) { + call(lteConnectionPath.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "UpdateUnsaved", QVariant::fromValue(settings)); // update is temporary + deactivateConnection(lteConnectionPath); + activateModemConnection(lteConnectionPath); + } + } +} + +// Functions for tethering +void WifiManager::addTetheringConnection() { + Connection connection; + connection["connection"]["id"] = "Hotspot"; + connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); + connection["connection"]["type"] = "802-11-wireless"; + connection["connection"]["interface-name"] = "wlan0"; + connection["connection"]["autoconnect"] = false; + + connection["802-11-wireless"]["band"] = "bg"; + connection["802-11-wireless"]["mode"] = "ap"; + connection["802-11-wireless"]["ssid"] = tethering_ssid.toUtf8(); + + connection["802-11-wireless-security"]["group"] = QStringList("ccmp"); + connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; + connection["802-11-wireless-security"]["pairwise"] = QStringList("ccmp"); + connection["802-11-wireless-security"]["proto"] = QStringList("rsn"); + connection["802-11-wireless-security"]["psk"] = defaultTetheringPassword; + + connection["ipv4"]["method"] = "shared"; + QVariantMap address; + address["address"] = "192.168.43.1"; + address["prefix"] = 24u; + connection["ipv4"]["address-data"] = QVariant::fromValue(IpConfig() << address); + connection["ipv4"]["gateway"] = "192.168.43.1"; + connection["ipv4"]["route-metric"] = 1100; + connection["ipv6"]["method"] = "ignore"; + + call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); +} + +void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { + int prime_type = uiState()->prime_type; + int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); + + if (!ipv4_forward) { + QTimer::singleShot(5000, this, [=] { + qWarning() << "net.ipv4.ip_forward = 0"; + std::system("sudo sysctl net.ipv4.ip_forward=0"); + }); + } + call->deleteLater(); +} + +void WifiManager::setTetheringEnabled(bool enabled) { + if (enabled) { + if (!isKnownConnection(tethering_ssid)) { + addTetheringConnection(); + } + + auto pending_call = activateWifiConnection(tethering_ssid); + + if (pending_call) { + QDBusPendingCallWatcher *watcher = new QDBusPendingCallWatcher(*pending_call); + QObject::connect(watcher, &QDBusPendingCallWatcher::finished, this, &WifiManager::tetheringActivated); + } + + } else { + deactivateConnectionBySsid(tethering_ssid); + } +} + +bool WifiManager::isTetheringEnabled() { + if (activeAp != "" && activeAp != "/") { + return get_property(activeAp, "Ssid") == tethering_ssid; + } + return false; +} + +QString WifiManager::getTetheringPassword() { + if (!isKnownConnection(tethering_ssid)) { + addTetheringConnection(); + } + const QDBusObjectPath &path = getConnectionPath(tethering_ssid); + if (!path.path().isEmpty()) { + QDBusReply> response = call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "GetSecrets", "802-11-wireless-security"); + return response.value().value("802-11-wireless-security").value("psk").toString(); + } + return ""; +} + +void WifiManager::changeTetheringPassword(const QString &newPassword) { + const QDBusObjectPath &path = getConnectionPath(tethering_ssid); + if (!path.path().isEmpty()) { + Connection settings = getConnectionSettings(path); + settings["802-11-wireless-security"]["psk"] = newPassword; + call(path.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, "Update", QVariant::fromValue(settings)); + if (isTetheringEnabled()) { + activateWifiConnection(tethering_ssid); + } + } +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/offroad/wifiManager.h new file mode 100644 index 00000000000000..07b982c2c20b16 --- /dev/null +++ b/selfdrive/ui/qt/offroad/wifiManager.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include +#include + +#include "selfdrive/ui/qt/offroad/networkmanager.h" + +enum class SecurityType { + OPEN, + WPA, + UNSUPPORTED +}; +enum class ConnectedType { + DISCONNECTED, + CONNECTING, + CONNECTED +}; +enum class NetworkType { + NONE, + WIFI, + CELL, + ETHERNET +}; + +typedef QMap Connection; +typedef QVector IpConfig; + +struct Network { + QByteArray ssid; + unsigned int strength; + ConnectedType connected; + SecurityType security_type; +}; +bool compare_by_strength(const Network &a, const Network &b); + +class WifiManager : public QObject { + Q_OBJECT + +public: + QMap seenNetworks; + QMap knownConnections; + QString ipv4_address; + + explicit WifiManager(QObject* parent); + void start(); + void stop(); + void requestScan(); + void forgetConnection(const QString &ssid); + bool isKnownConnection(const QString &ssid); + std::optional activateWifiConnection(const QString &ssid); + NetworkType currentNetworkType(); + void updateGsmSettings(bool roaming, QString apn); + void connect(const Network &ssid, const QString &password = {}, const QString &username = {}); + + // Tethering functions + void setTetheringEnabled(bool enabled); + bool isTetheringEnabled(); + void changeTetheringPassword(const QString &newPassword); + QString getTetheringPassword(); + +private: + QString adapter; // Path to network manager wifi-device + QTimer timer; + unsigned int raw_adapter_state; // Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState + QString connecting_to_network; + QString tethering_ssid; + const QString defaultTetheringPassword = "swagswagcomma"; + QString activeAp; + QDBusObjectPath lteConnectionPath; + + QString getAdapter(const uint = NM_DEVICE_TYPE_WIFI); + uint getAdapterType(const QDBusObjectPath &path); + bool isWirelessAdapter(const QDBusObjectPath &path); + QString getIp4Address(); + void connect(const QByteArray &ssid, const QString &username, const QString &password, SecurityType security_type); + void deactivateConnectionBySsid(const QString &ssid); + void deactivateConnection(const QDBusObjectPath &path); + QVector getActiveConnections(); + QByteArray get_property(const QString &network_path, const QString &property); + SecurityType getSecurityType(const QVariantMap &properties); + QDBusObjectPath getConnectionPath(const QString &ssid); + Connection getConnectionSettings(const QDBusObjectPath &path); + void initConnections(); + void setup(); + void refreshNetworks(); + void activateModemConnection(const QDBusObjectPath &path); + void addTetheringConnection(); + +signals: + void wrongPassword(const QString &ssid); + void refreshSignal(); + +private slots: + void stateChange(unsigned int new_state, unsigned int previous_state, unsigned int change_reason); + void propertyChange(const QString &interface, const QVariantMap &props, const QStringList &invalidated_props); + void deviceAdded(const QDBusObjectPath &path); + void connectionRemoved(const QDBusObjectPath &path); + void newConnection(const QDBusObjectPath &path); + void refreshFinished(QDBusPendingCallWatcher *call); + void tetheringActivated(QDBusPendingCallWatcher *call); +}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc new file mode 100644 index 00000000000000..3920453a477d48 --- /dev/null +++ b/selfdrive/ui/qt/onroad.cc @@ -0,0 +1,580 @@ +#include "selfdrive/ui/qt/onroad.h" + +#include + +#include + +#include "common/timing.h" +#include "selfdrive/ui/qt/util.h" +#ifdef ENABLE_MAPS +#include "selfdrive/ui/qt/maps/map.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" +#endif + +OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(bdr_s); + QStackedLayout *stacked_layout = new QStackedLayout; + stacked_layout->setStackingMode(QStackedLayout::StackAll); + main_layout->addLayout(stacked_layout); + + nvg = new NvgWindow(VISION_STREAM_ROAD, this); + + QWidget * split_wrapper = new QWidget; + split = new QHBoxLayout(split_wrapper); + split->setContentsMargins(0, 0, 0, 0); + split->setSpacing(0); + split->addWidget(nvg); + + stacked_layout->addWidget(split_wrapper); + + alerts = new OnroadAlerts(this); + alerts->setAttribute(Qt::WA_TransparentForMouseEvents, true); + stacked_layout->addWidget(alerts); + + // setup stacking order + alerts->raise(); + + setAttribute(Qt::WA_OpaquePaintEvent); + QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState); + QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition); +} + +void OnroadWindow::updateState(const UIState &s) { + QColor bgColor = bg_colors[s.status]; + Alert alert = Alert::get(*(s.sm), s.scene.started_frame); + if (s.sm->updated("controlsState") || !alert.equal({})) { + if (alert.type == "controlsUnresponsive") { + bgColor = bg_colors[STATUS_ALERT]; + } else if (alert.type == "controlsUnresponsivePermanent") { + bgColor = bg_colors[STATUS_DISENGAGED]; + } + alerts->updateAlert(alert, bgColor); + } + + if (s.scene.map_on_left) { + split->setDirection(QBoxLayout::LeftToRight); + } else { + split->setDirection(QBoxLayout::RightToLeft); + } + + nvg->updateState(s); + + if (bg != bgColor) { + // repaint border + bg = bgColor; + update(); + } +} + +void OnroadWindow::mousePressEvent(QMouseEvent* e) { + if (map != nullptr) { + bool sidebarVisible = geometry().x() > 0; + map->setVisible(!sidebarVisible && !map->isVisible()); + } + // propagation event to parent(HomeWindow) + QWidget::mousePressEvent(e); +} + +void OnroadWindow::offroadTransition(bool offroad) { +#ifdef ENABLE_MAPS + if (!offroad) { + if (map == nullptr && (uiState()->prime_type || !MAPBOX_TOKEN.isEmpty())) { + MapWindow * m = new MapWindow(get_mapbox_settings()); + map = m; + + QObject::connect(uiState(), &UIState::offroadTransition, m, &MapWindow::offroadTransition); + + m->setFixedWidth(topWidget(this)->width() / 2); + split->insertWidget(0, m); + + // Make map visible after adding to split + m->offroadTransition(offroad); + } + } +#endif + + alerts->updateAlert({}, bg); + + // update stream type + bool wide_cam = Params().getBool("WideCameraOnly"); + nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); +} + +void OnroadWindow::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); +} + +// ***** onroad widgets ***** + +// OnroadAlerts +void OnroadAlerts::updateAlert(const Alert &a, const QColor &color) { + if (!alert.equal(a) || color != bg) { + alert = a; + bg = color; + update(); + } +} + +void OnroadAlerts::paintEvent(QPaintEvent *event) { + if (alert.size == cereal::ControlsState::AlertSize::NONE) { + return; + } + static std::map alert_sizes = { + {cereal::ControlsState::AlertSize::SMALL, 271}, + {cereal::ControlsState::AlertSize::MID, 420}, + {cereal::ControlsState::AlertSize::FULL, height()}, + }; + int h = alert_sizes[alert.size]; + QRect r = QRect(0, height() - h, width(), h); + + QPainter p(this); + + // draw background + gradient + p.setPen(Qt::NoPen); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + + p.setBrush(QBrush(bg)); + p.drawRect(r); + + QLinearGradient g(0, r.y(), 0, r.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.fillRect(r, g); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + + // text + const QPoint c = r.center(); + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setRenderHint(QPainter::TextAntialiasing); + if (alert.size == cereal::ControlsState::AlertSize::SMALL) { + configFont(p, "Inter", 74, "SemiBold"); + p.drawText(r, Qt::AlignCenter, alert.text1); + } else if (alert.size == cereal::ControlsState::AlertSize::MID) { + configFont(p, "Inter", 88, "Bold"); + p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1); + configFont(p, "Inter", 66, "Regular"); + p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2); + } else if (alert.size == cereal::ControlsState::AlertSize::FULL) { + bool l = alert.text1.length() > 15; + configFont(p, "Inter", l ? 132 : 177, "Bold"); + p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1); + configFont(p, "Inter", 88, "Regular"); + p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2); + } +} + +// NvgWindow + +NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); +} + +void NvgWindow::updateState(const UIState &s) { + const int SET_SPEED_NA = 255; + const SubMaster &sm = *(s.sm); + + const bool cs_alive = sm.alive("controlsState"); + const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid(); + + const auto cs = sm["controlsState"].getControlsState(); + + // Handle older routes where vCruiseCluster is not set + float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster(); + float set_speed = cs_alive ? v_cruise : SET_SPEED_NA; + bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA; + if (cruise_set && !s.scene.is_metric) { + set_speed *= KM_TO_MILE; + } + + // Handle older routes where vEgoCluster is not set + float v_ego; + if (sm["carState"].getCarState().getVEgoCluster() == 0.0 && !v_ego_cluster_seen) { + v_ego = sm["carState"].getCarState().getVEgo(); + } else { + v_ego = sm["carState"].getCarState().getVEgoCluster(); + v_ego_cluster_seen = true; + } + float cur_speed = cs_alive ? std::max(0.0, v_ego) : 0.0; + cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; + + auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign(); + float speed_limit = nav_alive ? sm["navInstruction"].getNavInstruction().getSpeedLimit() : 0.0; + speed_limit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + + setProperty("speedLimit", speed_limit); + setProperty("has_us_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); + setProperty("has_eu_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); + + setProperty("is_cruise_set", cruise_set); + setProperty("is_metric", s.scene.is_metric); + setProperty("speed", cur_speed); + setProperty("setSpeed", set_speed); + setProperty("speedUnit", s.scene.is_metric ? tr("km/h") : tr("mph")); + setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); + setProperty("status", s.status); + + // update engageability and DM icons at 2Hz + if (sm.frame % (UI_FREQ / 2) == 0) { + setProperty("engageable", cs.getEngageable() || cs.getEnabled()); + setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); + setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); + } + + if (s.scene.calibration_valid) { + CameraViewWidget::updateCalibration(s.scene.view_from_calib); + } else { + CameraViewWidget::updateCalibration(DEFAULT_CALIBRATION); + } +} + +void NvgWindow::drawHud(QPainter &p) { + p.save(); + + // Header gradient + QLinearGradient bg(0, header_h - (header_h / 2.5), 0, header_h); + bg.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.45)); + bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0)); + p.fillRect(0, 0, width(), header_h, bg); + + QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–"; + QString speedStr = QString::number(std::nearbyint(speed)); + QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–"; + + // Draw outer box + border to contain set speed and speed limit + int default_rect_width = 172; + int rect_width = default_rect_width; + if (is_metric || has_eu_speed_limit) rect_width = 200; + if (has_us_speed_limit && speedLimitStr.size() >= 3) rect_width = 223; + + int rect_height = 204; + if (has_us_speed_limit) rect_height = 402; + else if (has_eu_speed_limit) rect_height = 392; + + int top_radius = 32; + int bottom_radius = has_eu_speed_limit ? 100 : 32; + + QRect set_speed_rect(60 + default_rect_width / 2 - rect_width / 2, 45, rect_width, rect_height); + p.setPen(QPen(whiteColor(75), 6)); + p.setBrush(blackColor(166)); + drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius); + + // Draw MAX + if (is_cruise_set) { + if (status == STATUS_DISENGAGED) { + p.setPen(whiteColor()); + } else if (status == STATUS_OVERRIDE) { + p.setPen(QColor(0x91, 0x9b, 0x95, 0xff)); + } else if (speedLimit > 0) { + p.setPen(interpColor( + setSpeed, + {speedLimit + 5, speedLimit + 15, speedLimit + 25}, + {QColor(0x80, 0xd8, 0xa6, 0xff), QColor(0xff, 0xe4, 0xbf, 0xff), QColor(0xff, 0xbf, 0xbf, 0xff)} + )); + } else { + p.setPen(QColor(0x80, 0xd8, 0xa6, 0xff)); + } + } else { + p.setPen(QColor(0xa6, 0xa6, 0xa6, 0xff)); + } + configFont(p, "Inter", 40, "SemiBold"); + QRect max_rect = getTextRect(p, Qt::AlignCenter, tr("MAX")); + max_rect.moveCenter({set_speed_rect.center().x(), 0}); + max_rect.moveTop(set_speed_rect.top() + 27); + p.drawText(max_rect, Qt::AlignCenter, tr("MAX")); + + // Draw set speed + if (is_cruise_set) { + if (speedLimit > 0 && status != STATUS_DISENGAGED && status != STATUS_OVERRIDE) { + p.setPen(interpColor( + setSpeed, + {speedLimit + 5, speedLimit + 15, speedLimit + 25}, + {whiteColor(), QColor(0xff, 0x95, 0x00, 0xff), QColor(0xff, 0x00, 0x00, 0xff)} + )); + } else { + p.setPen(whiteColor()); + } + } else { + p.setPen(QColor(0x72, 0x72, 0x72, 0xff)); + } + configFont(p, "Inter", 90, "Bold"); + QRect speed_rect = getTextRect(p, Qt::AlignCenter, setSpeedStr); + speed_rect.moveCenter({set_speed_rect.center().x(), 0}); + speed_rect.moveTop(set_speed_rect.top() + 77); + p.drawText(speed_rect, Qt::AlignCenter, setSpeedStr); + + + + // US/Canada (MUTCD style) sign + if (has_us_speed_limit) { + const int border_width = 6; + const int sign_width = rect_width - 24; + const int sign_height = 186; + + // White outer square + QRect sign_rect_outer(set_speed_rect.left() + 12, set_speed_rect.bottom() - 11 - sign_height, sign_width, sign_height); + p.setPen(Qt::NoPen); + p.setBrush(whiteColor()); + p.drawRoundedRect(sign_rect_outer, 24, 24); + + // Smaller white square with black border + QRect sign_rect(sign_rect_outer.left() + 1.5 * border_width, sign_rect_outer.top() + 1.5 * border_width, sign_width - 3 * border_width, sign_height - 3 * border_width); + p.setPen(QPen(blackColor(), border_width)); + p.setBrush(whiteColor()); + p.drawRoundedRect(sign_rect, 16, 16); + + // "SPEED" + configFont(p, "Inter", 28, "SemiBold"); + QRect text_speed_rect = getTextRect(p, Qt::AlignCenter, tr("SPEED")); + text_speed_rect.moveCenter({sign_rect.center().x(), 0}); + text_speed_rect.moveTop(sign_rect_outer.top() + 22); + p.drawText(text_speed_rect, Qt::AlignCenter, tr("SPEED")); + + // "LIMIT" + QRect text_limit_rect = getTextRect(p, Qt::AlignCenter, tr("LIMIT")); + text_limit_rect.moveCenter({sign_rect.center().x(), 0}); + text_limit_rect.moveTop(sign_rect_outer.top() + 51); + p.drawText(text_limit_rect, Qt::AlignCenter, tr("LIMIT")); + + // Speed limit value + configFont(p, "Inter", 70, "Bold"); + QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr); + speed_limit_rect.moveCenter({sign_rect.center().x(), 0}); + speed_limit_rect.moveTop(sign_rect_outer.top() + 85); + p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr); + } + + // EU (Vienna style) sign + if (has_eu_speed_limit) { + int outer_radius = 176 / 2; + int inner_radius_1 = outer_radius - 6; // White outer border + int inner_radius_2 = inner_radius_1 - 20; // Red circle + + // Draw white circle with red border + QPoint center(set_speed_rect.center().x() + 1, set_speed_rect.top() + 204 + outer_radius); + p.setPen(Qt::NoPen); + p.setBrush(whiteColor()); + p.drawEllipse(center, outer_radius, outer_radius); + p.setBrush(QColor(255, 0, 0, 255)); + p.drawEllipse(center, inner_radius_1, inner_radius_1); + p.setBrush(whiteColor()); + p.drawEllipse(center, inner_radius_2, inner_radius_2); + + // Speed limit value + int font_size = (speedLimitStr.size() >= 3) ? 60 : 70; + configFont(p, "Inter", font_size, "Bold"); + QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr); + speed_limit_rect.moveCenter(center); + p.setPen(blackColor()); + p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr); + } + + // current speed + configFont(p, "Inter", 176, "Bold"); + drawText(p, rect().center().x(), 210, speedStr); + configFont(p, "Inter", 66, "Regular"); + drawText(p, rect().center().x(), 290, speedUnit, 200); + + // engage-ability icon + if (engageable) { + drawIcon(p, rect().right() - radius / 2 - bdr_s * 2, radius / 2 + int(bdr_s * 1.5), + engage_img, bg_colors[status], 1.0); + } + + // dm icon + if (!hideDM) { + int dm_icon_x = rightHandDM ? rect().right() - radius / 2 - (bdr_s * 2) : radius / 2 + (bdr_s * 2); + drawIcon(p, dm_icon_x, rect().bottom() - footer_h / 2, + dm_img, blackColor(70), dmActive ? 1.0 : 0.2); + } + p.restore(); +} + +void NvgWindow::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { + QRect real_rect = getTextRect(p, 0, text); + real_rect.moveCenter({x, y - real_rect.height() / 2}); + + p.setPen(QColor(0xff, 0xff, 0xff, alpha)); + p.drawText(real_rect.x(), real_rect.bottom(), text); +} + +void NvgWindow::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) { + p.setPen(Qt::NoPen); + p.setBrush(bg); + p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); + p.setOpacity(opacity); + p.drawPixmap(x - img_size / 2, y - img_size / 2, img); +} + + +void NvgWindow::initializeGL() { + CameraViewWidget::initializeGL(); + qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION)); + qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR)); + qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER)); + qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION)); + + prev_draw_t = millis_since_boot(); + setBackgroundColor(bg_colors[STATUS_DISENGAGED]); +} + +void NvgWindow::updateFrameMat() { + CameraViewWidget::updateFrameMat(); + UIState *s = uiState(); + int w = width(), h = height(); + + s->fb_w = w; + s->fb_h = h; + + // Apply transformation such that video pixel coordinates match video + // 1) Put (0, 0) in the middle of the video + // 2) Apply same scaling as video + // 3) Put (0, 0) in top left corner of video + s->car_space_transform.reset(); + s->car_space_transform.translate(w / 2 - x_offset, h / 2 - y_offset) + .scale(zoom, zoom) + .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); +} + +void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) { + painter.save(); + + const UIScene &scene = s->scene; + // lanelines + for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) { + painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(scene.lane_line_probs[i], 0.0, 0.7))); + painter.drawPolygon(scene.lane_line_vertices[i]); + } + + // road edges + for (int i = 0; i < std::size(scene.road_edge_vertices); ++i) { + painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0))); + painter.drawPolygon(scene.road_edge_vertices[i]); + } + + // paint path + QLinearGradient bg(0, height(), 0, height() / 4); + float start_hue, end_hue; + if (scene.end_to_end_long) { + const auto &acceleration = (*s->sm)["modelV2"].getModelV2().getAcceleration(); + float acceleration_future = 0; + if (acceleration.getZ().size() > 16) { + acceleration_future = acceleration.getX()[16]; // 2.5 seconds + } + start_hue = 60; + // speed up: 120, slow down: 0 + end_hue = fmax(fmin(start_hue + acceleration_future * 30, 120), 0); + + // FIXME: painter.drawPolygon can be slow if hue is not rounded + end_hue = int(end_hue * 100 + 0.5) / 100; + + bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); + bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); + bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); + } else { + const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation(); + float orientation_future = 0; + if (orientation.getZ().size() > 16) { + orientation_future = std::abs(orientation.getZ()[16]); // 2.5 seconds + } + start_hue = 148; + // straight: 112, in turns: 70 + end_hue = fmax(70, 112 - (orientation_future * 420)); + + // FIXME: painter.drawPolygon can be slow if hue is not rounded + end_hue = int(end_hue * 100 + 0.5) / 100; + + bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.94, 0.51, 0.4)); + bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); + bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); + } + + painter.setBrush(bg); + painter.drawPolygon(scene.track_vertices); + + painter.restore(); +} + +void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd) { + painter.save(); + + const float speedBuff = 10.; + const float leadBuff = 40.; + const float d_rel = lead_data.getX()[0]; + const float v_rel = lead_data.getV()[0]; + + float fillAlpha = 0; + if (d_rel < leadBuff) { + fillAlpha = 255 * (1.0 - (d_rel / leadBuff)); + if (v_rel < 0) { + fillAlpha += 255 * (-1 * (v_rel / speedBuff)); + } + fillAlpha = (int)(fmin(fillAlpha, 255)); + } + + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35; + float x = std::clamp((float)vd.x(), 0.f, width() - sz / 2); + float y = std::fmin(height() - sz * .6, (float)vd.y()); + + float g_xo = sz / 5; + float g_yo = sz / 10; + + QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; + painter.setBrush(QColor(218, 202, 37, 255)); + painter.drawPolygon(glow, std::size(glow)); + + // chevron + QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; + painter.setBrush(redColor(fillAlpha)); + painter.drawPolygon(chevron, std::size(chevron)); + + painter.restore(); +} + +void NvgWindow::paintGL() { + UIState *s = uiState(); + const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2(); + CameraViewWidget::setFrameId(model.getFrameId()); + CameraViewWidget::paintGL(); + + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + if (s->worldObjectsVisible()) { + + drawLaneLines(painter, s); + + if (s->scene.longitudinal_control) { + const auto leads = model.getLeadsV3(); + if (leads[0].getProb() > .5) { + drawLead(painter, leads[0], s->scene.lead_vertices[0]); + } + if (leads[1].getProb() > .5 && (std::abs(leads[1].getX()[0] - leads[0].getX()[0]) > 3.0)) { + drawLead(painter, leads[1], s->scene.lead_vertices[1]); + } + } + } + + drawHud(painter); + + double cur_draw_t = millis_since_boot(); + double dt = cur_draw_t - prev_draw_t; + double fps = fps_filter.update(1. / dt * 1000); + if (fps < 15) { + LOGW("slow frame rate: %.2f fps", fps); + } + prev_draw_t = cur_draw_t; +} + +void NvgWindow::showEvent(QShowEvent *event) { + CameraViewWidget::showEvent(event); + + ui_update_params(uiState()); + prev_draw_t = millis_since_boot(); +} diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h new file mode 100644 index 00000000000000..25920ccc6a3d08 --- /dev/null +++ b/selfdrive/ui/qt/onroad.h @@ -0,0 +1,108 @@ +#pragma once + +#include +#include + +#include "common/util.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" +#include "selfdrive/ui/ui.h" + + +// ***** onroad widgets ***** +class OnroadAlerts : public QWidget { + Q_OBJECT + +public: + OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {}; + void updateAlert(const Alert &a, const QColor &color); + +protected: + void paintEvent(QPaintEvent*) override; + +private: + QColor bg; + Alert alert = {}; +}; + +// container window for the NVG UI +class NvgWindow : public CameraViewWidget { + Q_OBJECT + Q_PROPERTY(float speed MEMBER speed); + Q_PROPERTY(QString speedUnit MEMBER speedUnit); + Q_PROPERTY(float setSpeed MEMBER setSpeed); + Q_PROPERTY(float speedLimit MEMBER speedLimit); + Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set); + Q_PROPERTY(bool has_eu_speed_limit MEMBER has_eu_speed_limit); + Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit); + Q_PROPERTY(bool is_metric MEMBER is_metric); + + Q_PROPERTY(bool engageable MEMBER engageable); + Q_PROPERTY(bool dmActive MEMBER dmActive); + Q_PROPERTY(bool hideDM MEMBER hideDM); + Q_PROPERTY(bool rightHandDM MEMBER rightHandDM); + Q_PROPERTY(int status MEMBER status); + +public: + explicit NvgWindow(VisionStreamType type, QWidget* parent = 0); + void updateState(const UIState &s); + +private: + void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); + void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); + + QPixmap engage_img; + QPixmap dm_img; + const int radius = 192; + const int img_size = (radius / 2) * 1.5; + float speed; + QString speedUnit; + float setSpeed; + float speedLimit; + bool is_cruise_set = false; + bool is_metric = false; + bool engageable = false; + bool dmActive = false; + bool hideDM = false; + bool rightHandDM = false; + bool has_us_speed_limit = false; + bool has_eu_speed_limit = false; + bool v_ego_cluster_seen = false; + int status = STATUS_DISENGAGED; + +protected: + void paintGL() override; + void initializeGL() override; + void showEvent(QShowEvent *event) override; + void updateFrameMat() override; + void drawLaneLines(QPainter &painter, const UIState *s); + void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd); + void drawHud(QPainter &p); + inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } + inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } + + double prev_draw_t = 0; + FirstOrderFilter fps_filter; +}; + +// container for all onroad widgets +class OnroadWindow : public QWidget { + Q_OBJECT + +public: + OnroadWindow(QWidget* parent = 0); + bool isMapVisible() const { return map && map->isVisible(); } + +private: + void paintEvent(QPaintEvent *event); + void mousePressEvent(QMouseEvent* e) override; + OnroadAlerts *alerts; + NvgWindow *nvg; + QColor bg = bg_colors[STATUS_DISENGAGED]; + QWidget *map = nullptr; + QHBoxLayout* split; + +private slots: + void offroadTransition(bool offroad); + void updateState(const UIState &s); +}; diff --git a/selfdrive/ui/qt/python_helpers.py b/selfdrive/ui/qt/python_helpers.py new file mode 100644 index 00000000000000..905d41a6344f7e --- /dev/null +++ b/selfdrive/ui/qt/python_helpers.py @@ -0,0 +1,20 @@ +import os +from cffi import FFI + +import sip # pylint: disable=import-error + +from common.ffi_wrapper import suffix +from common.basedir import BASEDIR + + +def get_ffi(): + lib = os.path.join(BASEDIR, "selfdrive", "ui", "qt", "libpython_helpers" + suffix()) + + ffi = FFI() + ffi.cdef("void set_main_window(void *w);") + return ffi, ffi.dlopen(lib) + + +def set_main_window(widget): + ffi, lib = get_ffi() + lib.set_main_window(ffi.cast('void*', sip.unwrapinstance(widget))) diff --git a/selfdrive/ui/qt/qt_window.cc b/selfdrive/ui/qt/qt_window.cc new file mode 100644 index 00000000000000..d630b560bb967f --- /dev/null +++ b/selfdrive/ui/qt/qt_window.cc @@ -0,0 +1,30 @@ +#include "selfdrive/ui/qt/qt_window.h" + +void setMainWindow(QWidget *w) { + const QSize sz = QGuiApplication::primaryScreen()->size(); + if (Hardware::PC() && sz.width() <= 1920 && sz.height() <= 1080 && getenv("SCALE") == nullptr) { + w->setMinimumSize(QSize(640, 480)); // allow resize smaller than fullscreen + w->setMaximumSize(QSize(2160, 1080)); + w->resize(sz); + } else { + const float scale = util::getenv("SCALE", 1.0f); + const bool wide = (sz.width() >= WIDE_WIDTH) ^ (getenv("INVERT_WIDTH") != NULL); + w->setFixedSize(QSize(wide ? WIDE_WIDTH : 1920, 1080) * scale); + } + w->show(); + +#ifdef QCOM2 + QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); + wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w->windowHandle())); + wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); + wl_surface_commit(s); + w->showFullScreen(); +#endif +} + + +extern "C" { + void set_main_window(void *w) { + setMainWindow((QWidget*)w); + } +} diff --git a/selfdrive/ui/qt/qt_window.h b/selfdrive/ui/qt/qt_window.h new file mode 100644 index 00000000000000..02d127e7ff8905 --- /dev/null +++ b/selfdrive/ui/qt/qt_window.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include +#include +#include + +#ifdef QCOM2 +#include +#include +#include +#endif + +#include "system/hardware/hw.h" + +const QString ASSET_PATH = ":/"; + +const int WIDE_WIDTH = 2160; + +void setMainWindow(QWidget *w); diff --git a/selfdrive/ui/qt/request_repeater.cc b/selfdrive/ui/qt/request_repeater.cc new file mode 100644 index 00000000000000..fa37c015f7b6e4 --- /dev/null +++ b/selfdrive/ui/qt/request_repeater.cc @@ -0,0 +1,27 @@ +#include "selfdrive/ui/qt/request_repeater.h" + +RequestRepeater::RequestRepeater(QObject *parent, const QString &requestURL, const QString &cacheKey, + int period, bool while_onroad) : HttpRequest(parent) { + timer = new QTimer(this); + timer->setTimerType(Qt::VeryCoarseTimer); + QObject::connect(timer, &QTimer::timeout, [=]() { + if ((!uiState()->scene.started || while_onroad) && uiState()->awake && !active()) { + sendRequest(requestURL); + } + }); + + timer->start(period * 1000); + + if (!cacheKey.isEmpty()) { + prevResp = QString::fromStdString(params.get(cacheKey.toStdString())); + if (!prevResp.isEmpty()) { + QTimer::singleShot(500, [=]() { emit requestDone(prevResp, true, QNetworkReply::NoError); }); + } + QObject::connect(this, &HttpRequest::requestDone, [=](const QString &resp, bool success) { + if (success && resp != prevResp) { + params.put(cacheKey.toStdString(), resp.toStdString()); + prevResp = resp; + } + }); + } +} diff --git a/selfdrive/ui/qt/request_repeater.h b/selfdrive/ui/qt/request_repeater.h new file mode 100644 index 00000000000000..c0e27582736138 --- /dev/null +++ b/selfdrive/ui/qt/request_repeater.h @@ -0,0 +1,15 @@ +#pragma once + +#include "common/util.h" +#include "selfdrive/ui/qt/api.h" +#include "selfdrive/ui/ui.h" + +class RequestRepeater : public HttpRequest { +public: + RequestRepeater(QObject *parent, const QString &requestURL, const QString &cacheKey = "", int period = 0, bool while_onroad=false); + +private: + Params params; + QTimer *timer; + QString prevResp; +}; diff --git a/selfdrive/ui/qt/setup/reset.cc b/selfdrive/ui/qt/setup/reset.cc new file mode 100644 index 00000000000000..582217c1d7c064 --- /dev/null +++ b/selfdrive/ui/qt/setup/reset.cc @@ -0,0 +1,116 @@ +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/setup/reset.h" + +#define NVME "/dev/nvme0n1" +#define USERDATA "/dev/disk/by-partlabel/userdata" + +void Reset::doReset() { + // best effort to wipe nvme and sd card + std::system("sudo umount " NVME); + std::system("yes | sudo mkfs.ext4 " NVME); + + // we handle two cases here + // * user-prompted factory reset + // * recovering from a corrupt userdata by formatting + int rm = std::system("sudo rm -rf /data/*"); + std::system("sudo umount " USERDATA); + int fmt = std::system("yes | sudo mkfs.ext4 " USERDATA); + + if (rm == 0 || fmt == 0) { + std::system("sudo reboot"); + } + body->setText(tr("Reset failed. Reboot to try again.")); + rebootBtn->show(); +} + +void Reset::confirm() { + const QString confirm_txt = tr("Are you sure you want to reset your device?"); + if (body->text() != confirm_txt) { + body->setText(confirm_txt); + } else { + body->setText(tr("Resetting device...")); + rejectBtn->hide(); + rebootBtn->hide(); + confirmBtn->hide(); +#ifdef __aarch64__ + QTimer::singleShot(100, this, &Reset::doReset); +#endif + } +} + +Reset::Reset(bool recover, QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(45, 220, 45, 45); + main_layout->setSpacing(0); + + QLabel *title = new QLabel(tr("System Reset")); + title->setStyleSheet("font-size: 90px; font-weight: 600;"); + main_layout->addWidget(title, 0, Qt::AlignTop | Qt::AlignLeft); + + main_layout->addSpacing(60); + + body = new QLabel(tr("System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot.")); + body->setWordWrap(true); + body->setStyleSheet("font-size: 80px; font-weight: light;"); + main_layout->addWidget(body, 1, Qt::AlignTop | Qt::AlignLeft); + + QHBoxLayout *blayout = new QHBoxLayout(); + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + rejectBtn = new QPushButton(tr("Cancel")); + blayout->addWidget(rejectBtn); + QObject::connect(rejectBtn, &QPushButton::clicked, QCoreApplication::instance(), &QCoreApplication::quit); + + rebootBtn = new QPushButton(tr("Reboot")); + blayout->addWidget(rebootBtn); +#ifdef __aarch64__ + QObject::connect(rebootBtn, &QPushButton::clicked, [=]{ + std::system("sudo reboot"); + }); +#endif + + confirmBtn = new QPushButton(tr("Confirm")); + confirmBtn->setStyleSheet("background-color: #465BEA;"); + blayout->addWidget(confirmBtn); + QObject::connect(confirmBtn, &QPushButton::clicked, this, &Reset::confirm); + + rejectBtn->setVisible(!recover); + rebootBtn->setVisible(recover); + if (recover) { + body->setText(tr("Unable to mount data partition. Press confirm to reset your device.")); + } + + setStyleSheet(R"( + * { + font-family: Inter; + color: white; + background-color: black; + } + QLabel { + margin-left: 140; + } + QPushButton { + height: 160; + font-size: 55px; + font-weight: 400; + border-radius: 10px; + background-color: #333333; + } + )"); +} + +int main(int argc, char *argv[]) { + bool recover = argc > 1 && strcmp(argv[1], "--recover") == 0; + QApplication a(argc, argv); + Reset reset(recover); + setMainWindow(&reset); + return a.exec(); +} diff --git a/selfdrive/ui/qt/setup/reset.h b/selfdrive/ui/qt/setup/reset.h new file mode 100644 index 00000000000000..3a4994077c0818 --- /dev/null +++ b/selfdrive/ui/qt/setup/reset.h @@ -0,0 +1,20 @@ +#include +#include +#include + +class Reset : public QWidget { + Q_OBJECT + +public: + explicit Reset(bool recover = false, QWidget *parent = 0); + +private: + QLabel *body; + QPushButton *rejectBtn; + QPushButton *rebootBtn; + QPushButton *confirmBtn; + void doReset(); + +private slots: + void confirm(); +}; diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc new file mode 100644 index 00000000000000..69dafcf741e8d9 --- /dev/null +++ b/selfdrive/ui/qt/setup/setup.cc @@ -0,0 +1,430 @@ +#include "selfdrive/ui/qt/setup/setup.h" + +#include +#include +#include + +#include +#include +#include + +#include + +#include "common/util.h" +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/api.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/offroad/networking.h" +#include "selfdrive/ui/qt/widgets/input.h" + +const std::string USER_AGENT = "AGNOSSetup-"; +const QString DASHCAM_URL = "https://dashcam.comma.ai"; + +void Setup::download(QString url) { + CURL *curl = curl_easy_init(); + if (!curl) { + emit finished(false); + return; + } + + auto version = util::read_file("/VERSION"); + + char tmpfile[] = "/tmp/installer_XXXXXX"; + FILE *fp = fdopen(mkstemp(tmpfile), "w"); + + curl_easy_setopt(curl, CURLOPT_URL, url.toStdString().c_str()); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, NULL); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, fp); + curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); + curl_easy_setopt(curl, CURLOPT_USERAGENT, (USER_AGENT + version).c_str()); + + int ret = curl_easy_perform(curl); + + long res_status = 0; + curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &res_status); + if (ret == CURLE_OK && res_status == 200) { + rename(tmpfile, "/tmp/installer"); + emit finished(true); + } else { + emit finished(false); + } + + curl_easy_cleanup(curl); + fclose(fp); +} + +QWidget * Setup::low_voltage() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + main_layout->setContentsMargins(55, 0, 55, 55); + main_layout->setSpacing(0); + + // inner text layout: warning icon, title, and body + QVBoxLayout *inner_layout = new QVBoxLayout(); + inner_layout->setContentsMargins(110, 144, 365, 0); + main_layout->addLayout(inner_layout); + + QLabel *triangle = new QLabel(); + triangle->setPixmap(QPixmap(ASSET_PATH + "offroad/icon_warning.png")); + inner_layout->addWidget(triangle, 0, Qt::AlignTop | Qt::AlignLeft); + inner_layout->addSpacing(80); + + QLabel *title = new QLabel(tr("WARNING: Low Voltage")); + title->setStyleSheet("font-size: 90px; font-weight: 500; color: #FF594F;"); + inner_layout->addWidget(title, 0, Qt::AlignTop | Qt::AlignLeft); + + inner_layout->addSpacing(25); + + QLabel *body = new QLabel(tr("Power your device in a car with a harness or proceed at your own risk.")); + body->setWordWrap(true); + body->setAlignment(Qt::AlignTop | Qt::AlignLeft); + body->setStyleSheet("font-size: 80px; font-weight: 300;"); + inner_layout->addWidget(body); + + inner_layout->addStretch(); + + // power off + continue buttons + QHBoxLayout *blayout = new QHBoxLayout(); + blayout->setSpacing(50); + main_layout->addLayout(blayout, 0); + + QPushButton *poweroff = new QPushButton(tr("Power off")); + poweroff->setObjectName("navBtn"); + blayout->addWidget(poweroff); + QObject::connect(poweroff, &QPushButton::clicked, this, [=]() { + Hardware::poweroff(); + }); + + QPushButton *cont = new QPushButton(tr("Continue")); + cont->setObjectName("navBtn"); + blayout->addWidget(cont); + QObject::connect(cont, &QPushButton::clicked, this, &Setup::nextPage); + + return widget; +} + +QWidget * Setup::getting_started() { + QWidget *widget = new QWidget(); + + QHBoxLayout *main_layout = new QHBoxLayout(widget); + main_layout->setMargin(0); + + QVBoxLayout *vlayout = new QVBoxLayout(); + vlayout->setContentsMargins(165, 280, 100, 0); + main_layout->addLayout(vlayout); + + QLabel *title = new QLabel(tr("Getting Started")); + title->setStyleSheet("font-size: 90px; font-weight: 500;"); + vlayout->addWidget(title, 0, Qt::AlignTop | Qt::AlignLeft); + + vlayout->addSpacing(90); + QLabel *desc = new QLabel(tr("Before we get on the road, let’s finish installation and cover some details.")); + desc->setWordWrap(true); + desc->setStyleSheet("font-size: 80px; font-weight: 300;"); + vlayout->addWidget(desc, 0, Qt::AlignTop | Qt::AlignLeft); + + vlayout->addStretch(); + + QPushButton *btn = new QPushButton(); + btn->setIcon(QIcon(":/img_continue_triangle.svg")); + btn->setIconSize(QSize(54, 106)); + btn->setFixedSize(310, 1080); + btn->setProperty("primary", true); + btn->setStyleSheet("border: none;"); + main_layout->addWidget(btn, 0, Qt::AlignRight); + QObject::connect(btn, &QPushButton::clicked, this, &Setup::nextPage); + + return widget; +} + +QWidget * Setup::network_setup() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + main_layout->setContentsMargins(55, 50, 55, 50); + + // title + QLabel *title = new QLabel(tr("Connect to Wi-Fi")); + title->setStyleSheet("font-size: 90px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); + + main_layout->addSpacing(25); + + // wifi widget + Networking *networking = new Networking(this, false); + networking->setStyleSheet("Networking {background-color: #292929; border-radius: 13px;}"); + main_layout->addWidget(networking, 1); + + main_layout->addSpacing(35); + + // back + continue buttons + QHBoxLayout *blayout = new QHBoxLayout; + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + QPushButton *back = new QPushButton(tr("Back")); + back->setObjectName("navBtn"); + QObject::connect(back, &QPushButton::clicked, this, &Setup::prevPage); + blayout->addWidget(back); + + QPushButton *cont = new QPushButton(); + cont->setObjectName("navBtn"); + cont->setProperty("primary", true); + QObject::connect(cont, &QPushButton::clicked, this, &Setup::nextPage); + blayout->addWidget(cont); + + // setup timer for testing internet connection + HttpRequest *request = new HttpRequest(this, false, 2500); + QObject::connect(request, &HttpRequest::requestDone, [=](const QString &, bool success) { + cont->setEnabled(success); + if (success) { + const bool cell = networking->wifi->currentNetworkType() == NetworkType::CELL; + cont->setText(cell ? tr("Continue without Wi-Fi") : tr("Continue")); + } else { + cont->setText(tr("Waiting for internet")); + } + repaint(); + }); + request->sendRequest(DASHCAM_URL); + QTimer *timer = new QTimer(this); + QObject::connect(timer, &QTimer::timeout, [=]() { + if (!request->active() && cont->isVisible()) { + request->sendRequest(DASHCAM_URL); + } + }); + timer->start(1000); + + return widget; +} + +QWidget * radio_button(QString title, QButtonGroup *group) { + QPushButton *btn = new QPushButton(title); + btn->setCheckable(true); + group->addButton(btn); + btn->setStyleSheet(R"( + QPushButton { + height: 230; + padding-left: 100px; + padding-right: 100px; + text-align: left; + font-size: 80px; + font-weight: 400; + border-radius: 10px; + background-color: #4F4F4F; + } + QPushButton:checked { + background-color: #465BEA; + } + )"); + + // checkmark icon + QPixmap pix(":/img_circled_check.svg"); + btn->setIcon(pix); + btn->setIconSize(QSize(0, 0)); + btn->setLayoutDirection(Qt::RightToLeft); + QObject::connect(btn, &QPushButton::toggled, [=](bool checked) { + btn->setIconSize(checked ? QSize(104, 104) : QSize(0, 0)); + }); + return btn; +} + +QWidget * Setup::software_selection() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + main_layout->setContentsMargins(55, 50, 55, 50); + main_layout->setSpacing(0); + + // title + QLabel *title = new QLabel(tr("Choose Software to Install")); + title->setStyleSheet("font-size: 90px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); + + main_layout->addSpacing(50); + + // dashcam + custom radio buttons + QButtonGroup *group = new QButtonGroup(widget); + group->setExclusive(true); + + QWidget *dashcam = radio_button(tr("Dashcam"), group); + main_layout->addWidget(dashcam); + + main_layout->addSpacing(30); + + QWidget *custom = radio_button(tr("Custom Software"), group); + main_layout->addWidget(custom); + + main_layout->addStretch(); + + // back + continue buttons + QHBoxLayout *blayout = new QHBoxLayout; + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + QPushButton *back = new QPushButton(tr("Back")); + back->setObjectName("navBtn"); + QObject::connect(back, &QPushButton::clicked, this, &Setup::prevPage); + blayout->addWidget(back); + + QPushButton *cont = new QPushButton(tr("Continue")); + cont->setObjectName("navBtn"); + cont->setEnabled(false); + cont->setProperty("primary", true); + blayout->addWidget(cont); + + QObject::connect(cont, &QPushButton::clicked, [=]() { + auto w = currentWidget(); + QTimer::singleShot(0, [=]() { + setCurrentWidget(downloading_widget); + }); + QString url = DASHCAM_URL; + if (group->checkedButton() != dashcam) { + url = InputDialog::getText(tr("Enter URL"), this, tr("for Custom Software")); + } + if (!url.isEmpty()) { + QTimer::singleShot(1000, this, [=]() { + download(url); + }); + } else { + setCurrentWidget(w); + } + }); + + connect(group, QOverload::of(&QButtonGroup::buttonClicked), [=](QAbstractButton *btn) { + btn->setChecked(true); + cont->setEnabled(true); + }); + + return widget; +} + +QWidget * Setup::downloading() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + QLabel *txt = new QLabel(tr("Downloading...")); + txt->setStyleSheet("font-size: 90px; font-weight: 500;"); + main_layout->addWidget(txt, 0, Qt::AlignCenter); + return widget; +} + +QWidget * Setup::download_failed() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + main_layout->setContentsMargins(55, 225, 55, 55); + main_layout->setSpacing(0); + + QLabel *title = new QLabel(tr("Download Failed")); + title->setStyleSheet("font-size: 90px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignTop | Qt::AlignLeft); + + main_layout->addSpacing(67); + + QLabel *body = new QLabel(tr("Ensure the entered URL is valid, and the device’s internet connection is good.")); + body->setWordWrap(true); + body->setAlignment(Qt::AlignTop | Qt::AlignLeft); + body->setStyleSheet("font-size: 80px; font-weight: 300; margin-right: 100px;"); + main_layout->addWidget(body); + + main_layout->addStretch(); + + // reboot + start over buttons + QHBoxLayout *blayout = new QHBoxLayout(); + blayout->setSpacing(50); + main_layout->addLayout(blayout, 0); + + QPushButton *reboot = new QPushButton(tr("Reboot device")); + reboot->setObjectName("navBtn"); + blayout->addWidget(reboot); + QObject::connect(reboot, &QPushButton::clicked, this, [=]() { + Hardware::reboot(); + }); + + QPushButton *restart = new QPushButton(tr("Start over")); + restart->setObjectName("navBtn"); + restart->setProperty("primary", true); + blayout->addWidget(restart); + QObject::connect(restart, &QPushButton::clicked, this, [=]() { + setCurrentIndex(2); + }); + + widget->setStyleSheet(R"( + QLabel { + margin-left: 117; + } + )"); + return widget; +} + +void Setup::prevPage() { + setCurrentIndex(currentIndex() - 1); +} + +void Setup::nextPage() { + setCurrentIndex(currentIndex() + 1); +} + +Setup::Setup(QWidget *parent) : QStackedWidget(parent) { + std::stringstream buffer; + buffer << std::ifstream("/sys/class/hwmon/hwmon1/in1_input").rdbuf(); + float voltage = (float)std::atoi(buffer.str().c_str()) / 1000.; + if (voltage < 7) { + addWidget(low_voltage()); + } + + addWidget(getting_started()); + addWidget(network_setup()); + addWidget(software_selection()); + + downloading_widget = downloading(); + addWidget(downloading_widget); + + failed_widget = download_failed(); + addWidget(failed_widget); + + QObject::connect(this, &Setup::finished, [=](bool success) { + // hide setup on success + qDebug() << "finished" << success; + if (success) { + QTimer::singleShot(3000, this, &QWidget::hide); + } else { + setCurrentWidget(failed_widget); + } + }); + + // TODO: revisit pressed bg color + setStyleSheet(R"( + * { + color: white; + font-family: Inter; + } + Setup { + background-color: black; + } + QPushButton#navBtn { + height: 160; + font-size: 55px; + font-weight: 400; + border-radius: 10px; + background-color: #333333; + } + QPushButton#navBtn:disabled, QPushButton[primary='true']:disabled { + color: #808080; + background-color: #333333; + } + QPushButton#navBtn:pressed { + background-color: #444444; + } + QPushButton[primary='true'], #navBtn[primary='true'] { + background-color: #465BEA; + } + QPushButton[primary='true']:pressed, #navBtn:pressed[primary='true'] { + background-color: #3049F4; + } + )"); +} + +int main(int argc, char *argv[]) { + QApplication a(argc, argv); + Setup setup; + setMainWindow(&setup); + return a.exec(); +} diff --git a/selfdrive/ui/qt/setup/setup.h b/selfdrive/ui/qt/setup/setup.h new file mode 100644 index 00000000000000..8027e8bd4fd911 --- /dev/null +++ b/selfdrive/ui/qt/setup/setup.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +class Setup : public QStackedWidget { + Q_OBJECT + +public: + explicit Setup(QWidget *parent = 0); + +private: + QWidget *low_voltage(); + QWidget *getting_started(); + QWidget *network_setup(); + QWidget *software_selection(); + QWidget *downloading(); + QWidget *download_failed(); + + QWidget *failed_widget; + QWidget *downloading_widget; + +signals: + void finished(bool success); + +public slots: + void nextPage(); + void prevPage(); + void download(QString url); +}; diff --git a/selfdrive/ui/qt/setup/updater.cc b/selfdrive/ui/qt/setup/updater.cc new file mode 100644 index 00000000000000..fd7148c5340d42 --- /dev/null +++ b/selfdrive/ui/qt/setup/updater.cc @@ -0,0 +1,176 @@ +#include +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/setup/updater.h" +#include "selfdrive/ui/qt/offroad/networking.h" + +Updater::Updater(const QString &updater_path, const QString &manifest_path, QWidget *parent) + : updater(updater_path), manifest(manifest_path), QStackedWidget(parent) { + + assert(updater.size()); + assert(manifest.size()); + + // initial prompt screen + prompt = new QWidget; + { + QVBoxLayout *layout = new QVBoxLayout(prompt); + layout->setContentsMargins(100, 250, 100, 100); + + QLabel *title = new QLabel(tr("Update Required")); + title->setStyleSheet("font-size: 80px; font-weight: bold;"); + layout->addWidget(title); + + layout->addSpacing(75); + + QLabel *desc = new QLabel(tr("An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB.")); + desc->setWordWrap(true); + desc->setStyleSheet("font-size: 65px;"); + layout->addWidget(desc); + + layout->addStretch(); + + QHBoxLayout *hlayout = new QHBoxLayout; + hlayout->setSpacing(30); + layout->addLayout(hlayout); + + QPushButton *connect = new QPushButton(tr("Connect to Wi-Fi")); + connect->setObjectName("navBtn"); + QObject::connect(connect, &QPushButton::clicked, [=]() { + setCurrentWidget(wifi); + }); + hlayout->addWidget(connect); + + QPushButton *install = new QPushButton(tr("Install")); + install->setObjectName("navBtn"); + install->setStyleSheet("background-color: #465BEA;"); + QObject::connect(install, &QPushButton::clicked, this, &Updater::installUpdate); + hlayout->addWidget(install); + } + + // wifi connection screen + wifi = new QWidget; + { + QVBoxLayout *layout = new QVBoxLayout(wifi); + layout->setContentsMargins(100, 100, 100, 100); + + Networking *networking = new Networking(this, false); + networking->setStyleSheet("Networking { background-color: #292929; border-radius: 13px; }"); + layout->addWidget(networking, 1); + + QPushButton *back = new QPushButton(tr("Back")); + back->setObjectName("navBtn"); + back->setStyleSheet("padding-left: 60px; padding-right: 60px;"); + QObject::connect(back, &QPushButton::clicked, [=]() { + setCurrentWidget(prompt); + }); + layout->addWidget(back, 0, Qt::AlignLeft); + } + + // progress screen + progress = new QWidget; + { + QVBoxLayout *layout = new QVBoxLayout(progress); + layout->setContentsMargins(150, 330, 150, 150); + layout->setSpacing(0); + + text = new QLabel(tr("Loading...")); + text->setStyleSheet("font-size: 90px; font-weight: 600;"); + layout->addWidget(text, 0, Qt::AlignTop); + + layout->addSpacing(100); + + bar = new QProgressBar(); + bar->setRange(0, 100); + bar->setTextVisible(false); + bar->setFixedHeight(72); + layout->addWidget(bar, 0, Qt::AlignTop); + + layout->addStretch(); + + reboot = new QPushButton(tr("Reboot")); + reboot->setObjectName("navBtn"); + reboot->setStyleSheet("padding-left: 60px; padding-right: 60px;"); + QObject::connect(reboot, &QPushButton::clicked, [=]() { + Hardware::reboot(); + }); + layout->addWidget(reboot, 0, Qt::AlignLeft); + reboot->hide(); + + layout->addStretch(); + } + + addWidget(prompt); + addWidget(wifi); + addWidget(progress); + + setStyleSheet(R"( + * { + color: white; + outline: none; + font-family: Inter; + } + Updater { + color: white; + background-color: black; + } + QPushButton#navBtn { + height: 160; + font-size: 55px; + font-weight: 400; + border-radius: 10px; + background-color: #333333; + } + QProgressBar { + border: none; + background-color: #292929; + } + QProgressBar::chunk { + background-color: #364DEF; + } + )"); +} + +void Updater::installUpdate() { + setCurrentWidget(progress); + QObject::connect(&proc, &QProcess::readyReadStandardOutput, this, &Updater::readProgress); + QObject::connect(&proc, QOverload::of(&QProcess::finished), this, &Updater::updateFinished); + proc.setProcessChannelMode(QProcess::ForwardedErrorChannel); + proc.start(updater, {"--swap", manifest}); +} + +void Updater::readProgress() { + auto lines = QString(proc.readAllStandardOutput()); + for (const QString &line : lines.trimmed().split("\n")) { + auto parts = line.split(":"); + if (parts.size() == 2) { + text->setText(parts[0]); + bar->setValue((int)parts[1].toDouble()); + } else { + qDebug() << line; + } + } + update(); +} + +void Updater::updateFinished(int exitCode, QProcess::ExitStatus exitStatus) { + qDebug() << "finished with " << exitCode; + if (exitCode == 0) { + Hardware::reboot(); + } else { + text->setText(tr("Update failed")); + reboot->show(); + } +} + +int main(int argc, char *argv[]) { + initApp(argc, argv); + QApplication a(argc, argv); + Updater updater(argv[1], argv[2]); + setMainWindow(&updater); + a.installEventFilter(&updater); + return a.exec(); +} diff --git a/selfdrive/ui/qt/setup/updater.h b/selfdrive/ui/qt/setup/updater.h new file mode 100644 index 00000000000000..ce46c0aabd089a --- /dev/null +++ b/selfdrive/ui/qt/setup/updater.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +class Updater : public QStackedWidget { + Q_OBJECT + +public: + explicit Updater(const QString &updater_path, const QString &manifest_path, QWidget *parent = 0); + +private slots: + void installUpdate(); + void readProgress(); + void updateFinished(int exitCode, QProcess::ExitStatus exitStatus); + +private: + QProcess proc; + QString updater, manifest; + + QLabel *text; + QProgressBar *bar; + QPushButton *reboot; + QWidget *prompt, *wifi, *progress; +}; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc new file mode 100644 index 00000000000000..a84542d29107b6 --- /dev/null +++ b/selfdrive/ui/qt/sidebar.cc @@ -0,0 +1,129 @@ +#include "selfdrive/ui/qt/sidebar.h" + +#include + +#include "selfdrive/ui/qt/util.h" + +void Sidebar::drawMetric(QPainter &p, const QPair &label, QColor c, int y) { + const QRect rect = {30, y, 240, 126}; + + p.setPen(Qt::NoPen); + p.setBrush(QBrush(c)); + p.setClipRect(rect.x() + 4, rect.y(), 18, rect.height(), Qt::ClipOperation::ReplaceClip); + p.drawRoundedRect(QRect(rect.x() + 4, rect.y() + 4, 100, 118), 18, 18); + p.setClipping(false); + + QPen pen = QPen(QColor(0xff, 0xff, 0xff, 0x55)); + pen.setWidth(2); + p.setPen(pen); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(rect, 20, 20); + + p.setPen(QColor(0xff, 0xff, 0xff)); + configFont(p, "Inter", 35, "SemiBold"); + + QRect label_rect = getTextRect(p, Qt::AlignCenter, label.first); + label_rect.setWidth(218); + label_rect.moveLeft(rect.left() + 22); + label_rect.moveTop(rect.top() + 19); + p.drawText(label_rect, Qt::AlignCenter, label.first); + + label_rect.moveTop(rect.top() + 65); + p.drawText(label_rect, Qt::AlignCenter, label.second); +} + +Sidebar::Sidebar(QWidget *parent) : QFrame(parent) { + home_img = loadPixmap("../assets/images/button_home.png", home_btn.size()); + settings_img = loadPixmap("../assets/images/button_settings.png", settings_btn.size(), Qt::IgnoreAspectRatio); + + connect(this, &Sidebar::valueChanged, [=] { update(); }); + + setAttribute(Qt::WA_OpaquePaintEvent); + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); + setFixedWidth(300); + + QObject::connect(uiState(), &UIState::uiUpdate, this, &Sidebar::updateState); + + pm = std::make_unique>({"userFlag"}); +} + +void Sidebar::mouseReleaseEvent(QMouseEvent *event) { + if (home_btn.contains(event->pos())) { + MessageBuilder msg; + msg.initEvent().initUserFlag(); + pm->send("userFlag", msg); + } + if (settings_btn.contains(event->pos())) { + emit openSettings(); + } +} + +void Sidebar::updateState(const UIState &s) { + if (!isVisible()) return; + + auto &sm = *(s.sm); + + auto deviceState = sm["deviceState"].getDeviceState(); + setProperty("netType", network_type[deviceState.getNetworkType()]); + int strength = (int)deviceState.getNetworkStrength(); + setProperty("netStrength", strength > 0 ? strength + 1 : 0); + + ItemStatus connectStatus; + auto last_ping = deviceState.getLastAthenaPingTime(); + if (last_ping == 0) { + connectStatus = ItemStatus{{tr("CONNECT"), tr("OFFLINE")}, warning_color}; + } else { + connectStatus = nanos_since_boot() - last_ping < 80e9 ? ItemStatus{{tr("CONNECT"), tr("ONLINE")}, good_color} : ItemStatus{{tr("CONNECT"), tr("ERROR")}, danger_color}; + } + setProperty("connectStatus", QVariant::fromValue(connectStatus)); + + ItemStatus tempStatus = {{tr("TEMP"), tr("HIGH")}, danger_color}; + auto ts = deviceState.getThermalStatus(); + if (ts == cereal::DeviceState::ThermalStatus::GREEN) { + tempStatus = {{tr("TEMP"), tr("GOOD")}, good_color}; + } else if (ts == cereal::DeviceState::ThermalStatus::YELLOW) { + tempStatus = {{tr("TEMP"), tr("OK")}, warning_color}; + } + setProperty("tempStatus", QVariant::fromValue(tempStatus)); + + ItemStatus pandaStatus = {{tr("VEHICLE"), tr("ONLINE")}, good_color}; + if (s.scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) { + pandaStatus = {{tr("NO"), tr("PANDA")}, danger_color}; + } else if (s.scene.started && !sm["liveLocationKalman"].getLiveLocationKalman().getGpsOK()) { + pandaStatus = {{tr("GPS"), tr("SEARCH")}, warning_color}; + } + setProperty("pandaStatus", QVariant::fromValue(pandaStatus)); +} + +void Sidebar::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing); + + p.fillRect(rect(), QColor(57, 57, 57)); + + // static imgs + p.setOpacity(0.65); + p.drawPixmap(settings_btn.x(), settings_btn.y(), settings_img); + p.setOpacity(1.0); + p.drawPixmap(home_btn.x(), home_btn.y(), home_img); + + // network + int x = 58; + const QColor gray(0x54, 0x54, 0x54); + for (int i = 0; i < 5; ++i) { + p.setBrush(i < net_strength ? Qt::white : gray); + p.drawEllipse(x, 196, 27, 27); + x += 37; + } + + configFont(p, "Inter", 35, "Regular"); + p.setPen(QColor(0xff, 0xff, 0xff)); + const QRect r = QRect(50, 247, 100, 50); + p.drawText(r, Qt::AlignCenter, net_type); + + // metrics + drawMetric(p, temp_status.first, temp_status.second, 338); + drawMetric(p, panda_status.first, panda_status.second, 496); + drawMetric(p, connect_status.first, connect_status.second, 654); +} diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h new file mode 100644 index 00000000000000..621a21444d47dc --- /dev/null +++ b/selfdrive/ui/qt/sidebar.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include + +#include "selfdrive/ui/ui.h" + +typedef QPair, QColor> ItemStatus; +Q_DECLARE_METATYPE(ItemStatus); + +class Sidebar : public QFrame { + Q_OBJECT + Q_PROPERTY(ItemStatus connectStatus MEMBER connect_status NOTIFY valueChanged); + Q_PROPERTY(ItemStatus pandaStatus MEMBER panda_status NOTIFY valueChanged); + Q_PROPERTY(ItemStatus tempStatus MEMBER temp_status NOTIFY valueChanged); + Q_PROPERTY(QString netType MEMBER net_type NOTIFY valueChanged); + Q_PROPERTY(int netStrength MEMBER net_strength NOTIFY valueChanged); + +public: + explicit Sidebar(QWidget* parent = 0); + +signals: + void openSettings(); + void valueChanged(); + +public slots: + void updateState(const UIState &s); + +protected: + void paintEvent(QPaintEvent *event) override; + void mouseReleaseEvent(QMouseEvent *event) override; + void drawMetric(QPainter &p, const QPair &label, QColor c, int y); + + QPixmap home_img, settings_img; + const QMap network_type = { + {cereal::DeviceState::NetworkType::NONE, tr("--")}, + {cereal::DeviceState::NetworkType::WIFI, tr("Wi-Fi")}, + {cereal::DeviceState::NetworkType::ETHERNET, tr("ETH")}, + {cereal::DeviceState::NetworkType::CELL2_G, tr("2G")}, + {cereal::DeviceState::NetworkType::CELL3_G, tr("3G")}, + {cereal::DeviceState::NetworkType::CELL4_G, tr("LTE")}, + {cereal::DeviceState::NetworkType::CELL5_G, tr("5G")} + }; + + const QRect home_btn = QRect(60, 860, 180, 180); + const QRect settings_btn = QRect(50, 35, 200, 117); + const QColor good_color = QColor(255, 255, 255); + const QColor warning_color = QColor(218, 202, 37); + const QColor danger_color = QColor(201, 34, 49); + + ItemStatus connect_status, panda_status, temp_status; + QString net_type; + int net_strength = 0; + +private: + std::unique_ptr pm; +}; diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc new file mode 100644 index 00000000000000..8f13576fb26b5f --- /dev/null +++ b/selfdrive/ui/qt/spinner.cc @@ -0,0 +1,119 @@ +#include "selfdrive/ui/qt/spinner.h" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/util.h" + +TrackWidget::TrackWidget(QWidget *parent) : QWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + setFixedSize(spinner_size); + + // pre-compute all the track imgs. make this a gif instead? + QPixmap comma_img = loadPixmap("../assets/img_spinner_comma.png", spinner_size); + QPixmap track_img = loadPixmap("../assets/img_spinner_track.png", spinner_size); + + QTransform transform(1, 0, 0, 1, width() / 2, height() / 2); + QPixmap pm(spinner_size); + QPainter p(&pm); + p.setRenderHint(QPainter::SmoothPixmapTransform); + for (int i = 0; i < track_imgs.size(); ++i) { + p.resetTransform(); + p.fillRect(0, 0, spinner_size.width(), spinner_size.height(), Qt::black); + p.drawPixmap(0, 0, comma_img); + p.setTransform(transform.rotate(360 / spinner_fps)); + p.drawPixmap(-width() / 2, -height() / 2, track_img); + track_imgs[i] = pm.copy(); + } + + m_anim.setDuration(1000); + m_anim.setStartValue(0); + m_anim.setEndValue(int(track_imgs.size() -1)); + m_anim.setLoopCount(-1); + m_anim.start(); + connect(&m_anim, SIGNAL(valueChanged(QVariant)), SLOT(update())); +} + +void TrackWidget::paintEvent(QPaintEvent *event) { + QPainter painter(this); + painter.drawPixmap(0, 0, track_imgs[m_anim.currentValue().toInt()]); +} + +// Spinner + +Spinner::Spinner(QWidget *parent) : QWidget(parent) { + QGridLayout *main_layout = new QGridLayout(this); + main_layout->setSpacing(0); + main_layout->setMargin(200); + + main_layout->addWidget(new TrackWidget(this), 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); + + text = new QLabel(); + text->setWordWrap(true); + text->setVisible(false); + text->setAlignment(Qt::AlignCenter); + main_layout->addWidget(text, 1, 0, Qt::AlignHCenter); + + progress_bar = new QProgressBar(); + progress_bar->setRange(5, 100); + progress_bar->setTextVisible(false); + progress_bar->setVisible(false); + progress_bar->setFixedHeight(20); + main_layout->addWidget(progress_bar, 1, 0, Qt::AlignHCenter); + + setStyleSheet(R"( + Spinner { + background-color: black; + } + QLabel { + color: white; + font-size: 80px; + background-color: transparent; + } + QProgressBar { + background-color: #373737; + width: 1000px; + border solid white; + border-radius: 10px; + } + QProgressBar::chunk { + border-radius: 10px; + background-color: white; + } + )"); + + notifier = new QSocketNotifier(fileno(stdin), QSocketNotifier::Read); + QObject::connect(notifier, &QSocketNotifier::activated, this, &Spinner::update); +}; + +void Spinner::update(int n) { + std::string line; + std::getline(std::cin, line); + + if (line.length()) { + bool number = std::all_of(line.begin(), line.end(), ::isdigit); + text->setVisible(!number); + progress_bar->setVisible(number); + text->setText(QString::fromStdString(line)); + if (number) { + progress_bar->setValue(std::stoi(line)); + } + } +} + +int main(int argc, char *argv[]) { + initApp(argc, argv); + QApplication a(argc, argv); + Spinner spinner; + setMainWindow(&spinner); + return a.exec(); +} diff --git a/selfdrive/ui/qt/spinner.h b/selfdrive/ui/qt/spinner.h new file mode 100644 index 00000000000000..43d90a75b01036 --- /dev/null +++ b/selfdrive/ui/qt/spinner.h @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include +#include +#include + +constexpr int spinner_fps = 30; +constexpr QSize spinner_size = QSize(360, 360); + +class TrackWidget : public QWidget { + Q_OBJECT +public: + TrackWidget(QWidget *parent = nullptr); + +private: + void paintEvent(QPaintEvent *event) override; + std::array track_imgs; + QVariantAnimation m_anim; +}; + +class Spinner : public QWidget { + Q_OBJECT + +public: + explicit Spinner(QWidget *parent = 0); + +private: + QLabel *text; + QProgressBar *progress_bar; + QSocketNotifier *notifier; + +public slots: + void update(int n); +}; diff --git a/selfdrive/ui/qt/spinner_larch64 b/selfdrive/ui/qt/spinner_larch64 new file mode 100755 index 00000000000000..645bc4430e6773 Binary files /dev/null and b/selfdrive/ui/qt/spinner_larch64 differ diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc new file mode 100644 index 00000000000000..21ec5eedcf6be4 --- /dev/null +++ b/selfdrive/ui/qt/text.cc @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + +int main(int argc, char *argv[]) { + initApp(argc, argv); + QApplication a(argc, argv); + QWidget window; + setMainWindow(&window); + + QGridLayout *main_layout = new QGridLayout(&window); + main_layout->setMargin(50); + + QLabel *label = new QLabel(argv[1]); + label->setWordWrap(true); + label->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding); + ScrollView *scroll = new ScrollView(label); + scroll->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + main_layout->addWidget(scroll, 0, 0, Qt::AlignTop); + + // Scroll to the bottom + QObject::connect(scroll->verticalScrollBar(), &QAbstractSlider::rangeChanged, [=]() { + scroll->verticalScrollBar()->setValue(scroll->verticalScrollBar()->maximum()); + }); + + QPushButton *btn = new QPushButton(); +#ifdef __aarch64__ + btn->setText(QObject::tr("Reboot")); + QObject::connect(btn, &QPushButton::clicked, [=]() { + Hardware::reboot(); + }); +#else + btn->setText(QObject::tr("Exit")); + QObject::connect(btn, &QPushButton::clicked, &a, &QApplication::quit); +#endif + main_layout->addWidget(btn, 0, 0, Qt::AlignRight | Qt::AlignBottom); + + window.setStyleSheet(R"( + * { + outline: none; + color: white; + background-color: black; + font-size: 60px; + } + QPushButton { + padding: 50px; + padding-right: 100px; + padding-left: 100px; + border: 2px solid white; + border-radius: 20px; + margin-right: 40px; + } + )"); + + return a.exec(); +} diff --git a/selfdrive/ui/qt/text_larch64 b/selfdrive/ui/qt/text_larch64 new file mode 100755 index 00000000000000..eb0f535bf3b5d5 Binary files /dev/null and b/selfdrive/ui/qt/text_larch64 differ diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc new file mode 100644 index 00000000000000..c5697c10459842 --- /dev/null +++ b/selfdrive/ui/qt/util.cc @@ -0,0 +1,224 @@ +#include "selfdrive/ui/qt/util.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "common/swaglog.h" +#include "system/hardware/hw.h" + +QString getVersion() { + static QString version = QString::fromStdString(Params().get("Version")); + return version; +} + +QString getBrand() { + return Params().getBool("Passive") ? QObject::tr("dashcam") : QObject::tr("openpilot"); +} + +QString getBrandVersion() { + return getBrand() + " v" + getVersion().left(14).trimmed(); +} + +QString getUserAgent() { + return "openpilot-" + getVersion(); +} + +std::optional getDongleId() { + std::string id = Params().get("DongleId"); + + if (!id.empty() && (id != "UnregisteredDevice")) { + return QString::fromStdString(id); + } else { + return {}; + } +} + +QMap getSupportedLanguages() { + QFile f("translations/languages.json"); + f.open(QIODevice::ReadOnly | QIODevice::Text); + QString val = f.readAll(); + + QJsonObject obj = QJsonDocument::fromJson(val.toUtf8()).object(); + QMap map; + for (auto key : obj.keys()) { + map[key] = obj[key].toString(); + } + return map; +} + +void configFont(QPainter &p, const QString &family, int size, const QString &style) { + QFont f(family); + f.setPixelSize(size); + f.setStyleName(style); + p.setFont(f); +} + +void clearLayout(QLayout* layout) { + while (QLayoutItem* item = layout->takeAt(0)) { + if (QWidget* widget = item->widget()) { + widget->deleteLater(); + } + if (QLayout* childLayout = item->layout()) { + clearLayout(childLayout); + } + delete item; + } +} + +QString timeAgo(const QDateTime &date) { + int diff = date.secsTo(QDateTime::currentDateTimeUtc()); + + QString s; + if (diff < 60) { + s = "now"; + } else if (diff < 60 * 60) { + int minutes = diff / 60; + s = QObject::tr("%n minute(s) ago", "", minutes); + } else if (diff < 60 * 60 * 24) { + int hours = diff / (60 * 60); + s = QObject::tr("%n hour(s) ago", "", hours); + } else if (diff < 3600 * 24 * 7) { + int days = diff / (60 * 60 * 24); + s = QObject::tr("%n day(s) ago", "", days); + } else { + s = date.date().toString(); + } + + return s; +} + +void setQtSurfaceFormat() { + QSurfaceFormat fmt; +#ifdef __APPLE__ + fmt.setVersion(3, 2); + fmt.setProfile(QSurfaceFormat::OpenGLContextProfile::CoreProfile); + fmt.setRenderableType(QSurfaceFormat::OpenGL); +#else + fmt.setRenderableType(QSurfaceFormat::OpenGLES); +#endif + fmt.setSamples(16); + QSurfaceFormat::setDefaultFormat(fmt); +} + +void sigTermHandler(int s) { + std::signal(s, SIG_DFL); + qApp->quit(); +} + +void initApp(int argc, char *argv[]) { + Hardware::set_display_power(true); + Hardware::set_brightness(65); + + // setup signal handlers to exit gracefully + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); + +#ifdef __APPLE__ + { + // Get the devicePixelRatio, and scale accordingly to maintain 1:1 rendering + QApplication tmp(argc, argv); + qputenv("QT_SCALE_FACTOR", QString::number(1.0 / tmp.devicePixelRatio() ).toLocal8Bit()); + } +#endif + + setQtSurfaceFormat(); +} + +void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) { + static std::map levels = { + {QtMsgType::QtDebugMsg, CLOUDLOG_DEBUG}, + {QtMsgType::QtInfoMsg, CLOUDLOG_INFO}, + {QtMsgType::QtWarningMsg, CLOUDLOG_WARNING}, + {QtMsgType::QtCriticalMsg, CLOUDLOG_ERROR}, + {QtMsgType::QtSystemMsg, CLOUDLOG_ERROR}, + {QtMsgType::QtFatalMsg, CLOUDLOG_CRITICAL}, + }; + + std::string file, function; + if (context.file != nullptr) file = context.file; + if (context.function != nullptr) function = context.function; + + auto bts = msg.toUtf8(); + cloudlog_e(levels[type], file.c_str(), context.line, function.c_str(), "%s", bts.constData()); +} + + +QWidget* topWidget (QWidget* widget) { + while (widget->parentWidget() != nullptr) widget=widget->parentWidget(); + return widget; +} + +QPixmap loadPixmap(const QString &fileName, const QSize &size, Qt::AspectRatioMode aspectRatioMode) { + if (size.isEmpty()) { + return QPixmap(fileName); + } else { + return QPixmap(fileName).scaled(size, aspectRatioMode, Qt::SmoothTransformation); + } +} + +QRect getTextRect(QPainter &p, int flags, const QString &text) { + QFontMetrics fm(p.font()); + QRect init_rect = fm.boundingRect(text); + return fm.boundingRect(init_rect, flags, text); +} + +void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom){ + qreal w_2 = rect.width() / 2; + qreal h_2 = rect.height() / 2; + + xRadiusTop = 100 * qMin(xRadiusTop, w_2) / w_2; + yRadiusTop = 100 * qMin(yRadiusTop, h_2) / h_2; + + xRadiusBottom = 100 * qMin(xRadiusBottom, w_2) / w_2; + yRadiusBottom = 100 * qMin(yRadiusBottom, h_2) / h_2; + + qreal x = rect.x(); + qreal y = rect.y(); + qreal w = rect.width(); + qreal h = rect.height(); + + qreal rxx2Top = w*xRadiusTop/100; + qreal ryy2Top = h*yRadiusTop/100; + + qreal rxx2Bottom = w*xRadiusBottom/100; + qreal ryy2Bottom = h*yRadiusBottom/100; + + QPainterPath path; + path.arcMoveTo(x, y, rxx2Top, ryy2Top, 180); + path.arcTo(x, y, rxx2Top, ryy2Top, 180, -90); + path.arcTo(x+w-rxx2Top, y, rxx2Top, ryy2Top, 90, -90); + path.arcTo(x+w-rxx2Bottom, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 0, -90); + path.arcTo(x, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 270, -90); + path.closeSubpath(); + + painter.drawPath(path); +} + +QColor interpColor(float xv, std::vector xp, std::vector fp) { + assert(xp.size() == fp.size()); + + int N = xp.size(); + int hi = 0; + + while (hi < N and xv > xp[hi]) hi++; + int low = hi - 1; + + if (hi == N && xv > xp[low]) { + return fp[fp.size() - 1]; + } else if (hi == 0){ + return fp[0]; + } else { + return QColor( + (xv - xp[low]) * (fp[hi].red() - fp[low].red()) / (xp[hi] - xp[low]) + fp[low].red(), + (xv - xp[low]) * (fp[hi].green() - fp[low].green()) / (xp[hi] - xp[low]) + fp[low].green(), + (xv - xp[low]) * (fp[hi].blue() - fp[low].blue()) / (xp[hi] - xp[low]) + fp[low].blue(), + (xv - xp[low]) * (fp[hi].alpha() - fp[low].alpha()) / (xp[hi] - xp[low]) + fp[low].alpha() + ); + } +} diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h new file mode 100644 index 00000000000000..209f051963cddf --- /dev/null +++ b/selfdrive/ui/qt/util.h @@ -0,0 +1,29 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +QString getVersion(); +QString getBrand(); +QString getBrandVersion(); +QString getUserAgent(); +std::optional getDongleId(); +QMap getSupportedLanguages(); +void configFont(QPainter &p, const QString &family, int size, const QString &style); +void clearLayout(QLayout* layout); +void setQtSurfaceFormat(); +QString timeAgo(const QDateTime &date); +void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg); +void initApp(int argc, char *argv[]); +QWidget* topWidget (QWidget* widget); +QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); + +QRect getTextRect(QPainter &p, int flags, const QString &text); +void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom); +QColor interpColor(float xv, std::vector xp, std::vector fp); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc new file mode 100644 index 00000000000000..63d15660a08607 --- /dev/null +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -0,0 +1,380 @@ +#include "selfdrive/ui/qt/widgets/cameraview.h" + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include + +#include +#include + +namespace { + +const char frame_vertex_shader[] = +#ifdef __APPLE__ + "#version 330 core\n" +#else + "#version 300 es\n" +#endif + "layout(location = 0) in vec4 aPosition;\n" + "layout(location = 1) in vec2 aTexCoord;\n" + "uniform mat4 uTransform;\n" + "out vec2 vTexCoord;\n" + "void main() {\n" + " gl_Position = uTransform * aPosition;\n" + " vTexCoord = aTexCoord;\n" + "}\n"; + +#ifdef QCOM2 +const char frame_fragment_shader[] = + "#version 300 es\n" + "#extension GL_OES_EGL_image_external_essl3 : enable\n" + "precision mediump float;\n" + "uniform samplerExternalOES uTexture;\n" + "in vec2 vTexCoord;\n" + "out vec4 colorOut;\n" + "void main() {\n" + " colorOut = texture(uTexture, vTexCoord);\n" + "}\n"; +#else +const char frame_fragment_shader[] = +#ifdef __APPLE__ + "#version 330 core\n" +#else + "#version 300 es\n" + "precision mediump float;\n" +#endif + "uniform sampler2D uTextureY;\n" + "uniform sampler2D uTextureUV;\n" + "in vec2 vTexCoord;\n" + "out vec4 colorOut;\n" + "void main() {\n" + " float y = texture(uTextureY, vTexCoord).r;\n" + " vec2 uv = texture(uTextureUV, vTexCoord).rg - 0.5;\n" + " float r = y + 1.402 * uv.y;\n" + " float g = y - 0.344 * uv.x - 0.714 * uv.y;\n" + " float b = y + 1.772 * uv.x;\n" + " colorOut = vec4(r, g, b, 1.0);\n" + "}\n"; +#endif + +mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) { + const float driver_view_ratio = 2.0; + const float yscale = stream_height * driver_view_ratio / stream_width; + const float xscale = yscale*screen_height/screen_width*stream_width/stream_height; + mat4 transform = (mat4){{ + xscale, 0.0, 0.0, 0.0, + 0.0, yscale, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + return transform; +} + +mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) { + float zx = 1, zy = 1; + if (frame_aspect_ratio > widget_aspect_ratio) { + zy = widget_aspect_ratio / frame_aspect_ratio; + } else { + zx = frame_aspect_ratio / widget_aspect_ratio; + } + + const mat4 frame_transform = {{ + zx, 0.0, 0.0, 0.0, + 0.0, zy, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + return frame_transform; +} + +} // namespace + +CameraViewWidget::CameraViewWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : + stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection); + connect(this, &CameraViewWidget::vipcThreadFrameReceived, this, &CameraViewWidget::vipcFrameReceived); +} + +CameraViewWidget::~CameraViewWidget() { + makeCurrent(); + if (isValid()) { + glDeleteVertexArrays(1, &frame_vao); + glDeleteBuffers(1, &frame_vbo); + glDeleteBuffers(1, &frame_ibo); + glDeleteBuffers(2, textures); + } + doneCurrent(); +} + +void CameraViewWidget::initializeGL() { + initializeOpenGLFunctions(); + + program = std::make_unique(context()); + bool ret = program->addShaderFromSourceCode(QOpenGLShader::Vertex, frame_vertex_shader); + assert(ret); + ret = program->addShaderFromSourceCode(QOpenGLShader::Fragment, frame_fragment_shader); + assert(ret); + + program->link(); + GLint frame_pos_loc = program->attributeLocation("aPosition"); + GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); + + auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; + const float frame_coords[4][4] = { + {-1.0, -1.0, x2, y1}, // bl + {-1.0, 1.0, x2, y2}, // tl + { 1.0, 1.0, x1, y2}, // tr + { 1.0, -1.0, x1, y1}, // br + }; + + glGenVertexArrays(1, &frame_vao); + glBindVertexArray(frame_vao); + glGenBuffers(1, &frame_vbo); + glBindBuffer(GL_ARRAY_BUFFER, frame_vbo); + glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); + glEnableVertexAttribArray(frame_pos_loc); + glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)0); + glEnableVertexAttribArray(frame_texcoord_loc); + glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); + glGenBuffers(1, &frame_ibo); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, frame_ibo); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER, 0); + glBindVertexArray(0); + + glUseProgram(program->programId()); + +#ifdef QCOM2 + glUniform1i(program->uniformLocation("uTexture"), 0); +#else + glGenTextures(2, textures); + glUniform1i(program->uniformLocation("uTextureY"), 0); + glUniform1i(program->uniformLocation("uTextureUV"), 1); +#endif +} + +void CameraViewWidget::showEvent(QShowEvent *event) { + frames.clear(); + if (!vipc_thread) { + vipc_thread = new QThread(); + connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); + connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater); + vipc_thread->start(); + } +} + +void CameraViewWidget::hideEvent(QHideEvent *event) { + if (vipc_thread) { + vipc_thread->requestInterruption(); + vipc_thread->quit(); + vipc_thread->wait(); + vipc_thread = nullptr; + } +} + +void CameraViewWidget::updateFrameMat() { + int w = width(), h = height(); + + if (zoomed_view) { + if (stream_type == VISION_STREAM_DRIVER) { + frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); + } else { + intrinsic_matrix = (stream_type == VISION_STREAM_WIDE_ROAD) ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + zoom = (stream_type == VISION_STREAM_WIDE_ROAD) ? 2.5 : 1.1; + + // Project point at "infinity" to compute x and y offsets + // to ensure this ends up in the middle of the screen + // TODO: use proper perspective transform? + const vec3 inf = {{1000., 0., 0.}}; + const vec3 Ep = matvecmul3(calibration, inf); + const vec3 Kep = matvecmul3(intrinsic_matrix, Ep); + + float x_offset_ = (Kep.v[0] / Kep.v[2] - intrinsic_matrix.v[2]) * zoom; + float y_offset_ = (Kep.v[1] / Kep.v[2] - intrinsic_matrix.v[5]) * zoom; + + float max_x_offset = intrinsic_matrix.v[2] * zoom - w / 2 - 5; + float max_y_offset = intrinsic_matrix.v[5] * zoom - h / 2 - 5; + + x_offset = std::clamp(x_offset_, -max_x_offset, max_x_offset); + y_offset = std::clamp(y_offset_, -max_y_offset, max_y_offset); + + float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); + float zy = zoom * 2 * intrinsic_matrix.v[5] / height(); + const mat4 frame_transform = {{ + zx, 0.0, 0.0, -x_offset / width() * 2, + 0.0, zy, 0.0, y_offset / height() * 2, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + frame_mat = frame_transform; + } + } else if (stream_width > 0 && stream_height > 0) { + // fit frame to widget size + float widget_aspect_ratio = (float)width() / height(); + float frame_aspect_ratio = (float)stream_width / stream_height; + frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio); + } +} + +void CameraViewWidget::updateCalibration(const mat3 &calib) { + calibration = calib; + updateFrameMat(); +} + +void CameraViewWidget::paintGL() { + glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + + if (frames.empty()) return; + + int frame_idx = frames.size() - 1; + + // Always draw latest frame until sync logic is more stable + // for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { + // if (frames[frame_idx].first == draw_frame_id) break; + // } + + // Log duplicate/dropped frames + if (frames[frame_idx].first == prev_frame_id) { + qDebug() << "Drawing same frame twice" << frames[frame_idx].first; + } else if (frames[frame_idx].first != prev_frame_id + 1) { + qDebug() << "Skipped frame" << frames[frame_idx].first; + } + prev_frame_id = frames[frame_idx].first; + + glViewport(0, 0, width(), height()); + glBindVertexArray(frame_vao); + glUseProgram(program->programId()); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + + VisionBuf *frame = frames[frame_idx].second; + +#ifdef QCOM2 + glActiveTexture(GL_TEXTURE0); + glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); + assert(glGetError() == GL_NO_ERROR); +#else + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, textures[0]); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width, stream_height, GL_RED, GL_UNSIGNED_BYTE, frame->y); + assert(glGetError() == GL_NO_ERROR); + + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride/2); + glActiveTexture(GL_TEXTURE0 + 1); + glBindTexture(GL_TEXTURE_2D, textures[1]); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width/2, stream_height/2, GL_RG, GL_UNSIGNED_BYTE, frame->uv); + assert(glGetError() == GL_NO_ERROR); +#endif + + glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); + glEnableVertexAttribArray(0); + glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); + glDisableVertexAttribArray(0); + glBindVertexArray(0); + glBindTexture(GL_TEXTURE_2D, 0); + glActiveTexture(GL_TEXTURE0); + glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); +} + +void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { + makeCurrent(); + frames.clear(); + stream_width = vipc_client->buffers[0].width; + stream_height = vipc_client->buffers[0].height; + stream_stride = vipc_client->buffers[0].stride; + +#ifdef QCOM2 + egl_display = eglGetCurrentDisplay(); + + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); + + for (int i = 0; i < vipc_client->num_buffers; i++) { // import buffers into OpenGL + int fd = dup(vipc_client->buffers[i].fd); // eglDestroyImageKHR will close, so duplicate + EGLint img_attrs[] = { + EGL_WIDTH, (int)vipc_client->buffers[i].width, + EGL_HEIGHT, (int)vipc_client->buffers[i].height, + EGL_LINUX_DRM_FOURCC_EXT, DRM_FORMAT_NV12, + EGL_DMA_BUF_PLANE0_FD_EXT, fd, + EGL_DMA_BUF_PLANE0_OFFSET_EXT, 0, + EGL_DMA_BUF_PLANE0_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_DMA_BUF_PLANE1_FD_EXT, fd, + EGL_DMA_BUF_PLANE1_OFFSET_EXT, (int)vipc_client->buffers[i].uv_offset, + EGL_DMA_BUF_PLANE1_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_NONE + }; + egl_images[i] = eglCreateImageKHR(egl_display, EGL_NO_CONTEXT, EGL_LINUX_DMA_BUF_EXT, 0, img_attrs); + assert(eglGetError() == EGL_SUCCESS); + } +#else + glBindTexture(GL_TEXTURE_2D, textures[0]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, stream_width, stream_height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr); + assert(glGetError() == GL_NO_ERROR); + + glBindTexture(GL_TEXTURE_2D, textures[1]); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, stream_width/2, stream_height/2, 0, GL_RG, GL_UNSIGNED_BYTE, nullptr); + assert(glGetError() == GL_NO_ERROR); +#endif + + updateFrameMat(); +} + +void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) { + frames.push_back(std::make_pair(frame_id, buf)); + while (frames.size() > FRAME_BUFFER_SIZE) { + frames.pop_front(); + } + update(); +} + +void CameraViewWidget::vipcThread() { + VisionStreamType cur_stream_type = stream_type; + std::unique_ptr vipc_client; + VisionIpcBufExtra meta_main = {0}; + + while (!QThread::currentThread()->isInterruptionRequested()) { + if (!vipc_client || cur_stream_type != stream_type) { + cur_stream_type = stream_type; + vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false)); + } + + if (!vipc_client->connected) { + if (!vipc_client->connect(false)) { + QThread::msleep(100); + continue; + } + emit vipcThreadConnected(vipc_client.get()); + } + + if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) { + emit vipcThreadFrameReceived(buf, meta_main.frame_id); + } + } + +#ifdef QCOM2 + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); +#endif +} diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h new file mode 100644 index 00000000000000..016522b05c275a --- /dev/null +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -0,0 +1,86 @@ +#pragma once + +#include + +#include +#include +#include +#include + +#ifdef QCOM2 +#define EGL_EGLEXT_PROTOTYPES +#define EGL_NO_X11 +#define GL_TEXTURE_EXTERNAL_OES 0x8D65 +#include +#include +#include +#endif + +#include "cereal/visionipc/visionipc_client.h" +#include "system/camerad/cameras/camera_common.h" +#include "selfdrive/ui/ui.h" + +const int FRAME_BUFFER_SIZE = 5; +static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT); + +class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { + Q_OBJECT + +public: + using QOpenGLWidget::QOpenGLWidget; + explicit CameraViewWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); + ~CameraViewWidget(); + void setStreamType(VisionStreamType type) { stream_type = type; } + void setBackgroundColor(const QColor &color) { bg = color; } + void setFrameId(int frame_id) { draw_frame_id = frame_id; } + +signals: + void clicked(); + void vipcThreadConnected(VisionIpcClient *); + void vipcThreadFrameReceived(VisionBuf *, quint32); + +protected: + void paintGL() override; + void initializeGL() override; + void resizeGL(int w, int h) override { updateFrameMat(); } + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } + virtual void updateFrameMat(); + void updateCalibration(const mat3 &calib); + void vipcThread(); + + bool zoomed_view; + GLuint frame_vao, frame_vbo, frame_ibo; + GLuint textures[2]; + mat4 frame_mat; + std::unique_ptr program; + QColor bg = QColor("#000000"); + +#ifdef QCOM2 + EGLDisplay egl_display; + std::map egl_images; +#endif + + std::string stream_name; + int stream_width = 0; + int stream_height = 0; + int stream_stride = 0; + std::atomic stream_type; + QThread *vipc_thread = nullptr; + + // Calibration + float x_offset = 0; + float y_offset = 0; + float zoom = 1.0; + mat3 calibration = DEFAULT_CALIBRATION; + mat3 intrinsic_matrix = fcam_intrinsic_matrix; + + std::deque> frames; + uint32_t draw_frame_id = 0; + int prev_frame_id = 0; + +protected slots: + void vipcConnected(VisionIpcClient *vipc_client); + void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id); +}; diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc new file mode 100644 index 00000000000000..3264fd3aac76c4 --- /dev/null +++ b/selfdrive/ui/qt/widgets/controls.cc @@ -0,0 +1,141 @@ +#include "selfdrive/ui/qt/widgets/controls.h" + +#include +#include + +QFrame *horizontal_line(QWidget *parent) { + QFrame *line = new QFrame(parent); + line->setFrameShape(QFrame::StyledPanel); + line->setStyleSheet(R"( + margin-left: 40px; + margin-right: 40px; + border-width: 1px; + border-bottom-style: solid; + border-color: gray; + )"); + line->setFixedHeight(2); + return line; +} + +AbstractControl::AbstractControl(const QString &title, const QString &desc, const QString &icon, QWidget *parent) : QFrame(parent) { + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(0); + + hlayout = new QHBoxLayout; + hlayout->setMargin(0); + hlayout->setSpacing(20); + + // left icon + if (!icon.isEmpty()) { + QPixmap pix(icon); + QLabel *icon_label = new QLabel(); + icon_label->setPixmap(pix.scaledToWidth(80, Qt::SmoothTransformation)); + icon_label->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + hlayout->addWidget(icon_label); + } + + // title + title_label = new QPushButton(title); + title_label->setFixedHeight(120); + title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left"); + hlayout->addWidget(title_label); + + main_layout->addLayout(hlayout); + + // description + description = new QLabel(desc); + description->setContentsMargins(40, 20, 40, 20); + description->setStyleSheet("font-size: 40px; color: grey"); + description->setWordWrap(true); + description->setVisible(false); + main_layout->addWidget(description); + + connect(title_label, &QPushButton::clicked, [=]() { + if (!description->isVisible()) { + emit showDescription(); + } + + if (!description->text().isEmpty()) { + description->setVisible(!description->isVisible()); + } + }); + + main_layout->addStretch(); +} + +void AbstractControl::hideEvent(QHideEvent *e) { + if(description != nullptr) { + description->hide(); + } +} + +// controls + +ButtonControl::ButtonControl(const QString &title, const QString &text, const QString &desc, QWidget *parent) : AbstractControl(title, desc, "", parent) { + btn.setText(text); + btn.setStyleSheet(R"( + QPushButton { + padding: 0; + border-radius: 50px; + font-size: 35px; + font-weight: 500; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"); + btn.setFixedSize(250, 100); + QObject::connect(&btn, &QPushButton::clicked, this, &ButtonControl::clicked); + hlayout->addWidget(&btn); +} + +// ElidedLabel + +ElidedLabel::ElidedLabel(QWidget *parent) : ElidedLabel({}, parent) {} + +ElidedLabel::ElidedLabel(const QString &text, QWidget *parent) : QLabel(text.trimmed(), parent) { + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + setMinimumWidth(1); +} + +void ElidedLabel::resizeEvent(QResizeEvent* event) { + QLabel::resizeEvent(event); + lastText_ = elidedText_ = ""; +} + +void ElidedLabel::paintEvent(QPaintEvent *event) { + const QString curText = text(); + if (curText != lastText_) { + elidedText_ = fontMetrics().elidedText(curText, Qt::ElideRight, contentsRect().width()); + lastText_ = curText; + } + + QPainter painter(this); + drawFrame(&painter); + QStyleOption opt; + opt.initFrom(this); + style()->drawItemText(&painter, contentsRect(), alignment(), opt.palette, isEnabled(), elidedText_, foregroundRole()); +} + +ClickableWidget::ClickableWidget(QWidget *parent) : QWidget(parent) { } + +void ClickableWidget::mouseReleaseEvent(QMouseEvent *event) { + if (rect().contains(event->pos())) { + emit clicked(); + } +} + +// Fix stylesheets +void ClickableWidget::paintEvent(QPaintEvent *) { + QStyleOption opt; + opt.init(this); + QPainter p(this); + style()->drawPrimitive(QStyle::PE_Widget, &opt, &p, this); +} diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h new file mode 100644 index 00000000000000..d8546bb3b508ef --- /dev/null +++ b/selfdrive/ui/qt/widgets/controls.h @@ -0,0 +1,201 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "selfdrive/ui/qt/widgets/toggle.h" + +QFrame *horizontal_line(QWidget *parent = nullptr); + +class ElidedLabel : public QLabel { + Q_OBJECT + +public: + explicit ElidedLabel(QWidget *parent = 0); + explicit ElidedLabel(const QString &text, QWidget *parent = 0); + +signals: + void clicked(); + +protected: + void paintEvent(QPaintEvent *event) override; + void resizeEvent(QResizeEvent* event) override; + void mouseReleaseEvent(QMouseEvent *event) override { + if (rect().contains(event->pos())) { + emit clicked(); + } + } + QString lastText_, elidedText_; +}; + + +class AbstractControl : public QFrame { + Q_OBJECT + +public: + void setDescription(const QString &desc) { + if (description) description->setText(desc); + } + + void setTitle(const QString &title) { + title_label->setText(title); + } + +signals: + void showDescription(); + +protected: + AbstractControl(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr); + void hideEvent(QHideEvent *e) override; + + QHBoxLayout *hlayout; + QPushButton *title_label; + QLabel *description = nullptr; +}; + +// widget to display a value +class LabelControl : public AbstractControl { + Q_OBJECT + +public: + LabelControl(const QString &title, const QString &text = "", const QString &desc = "", QWidget *parent = nullptr) : AbstractControl(title, desc, "", parent) { + label.setText(text); + label.setAlignment(Qt::AlignRight | Qt::AlignVCenter); + hlayout->addWidget(&label); + } + void setText(const QString &text) { label.setText(text); } + +private: + ElidedLabel label; +}; + +// widget for a button with a label +class ButtonControl : public AbstractControl { + Q_OBJECT + +public: + ButtonControl(const QString &title, const QString &text, const QString &desc = "", QWidget *parent = nullptr); + inline void setText(const QString &text) { btn.setText(text); } + inline QString text() const { return btn.text(); } + +signals: + void clicked(); + +public slots: + void setEnabled(bool enabled) { btn.setEnabled(enabled); }; + +private: + QPushButton btn; +}; + +class ToggleControl : public AbstractControl { + Q_OBJECT + +public: + ToggleControl(const QString &title, const QString &desc = "", const QString &icon = "", const bool state = false, QWidget *parent = nullptr) : AbstractControl(title, desc, icon, parent) { + toggle.setFixedSize(150, 100); + if (state) { + toggle.togglePosition(); + } + hlayout->addWidget(&toggle); + QObject::connect(&toggle, &Toggle::stateChanged, this, &ToggleControl::toggleFlipped); + } + + void setEnabled(bool enabled) { + toggle.setEnabled(enabled); + toggle.update(); + } + +signals: + void toggleFlipped(bool state); + +protected: + Toggle toggle; +}; + +// widget to toggle params +class ParamControl : public ToggleControl { + Q_OBJECT + +public: + ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, false, parent) { + key = param.toStdString(); + QObject::connect(this, &ParamControl::toggleFlipped, [=](bool state) { + params.putBool(key, state); + }); + } + + void refresh() { + if (params.getBool(key) != toggle.on) { + toggle.togglePosition(); + } + }; + + void showEvent(QShowEvent *event) override { + refresh(); + }; + +private: + std::string key; + Params params; +}; + +class ListWidget : public QWidget { + Q_OBJECT + public: + explicit ListWidget(QWidget *parent = 0) : QWidget(parent), outer_layout(this) { + outer_layout.setMargin(0); + outer_layout.setSpacing(0); + outer_layout.addLayout(&inner_layout); + inner_layout.setMargin(0); + inner_layout.setSpacing(25); // default spacing is 25 + outer_layout.addStretch(); + } + inline void addItem(QWidget *w) { inner_layout.addWidget(w); } + inline void addItem(QLayout *layout) { inner_layout.addLayout(layout); } + inline void setSpacing(int spacing) { inner_layout.setSpacing(spacing); } + +private: + void paintEvent(QPaintEvent *) override { + QPainter p(this); + p.setPen(Qt::gray); + for (int i = 0; i < inner_layout.count() - 1; ++i) { + QWidget *widget = inner_layout.itemAt(i)->widget(); + if (widget == nullptr || widget->isVisible()) { + QRect r = inner_layout.itemAt(i)->geometry(); + int bottom = r.bottom() + inner_layout.spacing() / 2; + p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom); + } + } + } + QVBoxLayout outer_layout; + QVBoxLayout inner_layout; +}; + +// convenience class for wrapping layouts +class LayoutWidget : public QWidget { + Q_OBJECT + +public: + LayoutWidget(QLayout *l, QWidget *parent = nullptr) : QWidget(parent) { + setLayout(l); + }; +}; + +class ClickableWidget : public QWidget { + Q_OBJECT + +public: + ClickableWidget(QWidget *parent = nullptr); + +protected: + void mouseReleaseEvent(QMouseEvent *event) override; + void paintEvent(QPaintEvent *) override; + +signals: + void clicked(); +}; diff --git a/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc new file mode 100644 index 00000000000000..31009f03ca4ac2 --- /dev/null +++ b/selfdrive/ui/qt/widgets/drive_stats.cc @@ -0,0 +1,97 @@ +#include "selfdrive/ui/qt/widgets/drive_stats.h" + +#include +#include +#include +#include + +#include "common/params.h" +#include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/util.h" + +static QLabel* newLabel(const QString& text, const QString &type) { + QLabel* label = new QLabel(text); + label->setProperty("type", type); + return label; +} + +DriveStats::DriveStats(QWidget* parent) : QFrame(parent) { + metric_ = Params().getBool("IsMetric"); + + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(50, 50, 50, 60); + + auto add_stats_layouts = [=](const QString &title, StatsLabels& labels) { + QGridLayout* grid_layout = new QGridLayout; + grid_layout->setVerticalSpacing(10); + grid_layout->setContentsMargins(0, 10, 0, 10); + + int row = 0; + grid_layout->addWidget(newLabel(title, "title"), row++, 0, 1, 3); + grid_layout->addItem(new QSpacerItem(0, 50), row++, 0, 1, 1); + + grid_layout->addWidget(labels.routes = newLabel("0", "number"), row, 0, Qt::AlignLeft); + grid_layout->addWidget(labels.distance = newLabel("0", "number"), row, 1, Qt::AlignLeft); + grid_layout->addWidget(labels.hours = newLabel("0", "number"), row, 2, Qt::AlignLeft); + + grid_layout->addWidget(newLabel((tr("Drives")), "unit"), row + 1, 0, Qt::AlignLeft); + grid_layout->addWidget(labels.distance_unit = newLabel(getDistanceUnit(), "unit"), row + 1, 1, Qt::AlignLeft); + grid_layout->addWidget(newLabel(tr("Hours"), "unit"), row + 1, 2, Qt::AlignLeft); + + main_layout->addLayout(grid_layout); + }; + + add_stats_layouts(tr("ALL TIME"), all_); + main_layout->addStretch(); + add_stats_layouts(tr("PAST WEEK"), week_); + + if (auto dongleId = getDongleId()) { + QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/stats"; + RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_DriveStats", 30); + QObject::connect(repeater, &RequestRepeater::requestDone, this, &DriveStats::parseResponse); + } + + setStyleSheet(R"( + DriveStats { + background-color: #333333; + border-radius: 10px; + } + + QLabel[type="title"] { font-size: 51px; font-weight: 500; } + QLabel[type="number"] { font-size: 78px; font-weight: 500; } + QLabel[type="unit"] { font-size: 51px; font-weight: 300; color: #A0A0A0; } + )"); +} + +void DriveStats::updateStats() { + auto update = [=](const QJsonObject& obj, StatsLabels& labels) { + labels.routes->setText(QString::number((int)obj["routes"].toDouble())); + labels.distance->setText(QString::number(int(obj["distance"].toDouble() * (metric_ ? MILE_TO_KM : 1)))); + labels.distance_unit->setText(getDistanceUnit()); + labels.hours->setText(QString::number((int)(obj["minutes"].toDouble() / 60))); + }; + + QJsonObject json = stats_.object(); + update(json["all"].toObject(), all_); + update(json["week"].toObject(), week_); +} + +void DriveStats::parseResponse(const QString& response, bool success) { + if (!success) return; + + QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting past drives statistics"; + return; + } + stats_ = doc; + updateStats(); +} + +void DriveStats::showEvent(QShowEvent* event) { + bool metric = Params().getBool("IsMetric"); + if (metric_ != metric) { + metric_ = metric; + updateStats(); + } +} diff --git a/selfdrive/ui/qt/widgets/drive_stats.h b/selfdrive/ui/qt/widgets/drive_stats.h new file mode 100644 index 00000000000000..5e2d96b24086e3 --- /dev/null +++ b/selfdrive/ui/qt/widgets/drive_stats.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +class DriveStats : public QFrame { + Q_OBJECT + +public: + explicit DriveStats(QWidget* parent = 0); + +private: + void showEvent(QShowEvent *event) override; + void updateStats(); + inline QString getDistanceUnit() const { return metric_ ? tr("KM") : tr("Miles"); } + + bool metric_; + QJsonDocument stats_; + struct StatsLabels { + QLabel *routes, *distance, *distance_unit, *hours; + } all_, week_; + +private slots: + void parseResponse(const QString &response, bool success); +}; diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc new file mode 100644 index 00000000000000..d7038258851ee3 --- /dev/null +++ b/selfdrive/ui/qt/widgets/input.cc @@ -0,0 +1,350 @@ +#include "selfdrive/ui/qt/widgets/input.h" + +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + + +QDialogBase::QDialogBase(QWidget *parent) : QDialog(parent) { + Q_ASSERT(parent != nullptr); + parent->installEventFilter(this); + + setStyleSheet(R"( + * { + outline: none; + color: white; + font-family: Inter; + } + QDialogBase { + background-color: black; + } + QPushButton { + height: 160; + font-size: 55px; + font-weight: 400; + border-radius: 10px; + color: white; + background-color: #333333; + } + QPushButton:pressed { + background-color: #444444; + } + )"); +} + +bool QDialogBase::eventFilter(QObject *o, QEvent *e) { + if (o == parent() && e->type() == QEvent::Hide) { + reject(); + } + return QDialog::eventFilter(o, e); +} + +int QDialogBase::exec() { + setMainWindow(this); + return QDialog::exec(); +} + +InputDialog::InputDialog(const QString &title, QWidget *parent, const QString &subtitle, bool secret) : QDialogBase(parent) { + main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(50, 55, 50, 50); + main_layout->setSpacing(0); + + // build header + QHBoxLayout *header_layout = new QHBoxLayout(); + + QVBoxLayout *vlayout = new QVBoxLayout; + header_layout->addLayout(vlayout); + label = new QLabel(title, this); + label->setStyleSheet("font-size: 90px; font-weight: bold;"); + vlayout->addWidget(label, 1, Qt::AlignTop | Qt::AlignLeft); + + if (!subtitle.isEmpty()) { + sublabel = new QLabel(subtitle, this); + sublabel->setStyleSheet("font-size: 55px; font-weight: light; color: #BDBDBD;"); + vlayout->addWidget(sublabel, 1, Qt::AlignTop | Qt::AlignLeft); + } + + QPushButton* cancel_btn = new QPushButton(tr("Cancel")); + cancel_btn->setFixedSize(386, 125); + cancel_btn->setStyleSheet(R"( + font-size: 48px; + border-radius: 10px; + color: #E4E4E4; + background-color: #444444; + )"); + header_layout->addWidget(cancel_btn, 0, Qt::AlignRight); + QObject::connect(cancel_btn, &QPushButton::clicked, this, &InputDialog::reject); + QObject::connect(cancel_btn, &QPushButton::clicked, this, &InputDialog::cancel); + + main_layout->addLayout(header_layout); + + // text box + main_layout->addStretch(2); + + QWidget *textbox_widget = new QWidget; + textbox_widget->setObjectName("textbox"); + QHBoxLayout *textbox_layout = new QHBoxLayout(textbox_widget); + textbox_layout->setContentsMargins(50, 0, 50, 0); + + textbox_widget->setStyleSheet(R"( + #textbox { + margin-left: 50px; + margin-right: 50px; + border-radius: 0; + border-bottom: 3px solid #BDBDBD; + } + * { + border: none; + font-size: 80px; + font-weight: light; + background-color: transparent; + } + )"); + + line = new QLineEdit(); + line->setStyleSheet("lineedit-password-character: 8226; lineedit-password-mask-delay: 1500;"); + textbox_layout->addWidget(line, 1); + + if (secret) { + eye_btn = new QPushButton(); + eye_btn->setCheckable(true); + eye_btn->setFixedSize(150, 120); + QObject::connect(eye_btn, &QPushButton::toggled, [=](bool checked) { + if (checked) { + eye_btn->setIcon(QIcon(ASSET_PATH + "img_eye_closed.svg")); + eye_btn->setIconSize(QSize(81, 54)); + line->setEchoMode(QLineEdit::Password); + } else { + eye_btn->setIcon(QIcon(ASSET_PATH + "img_eye_open.svg")); + eye_btn->setIconSize(QSize(81, 44)); + line->setEchoMode(QLineEdit::Normal); + } + }); + eye_btn->setChecked(true); + textbox_layout->addWidget(eye_btn); + } + + main_layout->addWidget(textbox_widget, 0, Qt::AlignBottom); + main_layout->addSpacing(25); + + k = new Keyboard(this); + QObject::connect(k, &Keyboard::emitEnter, this, &InputDialog::handleEnter); + QObject::connect(k, &Keyboard::emitBackspace, this, [=]() { + line->backspace(); + }); + QObject::connect(k, &Keyboard::emitKey, this, [=](const QString &key) { + line->insert(key.left(1)); + }); + + main_layout->addWidget(k, 2, Qt::AlignBottom); +} + +QString InputDialog::getText(const QString &prompt, QWidget *parent, const QString &subtitle, + bool secret, int minLength, const QString &defaultText) { + InputDialog d = InputDialog(prompt, parent, subtitle, secret); + d.line->setText(defaultText); + d.setMinLength(minLength); + const int ret = d.exec(); + return ret ? d.text() : QString(); +} + +QString InputDialog::text() { + return line->text(); +} + +void InputDialog::show() { + setMainWindow(this); +} + +void InputDialog::handleEnter() { + if (line->text().length() >= minLength) { + done(QDialog::Accepted); + emitText(line->text()); + } else { + setMessage(tr("Need at least %n character(s)!", "", minLength), false); + } +} + +void InputDialog::setMessage(const QString &message, bool clearInputField) { + label->setText(message); + if (clearInputField) { + line->setText(""); + } +} + +void InputDialog::setMinLength(int length) { + minLength = length; +} + +// ConfirmationDialog + +ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, const QString &cancel_text, + QWidget *parent) : QDialogBase(parent) { + QFrame *container = new QFrame(this); + container->setStyleSheet("QFrame { border-radius: 0; background-color: #ECECEC; }"); + QVBoxLayout *main_layout = new QVBoxLayout(container); + main_layout->setContentsMargins(32, 120, 32, 32); + + QLabel *prompt = new QLabel(prompt_text, this); + prompt->setWordWrap(true); + prompt->setAlignment(Qt::AlignHCenter); + prompt->setStyleSheet("font-size: 70px; font-weight: bold; color: black;"); + main_layout->addWidget(prompt, 1, Qt::AlignTop | Qt::AlignHCenter); + + // cancel + confirm buttons + QHBoxLayout *btn_layout = new QHBoxLayout(); + btn_layout->setSpacing(30); + main_layout->addLayout(btn_layout); + + if (cancel_text.length()) { + QPushButton* cancel_btn = new QPushButton(cancel_text); + btn_layout->addWidget(cancel_btn); + QObject::connect(cancel_btn, &QPushButton::clicked, this, &ConfirmationDialog::reject); + } + + if (confirm_text.length()) { + QPushButton* confirm_btn = new QPushButton(confirm_text); + btn_layout->addWidget(confirm_btn); + QObject::connect(confirm_btn, &QPushButton::clicked, this, &ConfirmationDialog::accept); + } + + QVBoxLayout *outer_layout = new QVBoxLayout(this); + outer_layout->setContentsMargins(210, 170, 210, 170); + outer_layout->addWidget(container); +} + +bool ConfirmationDialog::alert(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), "", parent); + return d.exec(); +} + +bool ConfirmationDialog::confirm(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Ok"), tr("Cancel"), parent); + return d.exec(); +} + + +// RichTextDialog + +RichTextDialog::RichTextDialog(const QString &prompt_text, const QString &btn_text, + QWidget *parent) : QDialogBase(parent) { + QFrame *container = new QFrame(this); + container->setStyleSheet("QFrame { background-color: #1B1B1B; }"); + QVBoxLayout *main_layout = new QVBoxLayout(container); + main_layout->setContentsMargins(32, 32, 32, 32); + + QLabel *prompt = new QLabel(prompt_text, this); + prompt->setWordWrap(true); + prompt->setAlignment(Qt::AlignLeft); + prompt->setTextFormat(Qt::RichText); + prompt->setStyleSheet("font-size: 42px; font-weight: light; color: #C9C9C9; margin: 45px;"); + main_layout->addWidget(new ScrollView(prompt, this), 1, Qt::AlignTop); + + // confirm button + QPushButton* confirm_btn = new QPushButton(btn_text); + main_layout->addWidget(confirm_btn); + QObject::connect(confirm_btn, &QPushButton::clicked, this, &QDialog::accept); + + QVBoxLayout *outer_layout = new QVBoxLayout(this); + outer_layout->setContentsMargins(100, 100, 100, 100); + outer_layout->addWidget(container); +} + +bool RichTextDialog::alert(const QString &prompt_text, QWidget *parent) { + auto d = RichTextDialog(prompt_text, tr("Ok"), parent); + return d.exec(); +} + +// MultiOptionDialog + +MultiOptionDialog::MultiOptionDialog(const QString &prompt_text, const QStringList &l, const QString ¤t, QWidget *parent) : QDialogBase(parent) { + QFrame *container = new QFrame(this); + container->setStyleSheet(R"( + QFrame { background-color: #1B1B1B; } + #confirm_btn[enabled="false"] { background-color: #2B2B2B; } + #confirm_btn:enabled { background-color: #465BEA; } + #confirm_btn:enabled:pressed { background-color: #3049F4; } + )"); + + QVBoxLayout *main_layout = new QVBoxLayout(container); + main_layout->setContentsMargins(55, 50, 55, 50); + + QLabel *title = new QLabel(prompt_text, this); + title->setStyleSheet("font-size: 70px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); + main_layout->addSpacing(25); + + QWidget *listWidget = new QWidget(this); + QVBoxLayout *listLayout = new QVBoxLayout(listWidget); + listLayout->setSpacing(20); + listWidget->setStyleSheet(R"( + QPushButton { + height: 135; + padding: 0px 50px; + text-align: left; + font-size: 55px; + font-weight: 300; + border-radius: 10px; + background-color: #4F4F4F; + } + QPushButton:checked { background-color: #465BEA; } + )"); + + QButtonGroup *group = new QButtonGroup(listWidget); + group->setExclusive(true); + + QPushButton *confirm_btn = new QPushButton(tr("Select")); + confirm_btn->setObjectName("confirm_btn"); + confirm_btn->setEnabled(false); + + for (const QString &s : l) { + QPushButton *selectionLabel = new QPushButton(s); + selectionLabel->setCheckable(true); + selectionLabel->setChecked(s == current); + QObject::connect(selectionLabel, &QPushButton::toggled, [=](bool checked) { + if (checked) selection = s; + if (selection != current) { + confirm_btn->setEnabled(true); + } else { + confirm_btn->setEnabled(false); + } + }); + + group->addButton(selectionLabel); + listLayout->addWidget(selectionLabel); + } + + ScrollView *scroll_view = new ScrollView(listWidget, this); + scroll_view->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + main_layout->addWidget(scroll_view); + main_layout->addStretch(1); + main_layout->addSpacing(35); + + // cancel + confirm buttons + QHBoxLayout *blayout = new QHBoxLayout; + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + QPushButton *cancel_btn = new QPushButton(tr("Cancel")); + QObject::connect(cancel_btn, &QPushButton::clicked, this, &ConfirmationDialog::reject); + QObject::connect(confirm_btn, &QPushButton::clicked, this, &ConfirmationDialog::accept); + blayout->addWidget(cancel_btn); + blayout->addWidget(confirm_btn); + + QVBoxLayout *outer_layout = new QVBoxLayout(this); + outer_layout->setContentsMargins(50, 50, 50, 50); + outer_layout->addWidget(container); +} + +QString MultiOptionDialog::getSelection(const QString &prompt_text, const QStringList &l, const QString ¤t, QWidget *parent) { + MultiOptionDialog d = MultiOptionDialog(prompt_text, l, current, parent); + if (d.exec()) { + return d.selection; + } + return ""; +} diff --git a/selfdrive/ui/qt/widgets/input.h b/selfdrive/ui/qt/widgets/input.h new file mode 100644 index 00000000000000..6c47a31d872b9a --- /dev/null +++ b/selfdrive/ui/qt/widgets/input.h @@ -0,0 +1,79 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/keyboard.h" + + +class QDialogBase : public QDialog { + Q_OBJECT + +protected: + QDialogBase(QWidget *parent); + bool eventFilter(QObject *o, QEvent *e) override; + +public slots: + int exec() override; +}; + +class InputDialog : public QDialogBase { + Q_OBJECT + +public: + explicit InputDialog(const QString &title, QWidget *parent, const QString &subtitle = "", bool secret = false); + static QString getText(const QString &title, QWidget *parent, const QString &substitle = "", + bool secret = false, int minLength = -1, const QString &defaultText = ""); + QString text(); + void setMessage(const QString &message, bool clearInputField = true); + void setMinLength(int length); + void show(); + +private: + int minLength; + QLineEdit *line; + Keyboard *k; + QLabel *label; + QLabel *sublabel; + QVBoxLayout *main_layout; + QPushButton *eye_btn; + +private slots: + void handleEnter(); + +signals: + void cancel(); + void emitText(const QString &text); +}; + +class ConfirmationDialog : public QDialogBase { + Q_OBJECT + +public: + explicit ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, + const QString &cancel_text, QWidget* parent); + static bool alert(const QString &prompt_text, QWidget *parent); + static bool confirm(const QString &prompt_text, QWidget *parent); +}; + +// larger ConfirmationDialog for rich text +class RichTextDialog : public QDialogBase { + Q_OBJECT + +public: + explicit RichTextDialog(const QString &prompt_text, const QString &btn_text, QWidget* parent); + static bool alert(const QString &prompt_text, QWidget *parent); +}; + +class MultiOptionDialog : public QDialogBase { + Q_OBJECT + +public: + explicit MultiOptionDialog(const QString &prompt_text, const QStringList &l, const QString ¤t, QWidget *parent); + static QString getSelection(const QString &prompt_text, const QStringList &l, const QString ¤t, QWidget *parent); + QString selection; +}; diff --git a/selfdrive/ui/qt/widgets/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc new file mode 100644 index 00000000000000..1c596865353c85 --- /dev/null +++ b/selfdrive/ui/qt/widgets/keyboard.cc @@ -0,0 +1,160 @@ +#include "selfdrive/ui/qt/widgets/keyboard.h" + +#include + +#include +#include +#include +#include +#include + +const QString BACKSPACE_KEY = "⌫"; +const QString ENTER_KEY = "→"; + +const QMap KEY_STRETCH = {{" ", 5}, {ENTER_KEY, 2}}; + +const QStringList CONTROL_BUTTONS = {"↑", "↓", "ABC", "#+=", "123", BACKSPACE_KEY, ENTER_KEY}; + +const float key_spacing_vertical = 20; +const float key_spacing_horizontal = 15; + +KeyButton::KeyButton(const QString &text, QWidget *parent) : QPushButton(text, parent) { + setAttribute(Qt::WA_AcceptTouchEvents); + setFocusPolicy(Qt::NoFocus); +} + +bool KeyButton::event(QEvent *event) { + if (event->type() == QEvent::TouchBegin || event->type() == QEvent::TouchEnd) { + QTouchEvent *touchEvent = static_cast(event); + if (!touchEvent->touchPoints().empty()) { + const QEvent::Type mouseType = event->type() == QEvent::TouchBegin ? QEvent::MouseButtonPress : QEvent::MouseButtonRelease; + QMouseEvent mouseEvent(mouseType, touchEvent->touchPoints().front().pos(), Qt::LeftButton, Qt::LeftButton, Qt::NoModifier); + QPushButton::event(&mouseEvent); + event->accept(); + parentWidget()->update(); + return true; + } + } + return QPushButton::event(event); +} + +KeyboardLayout::KeyboardLayout(QWidget* parent, const std::vector>& layout) : QWidget(parent) { + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setMargin(0); + main_layout->setSpacing(0); + + QButtonGroup* btn_group = new QButtonGroup(this); + QObject::connect(btn_group, SIGNAL(buttonClicked(QAbstractButton*)), parent, SLOT(handleButton(QAbstractButton*))); + + for (const auto &s : layout) { + QHBoxLayout *hlayout = new QHBoxLayout; + hlayout->setSpacing(0); + + if (main_layout->count() == 1) { + hlayout->addSpacing(90); + } + + for (const QString &p : s) { + KeyButton* btn = new KeyButton(p); + if (p == BACKSPACE_KEY) { + btn->setAutoRepeat(true); + } else if (p == ENTER_KEY) { + btn->setStyleSheet("background-color: #465BEA;"); + } + btn->setFixedHeight(135 + key_spacing_vertical); + btn_group->addButton(btn); + hlayout->addWidget(btn, KEY_STRETCH.value(p, 1)); + } + + if (main_layout->count() == 1) { + hlayout->addSpacing(90); + } + + main_layout->addLayout(hlayout); + } + + setStyleSheet(QString(R"( + QPushButton { + font-size: 75px; + margin-left: %1px; + margin-right: %1px; + margin-top: %2px; + margin-bottom: %2px; + padding: 0px; + border-radius: 10px; + color: #dddddd; + background-color: #444444; + } + QPushButton:pressed { + background-color: #333333; + } + )").arg(key_spacing_vertical / 2).arg(key_spacing_horizontal / 2)); +} + +Keyboard::Keyboard(QWidget *parent) : QFrame(parent) { + main_layout = new QStackedLayout(this); + main_layout->setMargin(0); + + // lowercase + std::vector> lowercase = { + {"q","w","e","r","t","y","u","i","o","p"}, + {"a","s","d","f","g","h","j","k","l"}, + {"↑","z","x","c","v","b","n","m",BACKSPACE_KEY}, + {"123"," ",".",ENTER_KEY}, + }; + main_layout->addWidget(new KeyboardLayout(this, lowercase)); + + // uppercase + std::vector> uppercase = { + {"Q","W","E","R","T","Y","U","I","O","P"}, + {"A","S","D","F","G","H","J","K","L"}, + {"↓","Z","X","C","V","B","N","M",BACKSPACE_KEY}, + {"123"," ",".",ENTER_KEY}, + }; + main_layout->addWidget(new KeyboardLayout(this, uppercase)); + + // numbers + specials + std::vector> numbers = { + {"1","2","3","4","5","6","7","8","9","0"}, + {"-","/",":",";","(",")","$","&&","@","\""}, + {"#+=",".",",","?","!","`",BACKSPACE_KEY}, + {"ABC"," ",".",ENTER_KEY}, + }; + main_layout->addWidget(new KeyboardLayout(this, numbers)); + + // extra specials + std::vector> specials = { + {"[","]","{","}","#","%","^","*","+","="}, + {"_","\\","|","~","<",">","€","£","¥","•"}, + {"123",".",",","?","!","'",BACKSPACE_KEY}, + {"ABC"," ",".",ENTER_KEY}, + }; + main_layout->addWidget(new KeyboardLayout(this, specials)); + + main_layout->setCurrentIndex(0); +} + +void Keyboard::handleButton(QAbstractButton* btn) { + const QString &key = btn->text(); + if (CONTROL_BUTTONS.contains(key)) { + if (key == "↓" || key == "ABC") { + main_layout->setCurrentIndex(0); + } else if (key == "↑") { + main_layout->setCurrentIndex(1); + } else if (key == "123") { + main_layout->setCurrentIndex(2); + } else if (key == "#+=") { + main_layout->setCurrentIndex(3); + } else if (key == ENTER_KEY) { + main_layout->setCurrentIndex(0); + emit emitEnter(); + } else if (key == BACKSPACE_KEY) { + emit emitBackspace(); + } + } else { + if ("A" <= key && key <= "Z") { + main_layout->setCurrentIndex(0); + } + emit emitKey(key); + } +} diff --git a/selfdrive/ui/qt/widgets/keyboard.h b/selfdrive/ui/qt/widgets/keyboard.h new file mode 100644 index 00000000000000..516105719bd712 --- /dev/null +++ b/selfdrive/ui/qt/widgets/keyboard.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +class KeyButton : public QPushButton { + Q_OBJECT + +public: + KeyButton(const QString &text, QWidget *parent = 0); + bool event(QEvent *event) override; +}; + +class KeyboardLayout : public QWidget { + Q_OBJECT + +public: + explicit KeyboardLayout(QWidget* parent, const std::vector>& layout); +}; + +class Keyboard : public QFrame { + Q_OBJECT + +public: + explicit Keyboard(QWidget *parent = 0); + +private: + QStackedLayout* main_layout; + +private slots: + void handleButton(QAbstractButton* m_button); + +signals: + void emitKey(const QString &s); + void emitBackspace(); + void emitEnter(); +}; diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc new file mode 100644 index 00000000000000..937ea02f867aed --- /dev/null +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -0,0 +1,118 @@ +#include "selfdrive/ui/qt/widgets/offroad_alerts.h" + +#include +#include +#include + +#include "common/util.h" +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" + +AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(50); + main_layout->setSpacing(30); + + QWidget *widget = new QWidget; + scrollable_layout = new QVBoxLayout(widget); + widget->setStyleSheet("background-color: transparent;"); + main_layout->addWidget(new ScrollView(widget)); + + // bottom footer, dismiss + reboot buttons + QHBoxLayout *footer_layout = new QHBoxLayout(); + main_layout->addLayout(footer_layout); + + QPushButton *dismiss_btn = new QPushButton(tr("Close")); + dismiss_btn->setFixedSize(400, 125); + footer_layout->addWidget(dismiss_btn, 0, Qt::AlignBottom | Qt::AlignLeft); + QObject::connect(dismiss_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss); + + snooze_btn = new QPushButton(tr("Snooze Update")); + snooze_btn->setVisible(false); + snooze_btn->setFixedSize(550, 125); + footer_layout->addWidget(snooze_btn, 0, Qt::AlignBottom | Qt::AlignRight); + QObject::connect(snooze_btn, &QPushButton::clicked, [=]() { + params.putBool("SnoozeUpdate", true); + }); + QObject::connect(snooze_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss); + snooze_btn->setStyleSheet(R"(color: white; background-color: #4F4F4F;)"); + + if (hasRebootBtn) { + QPushButton *rebootBtn = new QPushButton(tr("Reboot and Update")); + rebootBtn->setFixedSize(600, 125); + footer_layout->addWidget(rebootBtn, 0, Qt::AlignBottom | Qt::AlignRight); + QObject::connect(rebootBtn, &QPushButton::clicked, [=]() { Hardware::reboot(); }); + } + + setStyleSheet(R"( + * { + font-size: 48px; + color: white; + } + QFrame { + border-radius: 30px; + background-color: #393939; + } + QPushButton { + color: black; + font-weight: 500; + border-radius: 30px; + background-color: white; + } + )"); +} + +int OffroadAlert::refresh() { + // build widgets for each offroad alert on first refresh + if (alerts.empty()) { + QString json = util::read_file("../controls/lib/alerts_offroad.json").c_str(); + QJsonObject obj = QJsonDocument::fromJson(json.toUtf8()).object(); + + // descending sort labels by severity + std::vector> sorted; + for (auto it = obj.constBegin(); it != obj.constEnd(); ++it) { + sorted.push_back({it.key().toStdString(), it.value()["severity"].toInt()}); + } + std::sort(sorted.begin(), sorted.end(), [=](auto &l, auto &r) { return l.second > r.second; }); + + for (auto &[key, severity] : sorted) { + QLabel *l = new QLabel(this); + alerts[key] = l; + l->setMargin(60); + l->setWordWrap(true); + l->setStyleSheet(QString("background-color: %1").arg(severity ? "#E22C2C" : "#292929")); + scrollable_layout->addWidget(l); + } + scrollable_layout->addStretch(1); + } + + int alertCount = 0; + for (const auto &[key, label] : alerts) { + QString text; + std::string bytes = params.get(key); + if (bytes.size()) { + auto doc_par = QJsonDocument::fromJson(bytes.c_str()); + text = doc_par["text"].toString(); + } + label->setText(text); + label->setVisible(!text.isEmpty()); + alertCount += !text.isEmpty(); + } + snooze_btn->setVisible(!alerts["Offroad_ConnectivityNeeded"]->text().isEmpty()); + return alertCount; +} + +UpdateAlert::UpdateAlert(QWidget *parent) : AbstractAlert(true, parent) { + releaseNotes = new QLabel(this); + releaseNotes->setWordWrap(true); + releaseNotes->setAlignment(Qt::AlignTop); + scrollable_layout->addWidget(releaseNotes); +} + +bool UpdateAlert::refresh() { + bool updateAvailable = params.getBool("UpdateAvailable"); + if (updateAvailable) { + releaseNotes->setText(params.get("ReleaseNotes").c_str()); + } + return updateAvailable; +} diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.h b/selfdrive/ui/qt/widgets/offroad_alerts.h new file mode 100644 index 00000000000000..69c12b0602aa23 --- /dev/null +++ b/selfdrive/ui/qt/widgets/offroad_alerts.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include +#include +#include + +#include "common/params.h" + +class AbstractAlert : public QFrame { + Q_OBJECT + +protected: + AbstractAlert(bool hasRebootBtn, QWidget *parent = nullptr); + + QPushButton *snooze_btn; + QVBoxLayout *scrollable_layout; + Params params; + +signals: + void dismiss(); +}; + +class UpdateAlert : public AbstractAlert { + Q_OBJECT + +public: + UpdateAlert(QWidget *parent = 0); + bool refresh(); + +private: + QLabel *releaseNotes = nullptr; +}; + +class OffroadAlert : public AbstractAlert { + Q_OBJECT + +public: + explicit OffroadAlert(QWidget *parent = 0) : AbstractAlert(false, parent) {} + int refresh(); + +private: + std::map alerts; +}; diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc new file mode 100644 index 00000000000000..a8ceee4ca9722f --- /dev/null +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -0,0 +1,333 @@ +#include "selfdrive/ui/qt/widgets/prime.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "selfdrive/ui/qt/request_repeater.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/qt_window.h" + +using qrcodegen::QrCode; + +PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) { + timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &PairingQRWidget::refresh); +} + +void PairingQRWidget::showEvent(QShowEvent *event) { + refresh(); + timer->start(5 * 60 * 1000); +} + +void PairingQRWidget::hideEvent(QHideEvent *event) { + timer->stop(); +} + +void PairingQRWidget::refresh() { + QString pairToken = CommaApi::create_jwt({{"pair", true}}); + QString qrString = "https://connect.comma.ai/?pair=" + pairToken; + this->updateQrCode(qrString); +} + +void PairingQRWidget::updateQrCode(const QString &text) { + QrCode qr = QrCode::encodeText(text.toUtf8().data(), QrCode::Ecc::LOW); + qint32 sz = qr.getSize(); + QImage im(sz, sz, QImage::Format_RGB32); + + QRgb black = qRgb(0, 0, 0); + QRgb white = qRgb(255, 255, 255); + for (int y = 0; y < sz; y++) { + for (int x = 0; x < sz; x++) { + im.setPixel(x, y, qr.getModule(x, y) ? black : white); + } + } + + // Integer division to prevent anti-aliasing + int final_sz = ((width() / sz) - 1) * sz; + img = QPixmap::fromImage(im.scaled(final_sz, final_sz, Qt::KeepAspectRatio), Qt::MonoOnly); +} + +void PairingQRWidget::paintEvent(QPaintEvent *e) { + QPainter p(this); + p.fillRect(rect(), Qt::white); + + QSize s = (size() - img.size()) / 2; + p.drawPixmap(s.width(), s.height(), img); +} + + +PairingPopup::PairingPopup(QWidget *parent) : QDialogBase(parent) { + QHBoxLayout *hlayout = new QHBoxLayout(this); + hlayout->setContentsMargins(0, 0, 0, 0); + hlayout->setSpacing(0); + + setStyleSheet("PairingPopup { background-color: #E0E0E0; }"); + + // text + QVBoxLayout *vlayout = new QVBoxLayout(); + vlayout->setContentsMargins(85, 70, 50, 70); + vlayout->setSpacing(50); + hlayout->addLayout(vlayout, 1); + { + QPushButton *close = new QPushButton(QIcon(":/icons/close.svg"), "", this); + close->setIconSize(QSize(80, 80)); + close->setStyleSheet("border: none;"); + vlayout->addWidget(close, 0, Qt::AlignLeft); + QObject::connect(close, &QPushButton::clicked, this, &QDialog::reject); + + vlayout->addSpacing(30); + + QLabel *title = new QLabel(tr("Pair your device to your comma account"), this); + title->setStyleSheet("font-size: 75px; color: black;"); + title->setWordWrap(true); + vlayout->addWidget(title); + + QLabel *instructions = new QLabel(QString(R"( +
      +
    1. %1
      2. +
    2. %2
      3. +
    3. %3
      4. +
    + )").arg(tr("Go to https://connect.comma.ai on your phone")) + .arg(tr("Click \"add new device\" and scan the QR code on the right")) + .arg(tr("Bookmark connect.comma.ai to your home screen to use it like an app")), this); + + instructions->setStyleSheet("font-size: 47px; font-weight: bold; color: black;"); + instructions->setWordWrap(true); + vlayout->addWidget(instructions); + + vlayout->addStretch(); + } + + // QR code + PairingQRWidget *qr = new PairingQRWidget(this); + hlayout->addWidget(qr, 1); +} + + +PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) { + mainLayout = new QVBoxLayout(this); + mainLayout->setMargin(0); + mainLayout->setSpacing(30); + + // subscribed prime layout + QWidget *primeWidget = new QWidget; + primeWidget->setObjectName("primeWidget"); + QVBoxLayout *primeLayout = new QVBoxLayout(primeWidget); + primeLayout->setMargin(0); + primeWidget->setContentsMargins(60, 50, 60, 50); + + QLabel* subscribed = new QLabel(tr("✓ SUBSCRIBED")); + subscribed->setStyleSheet("font-size: 41px; font-weight: bold; color: #86FF4E;"); + primeLayout->addWidget(subscribed, 0, Qt::AlignTop); + + primeLayout->addSpacing(60); + + QLabel* commaPrime = new QLabel(tr("comma prime")); + commaPrime->setStyleSheet("font-size: 75px; font-weight: bold;"); + primeLayout->addWidget(commaPrime, 0, Qt::AlignTop); + + primeLayout->addSpacing(20); + + QLabel* connectUrl = new QLabel(tr("CONNECT.COMMA.AI")); + connectUrl->setStyleSheet("font-size: 41px; font-family: Inter SemiBold; color: #A0A0A0;"); + primeLayout->addWidget(connectUrl, 0, Qt::AlignTop); + + mainLayout->addWidget(primeWidget); + + // comma points layout + QWidget *pointsWidget = new QWidget; + pointsWidget->setObjectName("primeWidget"); + QVBoxLayout *pointsLayout = new QVBoxLayout(pointsWidget); + pointsLayout->setMargin(0); + pointsWidget->setContentsMargins(60, 50, 60, 50); + + QLabel* commaPoints = new QLabel(tr("COMMA POINTS")); + commaPoints->setStyleSheet("font-size: 41px; font-family: Inter SemiBold;"); + pointsLayout->addWidget(commaPoints, 0, Qt::AlignTop); + + points = new QLabel("0"); + points->setStyleSheet("font-size: 91px; font-weight: bold;"); + pointsLayout->addWidget(points, 0, Qt::AlignTop); + + mainLayout->addWidget(pointsWidget); + + mainLayout->addStretch(); + + // set up API requests + if (auto dongleId = getDongleId()) { + QString url = CommaApi::BASE_URL + "/v1/devices/" + *dongleId + "/owner"; + RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_Owner", 6); + QObject::connect(repeater, &RequestRepeater::requestDone, this, &PrimeUserWidget::replyFinished); + } +} + +void PrimeUserWidget::replyFinished(const QString &response) { + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting points"; + return; + } + + QJsonObject json = doc.object(); + points->setText(QString::number(json["points"].toInt())); +} + +PrimeAdWidget::PrimeAdWidget(QWidget* parent) : QFrame(parent) { + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(80, 90, 80, 60); + main_layout->setSpacing(0); + + QLabel *upgrade = new QLabel(tr("Upgrade Now")); + upgrade->setStyleSheet("font-size: 75px; font-weight: bold;"); + main_layout->addWidget(upgrade, 0, Qt::AlignTop); + main_layout->addSpacing(50); + + QLabel *description = new QLabel(tr("Become a comma prime member at connect.comma.ai")); + description->setStyleSheet("font-size: 60px; font-weight: light; color: white;"); + description->setWordWrap(true); + main_layout->addWidget(description, 0, Qt::AlignTop); + + main_layout->addStretch(); + + QLabel *features = new QLabel(tr("PRIME FEATURES:")); + features->setStyleSheet("font-size: 41px; font-weight: bold; color: #E5E5E5;"); + main_layout->addWidget(features, 0, Qt::AlignBottom); + main_layout->addSpacing(30); + + QVector bullets = {tr("Remote access"), tr("1 year of storage"), tr("Developer perks")}; + for (auto &b: bullets) { + const QString check = "✓ "; + QLabel *l = new QLabel(check + b); + l->setAlignment(Qt::AlignLeft); + l->setStyleSheet("font-size: 50px; margin-bottom: 15px;"); + main_layout->addWidget(l, 0, Qt::AlignBottom); + } + + setStyleSheet(R"( + PrimeAdWidget { + border-radius: 10px; + background-color: #333333; + } + )"); +} + + +SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { + mainLayout = new QStackedWidget; + + // Unpaired, registration prompt layout + + QWidget* finishRegistration = new QWidget; + finishRegistration->setObjectName("primeWidget"); + QVBoxLayout* finishRegistationLayout = new QVBoxLayout(finishRegistration); + finishRegistationLayout->setContentsMargins(30, 75, 30, 45); + finishRegistationLayout->setSpacing(0); + + QLabel* registrationTitle = new QLabel(tr("Finish Setup")); + registrationTitle->setStyleSheet("font-size: 75px; font-weight: bold; margin-left: 55px;"); + finishRegistationLayout->addWidget(registrationTitle); + + finishRegistationLayout->addSpacing(30); + + QLabel* registrationDescription = new QLabel(tr("Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.")); + registrationDescription->setWordWrap(true); + registrationDescription->setStyleSheet("font-size: 55px; font-weight: light; margin-left: 55px;"); + finishRegistationLayout->addWidget(registrationDescription); + + finishRegistationLayout->addStretch(); + + QPushButton* pair = new QPushButton(tr("Pair device")); + pair->setFixedHeight(220); + pair->setStyleSheet(R"( + QPushButton { + font-size: 55px; + font-weight: 400; + border-radius: 10px; + background-color: #465BEA; + } + QPushButton:pressed { + background-color: #3049F4; + } + )"); + finishRegistationLayout->addWidget(pair); + + popup = new PairingPopup(this); + QObject::connect(pair, &QPushButton::clicked, popup, &PairingPopup::exec); + + mainLayout->addWidget(finishRegistration); + + // build stacked layout + QVBoxLayout *outer_layout = new QVBoxLayout(this); + outer_layout->setContentsMargins(0, 0, 0, 0); + outer_layout->addWidget(mainLayout); + + primeAd = new PrimeAdWidget; + mainLayout->addWidget(primeAd); + + primeUser = new PrimeUserWidget; + mainLayout->addWidget(primeUser); + + mainLayout->setCurrentWidget(primeAd); + + setFixedWidth(750); + setStyleSheet(R"( + #primeWidget { + border-radius: 10px; + background-color: #333333; + } + )"); + + // Retain size while hidden + QSizePolicy sp_retain = sizePolicy(); + sp_retain.setRetainSizeWhenHidden(true); + setSizePolicy(sp_retain); + + // set up API requests + if (auto dongleId = getDongleId()) { + QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/"; + RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_Device", 5); + + QObject::connect(repeater, &RequestRepeater::requestDone, this, &SetupWidget::replyFinished); + } + hide(); // Only show when first request comes back +} + +void SetupWidget::replyFinished(const QString &response, bool success) { + show(); + if (!success) return; + + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting pairing and prime status"; + return; + } + + QJsonObject json = doc.object(); + int prime_type = json["prime_type"].toInt(); + + if (uiState()->prime_type != prime_type) { + uiState()->prime_type = prime_type; + Params().put("PrimeType", std::to_string(prime_type)); + } + + if (!json["is_paired"].toBool()) { + mainLayout->setCurrentIndex(0); + } else { + popup->reject(); + + if (prime_type) { + mainLayout->setCurrentWidget(primeUser); + } else { + mainLayout->setCurrentWidget(primeAd); + } + } +} diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h new file mode 100644 index 00000000000000..0a1d93250d96c4 --- /dev/null +++ b/selfdrive/ui/qt/widgets/prime.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/input.h" + +enum PrimeType { + NONE = 0, + MAGENTA = 1, + LITE = 2, + BLUE = 3, + MAGENTA_NEW = 4, +}; + +// pairing QR code +class PairingQRWidget : public QWidget { + Q_OBJECT + +public: + explicit PairingQRWidget(QWidget* parent = 0); + void paintEvent(QPaintEvent*) override; + +private: + QPixmap img; + QTimer *timer; + void updateQrCode(const QString &text); + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + +private slots: + void refresh(); +}; + +// pairing popup widget +class PairingPopup : public QDialogBase { + Q_OBJECT + +public: + explicit PairingPopup(QWidget* parent); +}; + +// widget for paired users with prime +class PrimeUserWidget : public QWidget { + Q_OBJECT +public: + explicit PrimeUserWidget(QWidget* parent = 0); + +private: + QVBoxLayout* mainLayout; + QLabel* points; + +private slots: + void replyFinished(const QString &response); +}; + + +// widget for paired users without prime +class PrimeAdWidget : public QFrame { + Q_OBJECT +public: + explicit PrimeAdWidget(QWidget* parent = 0); +}; + +// container widget +class SetupWidget : public QFrame { + Q_OBJECT + +public: + explicit SetupWidget(QWidget* parent = 0); + +private: + PairingPopup *popup; + QStackedWidget *mainLayout; + PrimeAdWidget *primeAd; + PrimeUserWidget *primeUser; + +private slots: + void replyFinished(const QString &response, bool success); +}; diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc new file mode 100644 index 00000000000000..bd4309d8d06049 --- /dev/null +++ b/selfdrive/ui/qt/widgets/scrollview.cc @@ -0,0 +1,47 @@ +#include "selfdrive/ui/qt/widgets/scrollview.h" + +#include +#include + +ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent) { + setWidget(w); + setWidgetResizable(true); + setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setStyleSheet("background-color: transparent;"); + + QString style = R"( + QScrollBar:vertical { + border: none; + background: transparent; + width: 10px; + margin: 0; + } + QScrollBar::handle:vertical { + min-height: 0px; + border-radius: 5px; + background-color: white; + } + QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical { + height: 0px; + } + QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { + background: none; + } + )"; + verticalScrollBar()->setStyleSheet(style); + horizontalScrollBar()->setStyleSheet(style); + + QScroller *scroller = QScroller::scroller(this->viewport()); + QScrollerProperties sp = scroller->scrollerProperties(); + + sp.setScrollMetric(QScrollerProperties::VerticalOvershootPolicy, QVariant::fromValue(QScrollerProperties::OvershootAlwaysOff)); + sp.setScrollMetric(QScrollerProperties::HorizontalOvershootPolicy, QVariant::fromValue(QScrollerProperties::OvershootAlwaysOff)); + sp.setScrollMetric(QScrollerProperties::MousePressEventDelay, 0.01); + scroller->grabGesture(this->viewport(), QScroller::LeftMouseButtonGesture); + scroller->setScrollerProperties(sp); +} + +void ScrollView::hideEvent(QHideEvent *e) { + verticalScrollBar()->setValue(0); +} diff --git a/selfdrive/ui/qt/widgets/scrollview.h b/selfdrive/ui/qt/widgets/scrollview.h new file mode 100644 index 00000000000000..024331aa39dd2e --- /dev/null +++ b/selfdrive/ui/qt/widgets/scrollview.h @@ -0,0 +1,12 @@ +#pragma once + +#include + +class ScrollView : public QScrollArea { + Q_OBJECT + +public: + explicit ScrollView(QWidget *w = nullptr, QWidget *parent = nullptr); +protected: + void hideEvent(QHideEvent *e) override; +}; diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc new file mode 100644 index 00000000000000..f17604b3e5e8da --- /dev/null +++ b/selfdrive/ui/qt/widgets/ssh_keys.cc @@ -0,0 +1,65 @@ +#include "selfdrive/ui/qt/widgets/ssh_keys.h" + +#include "common/params.h" +#include "selfdrive/ui/qt/api.h" +#include "selfdrive/ui/qt/widgets/input.h" + +SshControl::SshControl() : ButtonControl(tr("SSH Keys"), "", tr("Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.")) { + username_label.setAlignment(Qt::AlignRight | Qt::AlignVCenter); + username_label.setStyleSheet("color: #aaaaaa"); + hlayout->insertWidget(1, &username_label); + + QObject::connect(this, &ButtonControl::clicked, [=]() { + if (text() == tr("ADD")) { + QString username = InputDialog::getText(tr("Enter your GitHub username"), this); + if (username.length() > 0) { + setText(tr("LOADING")); + setEnabled(false); + getUserKeys(username); + } + } else { + params.remove("GithubUsername"); + params.remove("GithubSshKeys"); + refresh(); + } + }); + + refresh(); +} + +void SshControl::refresh() { + QString param = QString::fromStdString(params.get("GithubSshKeys")); + if (param.length()) { + username_label.setText(QString::fromStdString(params.get("GithubUsername"))); + setText(tr("REMOVE")); + } else { + username_label.setText(""); + setText(tr("ADD")); + } + setEnabled(true); +} + +void SshControl::getUserKeys(const QString &username) { + HttpRequest *request = new HttpRequest(this, false); + QObject::connect(request, &HttpRequest::requestDone, [=](const QString &resp, bool success) { + if (success) { + if (!resp.isEmpty()) { + params.put("GithubUsername", username.toStdString()); + params.put("GithubSshKeys", resp.toStdString()); + } else { + ConfirmationDialog::alert(tr("Username '%1' has no keys on GitHub").arg(username), this); + } + } else { + if (request->timeout()) { + ConfirmationDialog::alert(tr("Request timed out"), this); + } else { + ConfirmationDialog::alert(tr("Username '%1' doesn't exist on GitHub").arg(username), this); + } + } + + refresh(); + request->deleteLater(); + }); + + request->sendRequest("https://github.com/" + username + ".keys"); +} diff --git a/selfdrive/ui/qt/widgets/ssh_keys.h b/selfdrive/ui/qt/widgets/ssh_keys.h new file mode 100644 index 00000000000000..01e2ab83ce897d --- /dev/null +++ b/selfdrive/ui/qt/widgets/ssh_keys.h @@ -0,0 +1,34 @@ +#pragma once + +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/qt/widgets/controls.h" + +// SSH enable toggle +class SshToggle : public ToggleControl { + Q_OBJECT + +public: + SshToggle() : ToggleControl(tr("Enable SSH"), "", "", Hardware::get_ssh_enabled()) { + QObject::connect(this, &SshToggle::toggleFlipped, [=](bool state) { + Hardware::set_ssh_enabled(state); + }); + } +}; + +// SSH key management widget +class SshControl : public ButtonControl { + Q_OBJECT + +public: + SshControl(); + +private: + Params params; + + QLabel username_label; + + void refresh(); + void getUserKeys(const QString &username); +}; diff --git a/selfdrive/ui/qt/widgets/toggle.cc b/selfdrive/ui/qt/widgets/toggle.cc new file mode 100644 index 00000000000000..82302ad5bc78f3 --- /dev/null +++ b/selfdrive/ui/qt/widgets/toggle.cc @@ -0,0 +1,83 @@ +#include "selfdrive/ui/qt/widgets/toggle.h" + +#include + +Toggle::Toggle(QWidget *parent) : QAbstractButton(parent), +_height(80), +_height_rect(60), +on(false), +_anim(new QPropertyAnimation(this, "offset_circle", this)) +{ + _radius = _height / 2; + _x_circle = _radius; + _y_circle = _radius; + _y_rect = (_height - _height_rect)/2; + circleColor = QColor(0xffffff); // placeholder + green = QColor(0xffffff); // placeholder + setEnabled(true); +} + +void Toggle::paintEvent(QPaintEvent *e) { + this->setFixedHeight(_height); + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing, true); + + // Draw toggle background left + p.setBrush(green); + p.drawRoundedRect(QRect(0, _y_rect, _x_circle + _radius, _height_rect), _height_rect/2, _height_rect/2); + + // Draw toggle background right + p.setBrush(QColor(0x393939)); + p.drawRoundedRect(QRect(_x_circle - _radius, _y_rect, width() - (_x_circle - _radius), _height_rect), _height_rect/2, _height_rect/2); + + // Draw toggle circle + p.setBrush(circleColor); + p.drawEllipse(QRectF(_x_circle - _radius, _y_circle - _radius, 2 * _radius, 2 * _radius)); +} + +void Toggle::mouseReleaseEvent(QMouseEvent *e) { + if (!enabled) { + return; + } + const int left = _radius; + const int right = width() - _radius; + if ((_x_circle != left && _x_circle != right) || !this->rect().contains(e->localPos().toPoint())) { + // If mouse release isn't in rect or animation is running, don't parse touch events + return; + } + if (e->button() & Qt::LeftButton) { + togglePosition(); + emit stateChanged(on); + } +} + +void Toggle::togglePosition() { + on = !on; + const int left = _radius; + const int right = width() - _radius; + _anim->setStartValue(on ? left + immediateOffset : right - immediateOffset); + _anim->setEndValue(on ? right : left); + _anim->setDuration(animation_duration); + _anim->start(); + repaint(); +} + +void Toggle::enterEvent(QEvent *e) { + QAbstractButton::enterEvent(e); +} + +bool Toggle::getEnabled() { + return enabled; +} + +void Toggle::setEnabled(bool value) { + enabled = value; + if (value) { + circleColor.setRgb(0xfafafa); + green.setRgb(0x33ab4c); + } else { + circleColor.setRgb(0x888888); + green.setRgb(0x227722); + } +} diff --git a/selfdrive/ui/qt/widgets/toggle.h b/selfdrive/ui/qt/widgets/toggle.h new file mode 100644 index 00000000000000..e7263a008fa183 --- /dev/null +++ b/selfdrive/ui/qt/widgets/toggle.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +class Toggle : public QAbstractButton { + Q_OBJECT + Q_PROPERTY(int offset_circle READ offset_circle WRITE set_offset_circle CONSTANT) + +public: + Toggle(QWidget* parent = nullptr); + void togglePosition(); + bool on; + int animation_duration = 150; + int immediateOffset = 0; + int offset_circle() const { + return _x_circle; + } + + void set_offset_circle(int o) { + _x_circle = o; + update(); + } + bool getEnabled(); + void setEnabled(bool value); + +protected: + void paintEvent(QPaintEvent*) override; + void mouseReleaseEvent(QMouseEvent*) override; + void enterEvent(QEvent*) override; + +private: + QColor circleColor; + QColor green; + bool enabled = true; + int _x_circle, _y_circle; + int _height, _radius; + int _height_rect, _y_rect; + QPropertyAnimation *_anim = nullptr; + +signals: + void stateChanged(bool new_state); +}; diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc new file mode 100644 index 00000000000000..04ce15ef230d52 --- /dev/null +++ b/selfdrive/ui/qt/window.cc @@ -0,0 +1,87 @@ +#include "selfdrive/ui/qt/window.h" + +#include + +#include "system/hardware/hw.h" + +MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { + main_layout = new QStackedLayout(this); + main_layout->setMargin(0); + + homeWindow = new HomeWindow(this); + main_layout->addWidget(homeWindow); + QObject::connect(homeWindow, &HomeWindow::openSettings, this, &MainWindow::openSettings); + QObject::connect(homeWindow, &HomeWindow::closeSettings, this, &MainWindow::closeSettings); + + settingsWindow = new SettingsWindow(this); + main_layout->addWidget(settingsWindow); + QObject::connect(settingsWindow, &SettingsWindow::closeSettings, this, &MainWindow::closeSettings); + QObject::connect(settingsWindow, &SettingsWindow::reviewTrainingGuide, [=]() { + onboardingWindow->showTrainingGuide(); + main_layout->setCurrentWidget(onboardingWindow); + }); + QObject::connect(settingsWindow, &SettingsWindow::showDriverView, [=] { + homeWindow->showDriverView(true); + }); + + onboardingWindow = new OnboardingWindow(this); + main_layout->addWidget(onboardingWindow); + QObject::connect(onboardingWindow, &OnboardingWindow::onboardingDone, [=]() { + main_layout->setCurrentWidget(homeWindow); + }); + if (!onboardingWindow->completed()) { + main_layout->setCurrentWidget(onboardingWindow); + } + + QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + if (!offroad) { + closeSettings(); + } + }); + QObject::connect(&device, &Device::interactiveTimout, [=]() { + if (main_layout->currentWidget() == settingsWindow) { + closeSettings(); + } + }); + + // load fonts + QFontDatabase::addApplicationFont("../assets/fonts/Inter-Black.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-Bold.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-ExtraBold.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-ExtraLight.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-Medium.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-Regular.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-SemiBold.ttf"); + QFontDatabase::addApplicationFont("../assets/fonts/Inter-Thin.ttf"); + + // no outline to prevent the focus rectangle + setStyleSheet(R"( + * { + font-family: Inter; + outline: none; + } + )"); + setAttribute(Qt::WA_NoSystemBackground); +} + +void MainWindow::openSettings() { + main_layout->setCurrentWidget(settingsWindow); +} + +void MainWindow::closeSettings() { + main_layout->setCurrentWidget(homeWindow); + + if (uiState()->scene.started) { + homeWindow->showSidebar(false); + } +} + +bool MainWindow::eventFilter(QObject *obj, QEvent *event) { + const static QSet evts({QEvent::MouseButtonPress, QEvent::MouseMove, + QEvent::TouchBegin, QEvent::TouchUpdate, QEvent::TouchEnd}); + + if (evts.contains(event->type())) { + device.resetInteractiveTimout(); + } + return false; +} diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h new file mode 100644 index 00000000000000..0bd328aa8ab017 --- /dev/null +++ b/selfdrive/ui/qt/window.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include + +#include "selfdrive/ui/qt/home.h" +#include "selfdrive/ui/qt/offroad/onboarding.h" +#include "selfdrive/ui/qt/offroad/settings.h" + +class MainWindow : public QWidget { + Q_OBJECT + +public: + explicit MainWindow(QWidget *parent = 0); + +private: + bool eventFilter(QObject *obj, QEvent *event) override; + void openSettings(); + void closeSettings(); + + Device device; + + QStackedLayout *main_layout; + HomeWindow *homeWindow; + SettingsWindow *settingsWindow; + OnboardingWindow *onboardingWindow; +}; diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py deleted file mode 100644 index d88410ada35949..00000000000000 --- a/selfdrive/ui/soundd.py +++ /dev/null @@ -1,174 +0,0 @@ -import math -import numpy as np -import time -import wave - - -from cereal import car, messaging -from openpilot.common.basedir import BASEDIR -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import Ratekeeper -from openpilot.common.utils import retry -from openpilot.common.swaglog import cloudlog - -from openpilot.system import micd -from openpilot.system.hardware import HARDWARE - -SAMPLE_RATE = 48000 -SAMPLE_BUFFER = 4096 # (approx 100ms) -MAX_VOLUME = 1.0 -MIN_VOLUME = 0.1 -SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds -FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES) - -AMBIENT_DB = 30 # DB where MIN_VOLUME is applied -DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied - -VOLUME_BASE = 20 -if HARDWARE.get_device_type() == "tizi": - VOLUME_BASE = 10 - -AudibleAlert = car.CarControl.HUDControl.AudibleAlert - - -sound_list: dict[int, tuple[str, int | None, float]] = { - # AudibleAlert, file name, play count (none for infinite) - AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), - AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), - AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), - - AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), - AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), - AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), - - AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), - AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), -} -if HARDWARE.get_device_type() == "tizi": - sound_list.update({ - AudibleAlert.engage: ("engage_tizi.wav", 1, MAX_VOLUME), - AudibleAlert.disengage: ("disengage_tizi.wav", 1, MAX_VOLUME), - }) - -def check_selfdrive_timeout_alert(sm): - ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] - - if ss_missing > SELFDRIVE_STATE_TIMEOUT: - if sm['selfdriveState'].enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10: - return True - - return False - - -class Soundd: - def __init__(self): - self.load_sounds() - - self.current_alert = AudibleAlert.none - self.current_volume = MIN_VOLUME - self.current_sound_frame = 0 - - self.selfdrive_timeout_alert = False - - self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) - - def load_sounds(self): - self.loaded_sounds: dict[int, np.ndarray] = {} - - # Load all sounds - for sound in sound_list: - filename, play_count, volume = sound_list[sound] - - with wave.open(BASEDIR + "/selfdrive/assets/sounds/" + filename, 'r') as wavefile: - assert wavefile.getnchannels() == 1 - assert wavefile.getsampwidth() == 2 - assert wavefile.getframerate() == SAMPLE_RATE - - length = wavefile.getnframes() - self.loaded_sounds[sound] = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2) - - def get_sound_data(self, frames): # get "frames" worth of data from the current alert sound, looping when required - - ret = np.zeros(frames, dtype=np.float32) - - if self.current_alert != AudibleAlert.none: - num_loops = sound_list[self.current_alert][1] - sound_data = self.loaded_sounds[self.current_alert] - written_frames = 0 - - current_sound_frame = self.current_sound_frame % len(sound_data) - loops = self.current_sound_frame // len(sound_data) - - while written_frames < frames and (num_loops is None or loops < num_loops): - available_frames = sound_data.shape[0] - current_sound_frame - frames_to_write = min(available_frames, frames - written_frames) - ret[written_frames:written_frames+frames_to_write] = sound_data[current_sound_frame:current_sound_frame+frames_to_write] - written_frames += frames_to_write - self.current_sound_frame += frames_to_write - - return ret * self.current_volume - - def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: - if status: - cloudlog.warning(f"soundd stream over/underflow: {status}") - data_out[:frames, 0] = self.get_sound_data(frames) - - def update_alert(self, new_alert): - current_alert_played_once = self.current_alert == AudibleAlert.none or self.current_sound_frame > len(self.loaded_sounds[self.current_alert]) - if self.current_alert != new_alert and (new_alert != AudibleAlert.none or current_alert_played_once): - self.current_alert = new_alert - self.current_sound_frame = 0 - - def get_audible_alert(self, sm): - if sm.updated['selfdriveState']: - new_alert = sm['selfdriveState'].alertSound.raw - self.update_alert(new_alert) - elif check_selfdrive_timeout_alert(sm): - self.update_alert(AudibleAlert.warningImmediate) - self.selfdrive_timeout_alert = True - elif self.selfdrive_timeout_alert: - self.update_alert(AudibleAlert.none) - self.selfdrive_timeout_alert = False - - def calculate_volume(self, weighted_db): - volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME - return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)) - - @retry(attempts=10, delay=3) - def get_stream(self, sd): - # reload sounddevice to reinitialize portaudio - sd._terminate() - sd._initialize() - return sd.OutputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER) - - def soundd_thread(self): - # sounddevice must be imported after forking processes - import sounddevice as sd - - sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) - - with self.get_stream(sd) as stream: - rk = Ratekeeper(20) - - cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}") - while True: - sm.update(0) - - if sm.updated['soundPressure'] and self.current_alert == AudibleAlert.none: # only update volume filter when not playing alert - self.spl_filter_weighted.update(sm["soundPressure"].soundPressureWeightedDb) - self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) - - self.get_audible_alert(sm) - - rk.keep_time() - - assert stream.active - - -def main(): - s = Soundd() - s.soundd_thread() - - -if __name__ == "__main__": - main() diff --git a/selfdrive/ui/soundd/.gitignore b/selfdrive/ui/soundd/.gitignore new file mode 100644 index 00000000000000..c47f949d377a77 --- /dev/null +++ b/selfdrive/ui/soundd/.gitignore @@ -0,0 +1 @@ +_soundd diff --git a/selfdrive/ui/soundd/main.cc b/selfdrive/ui/soundd/main.cc new file mode 100644 index 00000000000000..64088deff83d21 --- /dev/null +++ b/selfdrive/ui/soundd/main.cc @@ -0,0 +1,22 @@ +#include + +#include + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/soundd/sound.h" + +void sigHandler(int s) { + qApp->quit(); +} + +int main(int argc, char **argv) { + qInstallMessageHandler(swagLogMessageHandler); + setpriority(PRIO_PROCESS, 0, -20); + + QApplication a(argc, argv); + std::signal(SIGINT, sigHandler); + std::signal(SIGTERM, sigHandler); + + Sound sound; + return a.exec(); +} diff --git a/selfdrive/ui/soundd/sound.cc b/selfdrive/ui/soundd/sound.cc new file mode 100644 index 00000000000000..6830450d8f8cc3 --- /dev/null +++ b/selfdrive/ui/soundd/sound.cc @@ -0,0 +1,81 @@ +#include "selfdrive/ui/soundd/sound.h" + +#include + +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/util.h" + +// TODO: detect when we can't play sounds +// TODO: detect when we can't display the UI + +Sound::Sound(QObject *parent) : sm({"carState", "controlsState", "deviceState"}) { + qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName(); + + for (auto &[alert, fn, loops] : sound_list) { + QSoundEffect *s = new QSoundEffect(this); + QObject::connect(s, &QSoundEffect::statusChanged, [=]() { + assert(s->status() != QSoundEffect::Error); + }); + s->setVolume(Hardware::MIN_VOLUME); + s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn)); + sounds[alert] = {s, loops}; + } + + QTimer *timer = new QTimer(this); + QObject::connect(timer, &QTimer::timeout, this, &Sound::update); + timer->start(1000 / UI_FREQ); +}; + +void Sound::update() { + const bool started_prev = sm["deviceState"].getDeviceState().getStarted(); + sm.update(0); + + const bool started = sm["deviceState"].getDeviceState().getStarted(); + if (started && !started_prev) { + started_frame = sm.frame; + } + + // no sounds while offroad + // also no sounds if nothing is alive in case thermald crashes while offroad + const bool crashed = (sm.frame - std::max(sm.rcv_frame("deviceState"), sm.rcv_frame("controlsState"))) > 10*UI_FREQ; + if (!started || crashed) { + setAlert({}); + return; + } + + // scale volume with speed + if (sm.updated("carState")) { + float volume = util::map_val(sm["carState"].getCarState().getVEgo(), 11.f, 20.f, 0.f, 1.0f); + volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale); + volume = util::map_val(volume, 0.f, 1.f, Hardware::MIN_VOLUME, Hardware::MAX_VOLUME); + for (auto &[s, loops] : sounds) { + s->setVolume(std::round(100 * volume) / 100); + } + } + + setAlert(Alert::get(sm, started_frame)); +} + +void Sound::setAlert(const Alert &alert) { + if (!current_alert.equal(alert)) { + current_alert = alert; + // stop sounds + for (auto &[s, loops] : sounds) { + // Only stop repeating sounds + if (s->loopsRemaining() > 1 || s->loopsRemaining() == QSoundEffect::Infinite) { + s->stop(); + } + } + + // play sound + if (alert.sound != AudibleAlert::NONE) { + auto &[s, loops] = sounds[alert.sound]; + s->setLoopCount(loops); + s->play(); + } + } +} diff --git a/selfdrive/ui/soundd/sound.h b/selfdrive/ui/soundd/sound.h new file mode 100644 index 00000000000000..7e009d28ad5df4 --- /dev/null +++ b/selfdrive/ui/soundd/sound.h @@ -0,0 +1,34 @@ +#include +#include +#include + +#include "system/hardware/hw.h" +#include "selfdrive/ui/ui.h" + +const std::tuple sound_list[] = { + // AudibleAlert, file name, loop count + {AudibleAlert::ENGAGE, "engage.wav", 0}, + {AudibleAlert::DISENGAGE, "disengage.wav", 0}, + {AudibleAlert::REFUSE, "refuse.wav", 0}, + + {AudibleAlert::PROMPT, "prompt.wav", 0}, + {AudibleAlert::PROMPT_REPEAT, "prompt.wav", QSoundEffect::Infinite}, + {AudibleAlert::PROMPT_DISTRACTED, "prompt_distracted.wav", QSoundEffect::Infinite}, + + {AudibleAlert::WARNING_SOFT, "warning_soft.wav", QSoundEffect::Infinite}, + {AudibleAlert::WARNING_IMMEDIATE, "warning_immediate.wav", QSoundEffect::Infinite}, +}; + +class Sound : public QObject { +public: + explicit Sound(QObject *parent = 0); + +protected: + void update(); + void setAlert(const Alert &alert); + + Alert current_alert = {}; + QMap> sounds; + SubMaster sm; + uint64_t started_frame; +}; diff --git a/selfdrive/ui/soundd/soundd b/selfdrive/ui/soundd/soundd new file mode 100755 index 00000000000000..66dda46d5b68ea --- /dev/null +++ b/selfdrive/ui/soundd/soundd @@ -0,0 +1,5 @@ +#!/bin/sh +cd "$(dirname "$0")" +export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" +export QT_QPA_PLATFORM="offscreen" +exec ./_soundd diff --git a/selfdrive/ui/spinner b/selfdrive/ui/spinner new file mode 100755 index 00000000000000..6c8e533cc8d2e9 --- /dev/null +++ b/selfdrive/ui/spinner @@ -0,0 +1,8 @@ +#!/bin/sh + +if [ -f /TICI ] && [ ! -f qt/spinner ]; then + cp qt/spinner_larch64 qt/spinner +fi + +export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" +exec ./qt/spinner "$1" diff --git a/selfdrive/ui/tests/.gitignore b/selfdrive/ui/tests/.gitignore index 98f2a5e8ce9e1f..26335744f3daf4 100644 --- a/selfdrive/ui/tests/.gitignore +++ b/selfdrive/ui/tests/.gitignore @@ -1,9 +1,4 @@ test +playsound +test_sound test_translations -test_ui/report_1 -test_ui/raylib_report - -diff/*.mp4 -diff/*.html -diff/.coverage -diff/htmlcov/ diff --git a/selfdrive/ui/tests/body.py b/selfdrive/ui/tests/body.py index 07a2ef5128ae10..c34e717eafcad4 100755 --- a/selfdrive/ui/tests/body.py +++ b/selfdrive/ui/tests/body.py @@ -8,7 +8,7 @@ batt = 1. while True: msg = messaging.new_message('carParams') - msg.carParams.brand = "body" + msg.carParams.carName = "COMMA BODY" msg.carParams.notCar = True pm.send('carParams', msg) diff --git a/selfdrive/ui/tests/create_test_translations.sh b/selfdrive/ui/tests/create_test_translations.sh new file mode 100755 index 00000000000000..451a3cbfb04c10 --- /dev/null +++ b/selfdrive/ui/tests/create_test_translations.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +set -e + +UI_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"/.. +TEST_TEXT="(WRAPPED_SOURCE_TEXT)" +TEST_TS_FILE=$UI_DIR/translations/main_test_en.ts +TEST_QM_FILE=$UI_DIR/translations/main_test_en.qm + +# translation strings +UNFINISHED="<\/translation>" +TRANSLATED="$TEST_TEXT<\/translation>" + +mkdir -p $UI_DIR/translations +rm -f $TEST_TS_FILE $TEST_QM_FILE +lupdate -recursive "$UI_DIR" -ts $TEST_TS_FILE +sed -i "s/$UNFINISHED/$TRANSLATED/" $TEST_TS_FILE +lrelease $TEST_TS_FILE diff --git a/selfdrive/ui/tests/cycle_offroad_alerts.py b/selfdrive/ui/tests/cycle_offroad_alerts.py index b577b74b00130e..6b6aea44778733 100755 --- a/selfdrive/ui/tests/cycle_offroad_alerts.py +++ b/selfdrive/ui/tests/cycle_offroad_alerts.py @@ -4,22 +4,23 @@ import time import json -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.updated.updated import parse_release_notes +from common.basedir import BASEDIR +from common.params import Params +from selfdrive.controls.lib.alertmanager import set_offroad_alert if __name__ == "__main__": params = Params() - with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f: + with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: offroad_alerts = json.load(f) t = 10 if len(sys.argv) < 2 else int(sys.argv[1]) while True: print("setting alert update") params.put_bool("UpdateAvailable", True) - params.put("UpdaterNewReleaseNotes", parse_release_notes(BASEDIR)) + r = open(os.path.join(BASEDIR, "RELEASES.md")).read() + r = r[:r.find('\n\n')] # Slice latest release notes + params.put("ReleaseNotes", r + "\n") time.sleep(t) params.put_bool("UpdateAvailable", False) diff --git a/selfdrive/ui/tests/diff/diff.py b/selfdrive/ui/tests/diff/diff.py deleted file mode 100755 index be7af5438a7de4..00000000000000 --- a/selfdrive/ui/tests/diff/diff.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import subprocess -import tempfile -import base64 -import webbrowser -import argparse -from pathlib import Path -from openpilot.common.basedir import BASEDIR - -DIFF_OUT_DIR = Path(BASEDIR) / "selfdrive" / "ui" / "tests" / "diff" / "report" - - -def extract_frames(video_path, output_dir): - output_pattern = str(output_dir / "frame_%04d.png") - cmd = ['ffmpeg', '-i', video_path, '-vsync', '0', output_pattern, '-y'] - subprocess.run(cmd, capture_output=True, check=True) - frames = sorted(output_dir.glob("frame_*.png")) - return frames - - -def compare_frames(frame1_path, frame2_path): - result = subprocess.run(['cmp', '-s', frame1_path, frame2_path]) - return result.returncode == 0 - - -def frame_to_data_url(frame_path): - with open(frame_path, 'rb') as f: - data = f.read() - return f"data:image/png;base64,{base64.b64encode(data).decode()}" - - -def create_diff_video(video1, video2, output_path): - """Create a diff video using ffmpeg blend filter with difference mode.""" - print("Creating diff video...") - cmd = ['ffmpeg', '-i', video1, '-i', video2, '-filter_complex', '[0:v]blend=all_mode=difference', '-vsync', '0', '-y', output_path] - subprocess.run(cmd, capture_output=True, check=True) - - -def find_differences(video1, video2): - with tempfile.TemporaryDirectory() as tmpdir: - tmpdir = Path(tmpdir) - - print(f"Extracting frames from {video1}...") - frames1_dir = tmpdir / "frames1" - frames1_dir.mkdir() - frames1 = extract_frames(video1, frames1_dir) - - print(f"Extracting frames from {video2}...") - frames2_dir = tmpdir / "frames2" - frames2_dir.mkdir() - frames2 = extract_frames(video2, frames2_dir) - - if len(frames1) != len(frames2): - print(f"WARNING: Frame count mismatch: {len(frames1)} vs {len(frames2)}") - min_frames = min(len(frames1), len(frames2)) - frames1 = frames1[:min_frames] - frames2 = frames2[:min_frames] - - print(f"Comparing {len(frames1)} frames...") - different_frames = [] - frame_data = [] - - for i, (f1, f2) in enumerate(zip(frames1, frames2, strict=False)): - is_different = not compare_frames(f1, f2) - if is_different: - different_frames.append(i) - - if i < 10 or i >= len(frames1) - 10 or is_different: - frame_data.append({'index': i, 'different': is_different, 'frame1_url': frame_to_data_url(f1), 'frame2_url': frame_to_data_url(f2)}) - - return different_frames, frame_data, len(frames1) - - -def generate_html_report(video1, video2, basedir, different_frames, frame_data, total_frames): - chunks = [] - if different_frames: - current_chunk = [different_frames[0]] - for i in range(1, len(different_frames)): - if different_frames[i] == different_frames[i - 1] + 1: - current_chunk.append(different_frames[i]) - else: - chunks.append(current_chunk) - current_chunk = [different_frames[i]] - chunks.append(current_chunk) - - result_text = ( - f"✅ Videos are identical! ({total_frames} frames)" - if len(different_frames) == 0 - else f"❌ Found {len(different_frames)} different frames out of {total_frames} total ({(len(different_frames) / total_frames * 100):.1f}%)" - ) - - html = f"""

    UI Diff

    - - - - - - -
    -

    Video 1

    - -    		 -

    Video 2

    - -    		 -

    Pixel Diff

    - -
    - -
    -

    Results: {result_text}

    -""" - return html - - -def main(): - parser = argparse.ArgumentParser(description='Compare two videos and generate HTML diff report') - parser.add_argument('video1', help='First video file') - parser.add_argument('video2', help='Second video file') - parser.add_argument('output', nargs='?', default='diff.html', help='Output HTML file (default: diff.html)') - parser.add_argument("--basedir", type=str, help="Base directory for output", default="") - parser.add_argument('--no-open', action='store_true', help='Do not open HTML report in browser') - - args = parser.parse_args() - - os.makedirs(DIFF_OUT_DIR, exist_ok=True) - - print("=" * 60) - print("VIDEO DIFF - HTML REPORT") - print("=" * 60) - print(f"Video 1: {args.video1}") - print(f"Video 2: {args.video2}") - print(f"Output: {args.output}") - print() - - # Create diff video - diff_video_path = os.path.join(os.path.dirname(args.output), DIFF_OUT_DIR / "diff.mp4") - create_diff_video(args.video1, args.video2, diff_video_path) - - different_frames, frame_data, total_frames = find_differences(args.video1, args.video2) - - if different_frames is None: - sys.exit(1) - - print() - print("Generating HTML report...") - html = generate_html_report(args.video1, args.video2, args.basedir, different_frames, frame_data, total_frames) - - with open(DIFF_OUT_DIR / args.output, 'w') as f: - f.write(html) - - # Open in browser by default - if not args.no_open: - print(f"Opening {args.output} in browser...") - webbrowser.open(f'file://{os.path.abspath(DIFF_OUT_DIR / args.output)}') - - return 0 if len(different_frames) == 0 else 1 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/selfdrive/ui/tests/diff/replay.py b/selfdrive/ui/tests/diff/replay.py deleted file mode 100755 index 9da157660e6cf0..00000000000000 --- a/selfdrive/ui/tests/diff/replay.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import coverage -import pyray as rl -from dataclasses import dataclass -from openpilot.selfdrive.ui.tests.diff.diff import DIFF_OUT_DIR - -os.environ["RECORD"] = "1" -if "RECORD_OUTPUT" not in os.environ: - os.environ["RECORD_OUTPUT"] = "mici_ui_replay.mp4" - -os.environ["RECORD_OUTPUT"] = os.path.join(DIFF_OUT_DIR, os.environ["RECORD_OUTPUT"]) - -from openpilot.common.params import Params -from openpilot.system.version import terms_version, training_version -from openpilot.system.ui.lib.application import gui_app, MousePos, MouseEvent -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout - -FPS = 60 -HEADLESS = os.getenv("WINDOWED", "0") == "1" - - -@dataclass -class DummyEvent: - click: bool = False - # TODO: add some kind of intensity - swipe_left: bool = False - swipe_right: bool = False - swipe_down: bool = False - - -SCRIPT = [ - (0, DummyEvent()), - (FPS * 1, DummyEvent(click=True)), - (FPS * 2, DummyEvent(click=True)), - (FPS * 3, DummyEvent()), -] - - -def setup_state(): - params = Params() - params.put("HasAcceptedTerms", terms_version) - params.put("CompletedTrainingVersion", training_version) - params.put("DongleId", "test123456789") - params.put("UpdaterCurrentDescription", "0.10.1 / test-branch / abc1234 / Nov 30") - return None - - -def inject_click(coords): - events = [] - x, y = coords[0] - events.append(MouseEvent(pos=MousePos(x, y), slot=0, left_pressed=True, left_released=False, left_down=False, t=time.monotonic())) - for x, y in coords[1:]: - events.append(MouseEvent(pos=MousePos(x, y), slot=0, left_pressed=False, left_released=False, left_down=True, t=time.monotonic())) - x, y = coords[-1] - events.append(MouseEvent(pos=MousePos(x, y), slot=0, left_pressed=False, left_released=True, left_down=False, t=time.monotonic())) - - with gui_app._mouse._lock: - gui_app._mouse._events.extend(events) - - -def handle_event(event: DummyEvent): - if event.click: - inject_click([(gui_app.width // 2, gui_app.height // 2)]) - if event.swipe_left: - inject_click([(gui_app.width * 3 // 4, gui_app.height // 2), - (gui_app.width // 4, gui_app.height // 2), - (0, gui_app.height // 2)]) - if event.swipe_right: - inject_click([(gui_app.width // 4, gui_app.height // 2), - (gui_app.width * 3 // 4, gui_app.height // 2), - (gui_app.width, gui_app.height // 2)]) - if event.swipe_down: - inject_click([(gui_app.width // 2, gui_app.height // 4), - (gui_app.width // 2, gui_app.height * 3 // 4), - (gui_app.width // 2, gui_app.height)]) - - -def run_replay(): - setup_state() - os.makedirs(DIFF_OUT_DIR, exist_ok=True) - - if not HEADLESS: - rl.set_config_flags(rl.FLAG_WINDOW_HIDDEN) - gui_app.init_window("ui diff test", fps=FPS) - main_layout = MiciMainLayout() - main_layout.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - - frame = 0 - script_index = 0 - - for should_render in gui_app.render(): - while script_index < len(SCRIPT) and SCRIPT[script_index][0] == frame: - _, event = SCRIPT[script_index] - handle_event(event) - script_index += 1 - - ui_state.update() - - if should_render: - main_layout.render() - - frame += 1 - - if script_index >= len(SCRIPT): - break - - gui_app.close() - - print(f"Total frames: {frame}") - print(f"Video saved to: {os.environ['RECORD_OUTPUT']}") - - -def main(): - cov = coverage.coverage(source=['openpilot.selfdrive.ui.mici']) - with cov.collect(): - run_replay() - cov.stop() - cov.save() - cov.report() - cov.html_report(directory=os.path.join(DIFF_OUT_DIR, 'htmlcov')) - print("HTML report: htmlcov/index.html") - - -if __name__ == "__main__": - main() diff --git a/selfdrive/ui/tests/playsound.cc b/selfdrive/ui/tests/playsound.cc new file mode 100644 index 00000000000000..6487d0479038c2 --- /dev/null +++ b/selfdrive/ui/tests/playsound.cc @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +int main(int argc, char **argv) { + + QApplication a(argc, argv); + + QTimer::singleShot(0, [=]{ + QSoundEffect s; + const char *vol = getenv("VOLUME"); + s.setVolume(vol ? atof(vol) : 1.0); + for (int i = 1; i < argc; i++) { + QString fn = argv[i]; + qDebug() << "playing" << fn; + + QEventLoop loop; + s.setSource(QUrl::fromLocalFile(fn)); + QEventLoop::connect(&s, &QSoundEffect::loadedChanged, &loop, &QEventLoop::quit); + loop.exec(); + s.play(); + QEventLoop::connect(&s, &QSoundEffect::playingChanged, &loop, &QEventLoop::quit); + loop.exec(); + } + QCoreApplication::exit(); + }); + + return a.exec(); +} diff --git a/selfdrive/ui/tests/profile_onroad.py b/selfdrive/ui/tests/profile_onroad.py deleted file mode 100755 index fde4f25ffed40d..00000000000000 --- a/selfdrive/ui/tests/profile_onroad.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import cProfile -import pyray as rl -import numpy as np - -from msgq.visionipc import VisionIpcServer, VisionStreamType -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout -from openpilot.system.ui.lib.application import gui_app -from openpilot.tools.lib.logreader import LogReader - -FPS = 60 - - -def chunk_messages_by_time(messages): - dt_ns = 1e9 / FPS - chunks = [] - current_services = {} - next_time = messages[0].logMonoTime + dt_ns if messages else 0 - - for msg in messages: - if msg.logMonoTime >= next_time: - chunks.append(current_services) - current_services = {} - next_time += dt_ns * ((msg.logMonoTime - next_time) // dt_ns + 1) - current_services[msg.which()] = msg - - if current_services: - chunks.append(current_services) - return chunks - - -def patch_submaster(message_chunks): - def mock_update(timeout=None): - sm = ui_state.sm - sm.updated = dict.fromkeys(sm.services, False) - current_time = time.monotonic() - for service, msg in message_chunks[sm.frame].items(): - if service in sm.data: - sm.seen[service] = True - sm.updated[service] = True - - msg_builder = msg.as_builder() - sm.data[service] = getattr(msg_builder, service) - sm.logMonoTime[service] = msg.logMonoTime - sm.recv_time[service] = current_time - sm.recv_frame[service] = sm.frame - sm.valid[service] = True - sm.frame += 1 - ui_state.sm.update = mock_update - - -if __name__ == "__main__": - import argparse - parser = argparse.ArgumentParser(description='Profile openpilot UI rendering and state updates') - parser.add_argument('route', type=str, nargs='?', default="302bab07c1511180/00000006--0b9a7005f1/3", - help='Route to use for profiling') - parser.add_argument('--loop', type=int, default=1, - help='Number of times to loop the log (default: 1)') - parser.add_argument('--output', type=str, default='cachegrind.out.ui', - help='Output file prefix (default: cachegrind.out.ui)') - parser.add_argument('--max-seconds', type=float, default=None, - help='Maximum seconds of messages to process (default: all)') - parser.add_argument('--headless', action='store_true', - help='Run in headless mode without GPU (for CI/testing)') - args = parser.parse_args() - - print(f"Loading log from {args.route}...") - lr = LogReader(args.route, sort_by_time=True) - messages = list(lr) * args.loop - - print("Chunking messages...") - message_chunks = chunk_messages_by_time(messages) - if args.max_seconds: - message_chunks = message_chunks[:int(args.max_seconds * FPS)] - - print("Initializing UI with GPU rendering...") - - if args.headless: - os.environ['SDL_VIDEODRIVER'] = 'dummy' - - gui_app.init_window("UI Profiling", fps=600) - main_layout = MiciMainLayout() - main_layout.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - - print("Running...") - patch_submaster(message_chunks) - - W, H = 2048, 1216 - vipc = VisionIpcServer("camerad") - vipc.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 5, W, H) - vipc.start_listener() - yuv_buffer_size = W * H + (W // 2) * (H // 2) * 2 - yuv_data = np.random.randint(0, 256, yuv_buffer_size, dtype=np.uint8).tobytes() - with cProfile.Profile() as pr: - for should_render in gui_app.render(): - if ui_state.sm.frame >= len(message_chunks): - break - if ui_state.sm.frame % 3 == 0: - eof = int((ui_state.sm.frame % 3) * 0.05 * 1e9) - vipc.send(VisionStreamType.VISION_STREAM_ROAD, yuv_data, ui_state.sm.frame % 3, eof, eof) - ui_state.update() - if should_render: - main_layout.render() - pr.dump_stats(f'{args.output}_deterministic.stats') - - rl.close_window() - print("\nProfiling complete!") - print(f" run: python -m pstats {args.output}_deterministic.stats") diff --git a/selfdrive/ui/tests/test_feedbackd.py b/selfdrive/ui/tests/test_feedbackd.py deleted file mode 100644 index 6b7ec448632ded..00000000000000 --- a/selfdrive/ui/tests/test_feedbackd.py +++ /dev/null @@ -1,53 +0,0 @@ -import pytest -import cereal.messaging as messaging -from cereal import car -from openpilot.common.params import Params -from openpilot.system.manager.process_config import managed_processes - - -@pytest.mark.skip("tmp disabled") -class TestFeedbackd: - def setup_method(self): - self.pm = messaging.PubMaster(['carState', 'rawAudioData']) - self.sm = messaging.SubMaster(['audioFeedback']) - - def _send_lkas_button(self, pressed: bool): - msg = messaging.new_message('carState') - msg.carState.canValid = True - msg.carState.buttonEvents = [{'type': car.CarState.ButtonEvent.Type.lkas, 'pressed': pressed}] - self.pm.send('carState', msg) - - def _send_audio_data(self, count: int = 5): - for _ in range(count): - audio_msg = messaging.new_message('rawAudioData') - audio_msg.rawAudioData.data = bytes(1600) # 800 samples of int16 - audio_msg.rawAudioData.sampleRate = 16000 - self.pm.send('rawAudioData', audio_msg) - self.sm.update(timeout=100) - - @pytest.mark.parametrize("record_feedback", [False, True]) - def test_audio_feedback(self, record_feedback): - Params().put_bool("RecordAudioFeedback", record_feedback) - - managed_processes["feedbackd"].start() - assert self.pm.wait_for_readers_to_update('carState', timeout=5) - assert self.pm.wait_for_readers_to_update('rawAudioData', timeout=5) - - self._send_lkas_button(pressed=True) - self._send_audio_data() - self._send_lkas_button(pressed=False) - self._send_audio_data() - - if record_feedback: - assert self.sm.updated['audioFeedback'], "audioFeedback should be published when enabled" - else: - assert not self.sm.updated['audioFeedback'], "audioFeedback should not be published when disabled" - - self._send_lkas_button(pressed=True) - self._send_audio_data() - self._send_lkas_button(pressed=False) - self._send_audio_data() - - assert not self.sm.updated['audioFeedback'], "audioFeedback should not be published after second press" - - managed_processes["feedbackd"].stop() diff --git a/selfdrive/ui/tests/test_raylib_ui.py b/selfdrive/ui/tests/test_raylib_ui.py deleted file mode 100644 index 69ba946dcd9ae3..00000000000000 --- a/selfdrive/ui/tests/test_raylib_ui.py +++ /dev/null @@ -1,8 +0,0 @@ -import time -from openpilot.selfdrive.test.helpers import with_processes - - -@with_processes(["ui"]) -def test_raylib_ui(): - """Test initialization of the UI widgets is successful.""" - time.sleep(1) diff --git a/selfdrive/ui/tests/test_runner.cc b/selfdrive/ui/tests/test_runner.cc new file mode 100644 index 00000000000000..ac63139d178a89 --- /dev/null +++ b/selfdrive/ui/tests/test_runner.cc @@ -0,0 +1,25 @@ +#define CATCH_CONFIG_RUNNER +#include "catch2/catch.hpp" + +#include +#include +#include +#include + +int main(int argc, char **argv) { + // unit tests for Qt + QApplication app(argc, argv); + + QString language_file = "main_test_en"; + qDebug() << "Loading language:" << language_file; + + QTranslator translator; + QString translationsPath = QDir::cleanPath(qApp->applicationDirPath() + "/../translations"); + if (!translator.load(language_file, translationsPath)) { + qDebug() << "Failed to load translation file!"; + } + app.installTranslator(&translator); + + const int res = Catch::Session().run(argc, argv); + return (res < 0xff ? res : 0xff); +} diff --git a/selfdrive/ui/tests/test_sound.cc b/selfdrive/ui/tests/test_sound.cc new file mode 100644 index 00000000000000..43599f3828a6d0 --- /dev/null +++ b/selfdrive/ui/tests/test_sound.cc @@ -0,0 +1,75 @@ +#include +#include +#include + +#include "catch2/catch.hpp" +#include "selfdrive/ui/soundd/sound.h" + +class TestSound : public Sound { +public: + TestSound() : Sound() { + for (auto i = sounds.constBegin(); i != sounds.constEnd(); ++i) { + sound_stats[i.key()] = {0, 0}; + QObject::connect(i.value().first, &QSoundEffect::playingChanged, [=, s = i.value().first, a = i.key()]() { + if (s->isPlaying()) { + sound_stats[a].first++; + } else { + sound_stats[a].second++; + } + }); + } + } + + QMap> sound_stats; +}; + +void controls_thread(int loop_cnt) { + PubMaster pm({"controlsState", "deviceState"}); + MessageBuilder deviceStateMsg; + auto deviceState = deviceStateMsg.initEvent().initDeviceState(); + deviceState.setStarted(true); + + const int DT_CTRL = 10; // ms + for (int i = 0; i < loop_cnt; ++i) { + for (auto &[alert, fn, loops] : sound_list) { + printf("testing %s\n", qPrintable(fn)); + for (int j = 0; j < 1000 / DT_CTRL; ++j) { + MessageBuilder msg; + auto cs = msg.initEvent().initControlsState(); + cs.setAlertSound(alert); + cs.setAlertType(fn.toStdString()); + pm.send("controlsState", msg); + pm.send("deviceState", deviceStateMsg); + QThread::msleep(DT_CTRL); + } + } + } + + // send no alert sound + for (int j = 0; j < 1000 / DT_CTRL; ++j) { + MessageBuilder msg; + msg.initEvent().initControlsState(); + pm.send("controlsState", msg); + QThread::msleep(DT_CTRL); + } + + QThread::currentThread()->quit(); +} + +TEST_CASE("test soundd") { + QEventLoop loop; + TestSound test_sound; + const int test_loop_cnt = 2; + + QThread t; + QObject::connect(&t, &QThread::started, [=]() { controls_thread(test_loop_cnt); }); + QObject::connect(&t, &QThread::finished, [&]() { loop.quit(); }); + t.start(); + loop.exec(); + + for (const AudibleAlert alert : test_sound.sound_stats.keys()) { + auto [play, stop] = test_sound.sound_stats[alert]; + REQUIRE(play == test_loop_cnt); + REQUIRE(stop == test_loop_cnt); + } +} diff --git a/selfdrive/ui/tests/test_sound_stability.py b/selfdrive/ui/tests/test_sound_stability.py new file mode 100755 index 00000000000000..f0d51ec960dea1 --- /dev/null +++ b/selfdrive/ui/tests/test_sound_stability.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import os +import random +import subprocess +import time +from pathlib import Path +from common.basedir import BASEDIR + +os.environ["LD_LIBRARY_PATH"] = "" + +# pull this from the provisioning tests +play_sound = os.path.join(BASEDIR, "selfdrive/ui/test/play_sound") +waste = os.path.join(BASEDIR, "scripts/waste") +sound_path = Path(os.path.join(BASEDIR, "selfdrive/assets/sounds")) + +def sound_test(): + + # max volume + vol = 15 + sound_files = [p.absolute() for p in sound_path.iterdir() if str(p).endswith(".wav")] + + # start waste + p = subprocess.Popen([waste], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + + start_time = time.monotonic() + frame = 0 + while True: + # start a few processes + procs = [] + for _ in range(random.randint(5, 20)): + sound = random.choice(sound_files) + p = subprocess.Popen([play_sound, str(sound), str(vol)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + procs.append(p) + time.sleep(random.uniform(0, 0.75)) + + # and kill them + time.sleep(random.uniform(0, 5)) + for p in procs: + p.terminate() + + # write stats + stats = f"running time {time.monotonic() - start_time}s, cycle {frame}" + with open("/tmp/sound_stats.txt", "a") as f: + f.write(stats) + print(stats) + frame +=1 + +if __name__ == "__main__": + sound_test() diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py old mode 100644 new mode 100755 index a9da8455ebdd24..8cc9215b74a8ec --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -1,35 +1,75 @@ -from cereal import car -from cereal import messaging -from cereal.messaging import SubMaster, PubMaster -from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert - +#!/usr/bin/env python3 +import subprocess import time +import unittest + +from cereal import log, car +import cereal.messaging as messaging +from selfdrive.test.helpers import phone_only, with_processes +# TODO: rewrite for unittest +from common.realtime import DT_CTRL +from system.hardware import HARDWARE AudibleAlert = car.CarControl.HUDControl.AudibleAlert +SOUNDS = { + # sound: total writes + AudibleAlert.none: 0, + AudibleAlert.engage: 184, + AudibleAlert.disengage: 186, + AudibleAlert.refuse: 194, + AudibleAlert.prompt: 184, + AudibleAlert.promptRepeat: 487, + AudibleAlert.promptDistracted: 508, + AudibleAlert.warningSoft: 471, + AudibleAlert.warningImmediate: 470, +} -class TestSoundd: - def test_check_selfdrive_timeout_alert(self): - sm = SubMaster(['selfdriveState']) - pm = PubMaster(['selfdriveState']) +def get_total_writes(): + audio_flinger = subprocess.check_output('dumpsys media.audio_flinger', shell=True, encoding='utf-8').strip() + write_lines = [l for l in audio_flinger.split('\n') if l.strip().startswith('Total writes')] + return sum(int(l.split(':')[1]) for l in write_lines) - for _ in range(100): - cs = messaging.new_message('selfdriveState') - cs.selfdriveState.enabled = True +class TestSoundd(unittest.TestCase): + def test_sound_card_init(self): + assert HARDWARE.get_sound_card_online() - pm.send("selfdriveState", cs) + @phone_only + @with_processes(['soundd']) + def test_alert_sounds(self): + pm = messaging.PubMaster(['deviceState', 'controlsState']) - time.sleep(0.01) + # make sure they're all defined + alert_sounds = {v: k for k, v in car.CarControl.HUDControl.AudibleAlert.schema.enumerants.items()} + diff = set(SOUNDS.keys()).symmetric_difference(alert_sounds.keys()) + assert len(diff) == 0, f"not all sounds defined in test: {diff}" - sm.update(0) + # wait for procs to init + time.sleep(1) - assert not check_selfdrive_timeout_alert(sm) + for sound, expected_writes in SOUNDS.items(): + print(f"testing {alert_sounds[sound]}") + start_writes = get_total_writes() - for _ in range(SELFDRIVE_STATE_TIMEOUT * 110): - sm.update(0) - time.sleep(0.01) + for i in range(int(10 / DT_CTRL)): + msg = messaging.new_message('deviceState') + msg.deviceState.started = True + pm.send('deviceState', msg) - assert check_selfdrive_timeout_alert(sm) + msg = messaging.new_message('controlsState') + if i < int(6 / DT_CTRL): + msg.controlsState.alertSound = sound + msg.controlsState.alertType = str(sound) + msg.controlsState.alertText1 = "Testing Sounds" + msg.controlsState.alertText2 = f"playing {alert_sounds[sound]}" + msg.controlsState.alertSize = log.ControlsState.AlertSize.mid + pm.send('controlsState', msg) + time.sleep(DT_CTRL) - # TODO: add test with micd for checking that soundd actually outputs sounds + tolerance = expected_writes / 8 + actual_writes = get_total_writes() - start_writes + print(f" expected {expected_writes} writes, got {actual_writes}") + assert abs(expected_writes - actual_writes) <= tolerance, f"{alert_sounds[sound]}: expected {expected_writes} writes, got {actual_writes}" +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/ui/tests/test_translations.cc b/selfdrive/ui/tests/test_translations.cc new file mode 100644 index 00000000000000..fcefc5784fee7a --- /dev/null +++ b/selfdrive/ui/tests/test_translations.cc @@ -0,0 +1,48 @@ +#include "catch2/catch.hpp" + +#include "common/params.h" +#include "selfdrive/ui/qt/window.h" + +const QString TEST_TEXT = "(WRAPPED_SOURCE_TEXT)"; // what each string should be translated to +QRegExp RE_NUM("\\d*"); + +QStringList getParentWidgets(QWidget* widget){ + QStringList parentWidgets; + while (widget->parentWidget() != Q_NULLPTR) { + widget = widget->parentWidget(); + parentWidgets.append(widget->metaObject()->className()); + } + return parentWidgets; +} + +template +void checkWidgetTrWrap(MainWindow &w) { + for (auto widget : w.findChildren()) { + const QString text = widget->text(); + bool isNumber = RE_NUM.exactMatch(text); + bool wrapped = text.contains(TEST_TEXT); + QString parentWidgets = getParentWidgets(widget).join("->"); + + if (!text.isEmpty() && !isNumber && !wrapped) { + FAIL(("\"" + text + "\" must be wrapped. Parent widgets: " + parentWidgets).toStdString()); + } + + // warn if source string wrapped, but UI adds text + // TODO: add way to ignore this + if (wrapped && text != TEST_TEXT) { + WARN(("\"" + text + "\" is dynamic and needs a custom retranslate function. Parent widgets: " + parentWidgets).toStdString()); + } + } +} + +// Tests all strings in the UI are wrapped with tr() +TEST_CASE("UI: test all strings wrapped") { + Params().remove("LanguageSetting"); + Params().remove("HardwareSerial"); + Params().remove("DongleId"); + qputenv("TICI", "1"); + + MainWindow w; + checkWidgetTrWrap(w); + checkWidgetTrWrap(w); +} diff --git a/selfdrive/ui/tests/test_translations.py b/selfdrive/ui/tests/test_translations.py old mode 100644 new mode 100755 index 3177814f9f068e..d8609f110b999c --- a/selfdrive/ui/tests/test_translations.py +++ b/selfdrive/ui/tests/test_translations.py @@ -1,124 +1,98 @@ -import pytest +#!/usr/bin/env python3 import json import os import re +import shutil +import unittest import xml.etree.ElementTree as ET -import string -import requests -from parameterized import parameterized_class -from openpilot.system.ui.lib.multilang import TRANSLATIONS_DIR, LANGUAGES_FILE -with open(str(LANGUAGES_FILE)) as f: - translation_files = json.load(f) +from selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations -UNFINISHED_TRANSLATION_TAG = "" not in cur_translations, + f"{file} ({name}) translation file has unfinished translations. Finish translations or mark them as completed in Qt Linguist") def test_vanished_translations(self): - cur_translations = self._read_translation_file(TRANSLATIONS_DIR, self.file) - assert "" not in cur_translations, \ - f"{self.file} ({self.name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them" + for name, file in self.translation_files.items(): + with self.subTest(name=name, file=file): + cur_translations = self._read_translation_file(TRANSLATIONS_DIR, file) + self.assertTrue("" not in cur_translations, + f"{file} ({name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them") - def test_finished_translations(self): + def test_plural_translations(self): """ - Tests ran on each translation marked "finished" - Plural: - - that any numerus (plural) translations have all plural forms non-empty + Tests: + - that any numerus (plural) translations marked "finished" have all plural forms non-empty - that the correct format specifier is used (%n) - Non-plural: - - that translation is not empty - - that translation format arguments are consistent """ - tr_xml = ET.parse(os.path.join(TRANSLATIONS_DIR, f"{self.file}.ts")) - - for context in tr_xml.getroot(): - for message in context.iterfind("message"): - translation = message.find("translation") - source_text = message.find("source").text - - # Do not test unfinished translations - if translation.get("type") == "unfinished": - continue - - if message.get("numerus") == "yes": - numerusform = [t.text for t in translation.findall("numerusform")] - - for nf in numerusform: - assert nf is not None, f"Ensure all plural translation forms are completed: {source_text}" - assert "%n" in nf, "Ensure numerus argument (%n) exists in translation." - assert FORMAT_ARG.search(nf) is None, f"Plural translations must use %n, not %1, %2, etc.: {numerusform}" - - else: - assert translation.text is not None, f"Ensure translation is completed: {source_text}" - - source_args = FORMAT_ARG.findall(source_text) - translation_args = FORMAT_ARG.findall(translation.text) - assert sorted(source_args) == sorted(translation_args), \ - f"Ensure format arguments are consistent: `{source_text}` vs. `{translation.text}`" - - def test_no_locations(self): - for line in self._read_translation_file(TRANSLATIONS_DIR, self.file).splitlines(): - assert not line.strip().startswith(LOCATION_TAG), \ - f"Line contains location tag: {line.strip()}, remove all line numbers." - - def test_entities_error(self): - cur_translations = self._read_translation_file(TRANSLATIONS_DIR, self.file) - matches = re.findall(r'@(\w+);', cur_translations) - assert len(matches) == 0, f"The string(s) {matches} were found with '@' instead of '&'" - - def test_bad_language(self): - IGNORED_WORDS = {'pédale'} - - match = re.search(r'([a-zA-Z]{2,3})', self.file) - assert match, f"{self.name} - could not parse language" - - try: - response = requests.get( - f"https://raw.githubusercontent.com/LDNOOBW/List-of-Dirty-Naughty-Obscene-and-Otherwise-Bad-Words/master/{match.group(1)}" - ) - response.raise_for_status() - except requests.exceptions.HTTPError as e: - if e.response is not None and e.response.status_code == 429: - pytest.skip("word list rate limited") - raise - - banned_words = {line.strip() for line in response.text.splitlines()} - - for context in ET.parse(os.path.join(TRANSLATIONS_DIR, f"{self.file}.ts")).getroot(): - for message in context.iterfind("message"): - translation = message.find("translation") - if translation.get("type") == "unfinished": - continue - - translation_text = " ".join([t.text for t in translation.findall("numerusform")]) if message.get("numerus") == "yes" else translation.text - - if not translation_text: - continue - - words = set(translation_text.translate(str.maketrans('', '', string.punctuation + '%n')).lower().split()) - bad_words_found = words & (banned_words - IGNORED_WORDS) - assert not bad_words_found, f"Bad language found in {self.name}: '{translation_text}'. Bad word(s): {', '.join(bad_words_found)}" + for name, file in self.translation_files.items(): + with self.subTest(name=name, file=file): + tr_xml = ET.parse(os.path.join(TRANSLATIONS_DIR, f"{file}.ts")) + + for context in tr_xml.getroot(): + for message in context.iterfind("message"): + if message.get("numerus") == "yes": + translation = message.find("translation") + numerusform = [t.text for t in translation.findall("numerusform")] + + # Do not assert finished translations + if translation.get("type") == "unfinished": + continue + + self.assertNotIn(None, numerusform, "Ensure all plural translation forms are completed.") + self.assertTrue(all([re.search("%[0-9]+", t) is None for t in numerusform]), + "Plural translations must use %n, not %1, %2, etc.: {}".format(numerusform)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/ui/tests/test_ui/print_mouse_coords.py b/selfdrive/ui/tests/test_ui/print_mouse_coords.py deleted file mode 100755 index 1e88ce57d3e4d7..00000000000000 --- a/selfdrive/ui/tests/test_ui/print_mouse_coords.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env python3 -""" -Simple script to print mouse coordinates on Ubuntu. -Run with: python print_mouse_coords.py -Press Ctrl+C to exit. -""" - -from pynput import mouse - -print("Mouse coordinate printer - Press Ctrl+C to exit") -print("Click to set the top left origin") - -origin: tuple[int, int] | None = None -clicks: list[tuple[int, int]] = [] - - -def on_click(x, y, button, pressed): - global origin, clicks - if pressed: # Only on mouse down, not up - if origin is None: - origin = (x, y) - print(f"Origin set to: {x},{y}") - else: - rel_x = x - origin[0] - rel_y = y - origin[1] - clicks.append((rel_x, rel_y)) - print(f"Clicks: {clicks}") - - -if __name__ == "__main__": - try: - # Start mouse listener - with mouse.Listener(on_click=on_click) as listener: - listener.join() - except KeyboardInterrupt: - print("\nExiting...") diff --git a/selfdrive/ui/tests/test_ui/raylib_screenshots.py b/selfdrive/ui/tests/test_ui/raylib_screenshots.py deleted file mode 100755 index 481ac111beb9f5..00000000000000 --- a/selfdrive/ui/tests/test_ui/raylib_screenshots.py +++ /dev/null @@ -1,317 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import shutil -import time -import pathlib -from collections import namedtuple - -import pyautogui -import pywinctl - -from cereal import car, log -from cereal import messaging -from cereal.messaging import PubMaster -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.test.helpers import with_processes -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.updated.updated import parse_release_notes -from openpilot.system.version import terms_version, training_version - -AlertSize = log.SelfdriveState.AlertSize -AlertStatus = log.SelfdriveState.AlertStatus - -TEST_DIR = pathlib.Path(__file__).parent -TEST_OUTPUT_DIR = TEST_DIR / "raylib_report" -SCREENSHOTS_DIR = TEST_OUTPUT_DIR / "screenshots" -UI_DELAY = 0.5 - -BRANCH_NAME = "this-is-a-really-super-mega-ultra-max-extreme-ultimate-long-branch-name" -VERSION = f"0.10.1 / {BRANCH_NAME} / 7864838 / Oct 03" - -# Offroad alerts to test -OFFROAD_ALERTS = ['Offroad_IsTakingSnapshot'] - - -def put_update_params(params: Params): - params.put("UpdaterCurrentReleaseNotes", parse_release_notes(BASEDIR)) - params.put("UpdaterNewReleaseNotes", parse_release_notes(BASEDIR)) - params.put("UpdaterTargetBranch", BRANCH_NAME) - - -def setup_homescreen(click, pm: PubMaster): - pass - - -def setup_homescreen_update_available(click, pm: PubMaster): - params = Params() - params.put_bool("UpdateAvailable", True) - put_update_params(params) - setup_offroad_alert(click, pm) - - -def setup_settings(click, pm: PubMaster): - click(100, 100) - - -def close_settings(click, pm: PubMaster): - click(240, 216) - - -def setup_settings_network(click, pm: PubMaster): - setup_settings(click, pm) - click(278, 450) - - -def setup_settings_network_advanced(click, pm: PubMaster): - setup_settings_network(click, pm) - click(1880, 100) - - -def setup_settings_toggles(click, pm: PubMaster): - setup_settings(click, pm) - click(278, 600) - - -def setup_settings_software(click, pm: PubMaster): - put_update_params(Params()) - setup_settings(click, pm) - click(278, 720) - - -def setup_settings_software_download(click, pm: PubMaster): - params = Params() - # setup_settings_software but with "DOWNLOAD" button to test long text - params.put("UpdaterState", "idle") - params.put_bool("UpdaterFetchAvailable", True) - setup_settings_software(click, pm) - - -def setup_settings_software_release_notes(click, pm: PubMaster): - setup_settings_software(click, pm) - click(588, 110) # expand description for current version - - -def setup_settings_software_branch_switcher(click, pm: PubMaster): - setup_settings_software(click, pm) - params = Params() - params.put("UpdaterAvailableBranches", f"master,nightly,release,{BRANCH_NAME}") - params.put("GitBranch", BRANCH_NAME) # should be on top - params.put("UpdaterTargetBranch", "nightly") # should be selected - click(1984, 449) - - -def setup_settings_firehose(click, pm: PubMaster): - setup_settings(click, pm) - click(278, 845) - - -def setup_settings_developer(click, pm: PubMaster): - CP = car.CarParams() - CP.alphaLongitudinalAvailable = True # show alpha long control toggle - Params().put("CarParamsPersistent", CP.to_bytes()) - - setup_settings(click, pm) - click(278, 950) - - -def setup_keyboard(click, pm: PubMaster): - setup_settings_developer(click, pm) - click(1930, 470) - - -def setup_pair_device(click, pm: PubMaster): - click(1950, 800) - - -def setup_offroad_alert(click, pm: PubMaster): - put_update_params(Params()) - set_offroad_alert("Offroad_TemperatureTooHigh", True, extra_text='99C') - set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text='longitudinal') - for alert in OFFROAD_ALERTS: - set_offroad_alert(alert, True) - - setup_settings(click, pm) - close_settings(click, pm) - - -def setup_confirmation_dialog(click, pm: PubMaster): - setup_settings(click, pm) - click(1985, 791) # reset calibration - - -def setup_experimental_mode_description(click, pm: PubMaster): - setup_settings_toggles(click, pm) - click(1200, 280) # expand description for experimental mode - - -def setup_openpilot_long_confirmation_dialog(click, pm: PubMaster): - setup_settings_developer(click, pm) - click(2000, 960) # toggle openpilot longitudinal control - - -def setup_onroad(click, pm: PubMaster): - ds = messaging.new_message('deviceState') - ds.deviceState.started = True - - ps = messaging.new_message('pandaStates', 1) - ps.pandaStates[0].pandaType = log.PandaState.PandaType.dos - ps.pandaStates[0].ignitionLine = True - - driverState = messaging.new_message('driverStateV2') - driverState.driverStateV2.leftDriverData.faceOrientation = [0, 0, 0] - - for _ in range(5): - pm.send('deviceState', ds) - pm.send('pandaStates', ps) - pm.send('driverStateV2', driverState) - ds.clear_write_flag() - ps.clear_write_flag() - driverState.clear_write_flag() - time.sleep(0.05) - - -def setup_onroad_sidebar(click, pm: PubMaster): - setup_onroad(click, pm) - click(100, 100) # open sidebar - - -def setup_onroad_alert(click, pm: PubMaster, size: log.SelfdriveState.AlertSize, text1: str, text2: str, status: log.SelfdriveState.AlertStatus): - setup_onroad(click, pm) - alert = messaging.new_message('selfdriveState') - ss = alert.selfdriveState - ss.alertSize = size - ss.alertText1 = text1 - ss.alertText2 = text2 - ss.alertStatus = status - for _ in range(5): - pm.send('selfdriveState', alert) - alert.clear_write_flag() - time.sleep(0.05) - - -def setup_onroad_small_alert(click, pm: PubMaster): - setup_onroad_alert(click, pm, AlertSize.small, "Small Alert", "This is a small alert", AlertStatus.normal) - - -def setup_onroad_medium_alert(click, pm: PubMaster): - setup_onroad_alert(click, pm, AlertSize.mid, "Medium Alert", "This is a medium alert", AlertStatus.userPrompt) - - -def setup_onroad_full_alert(click, pm: PubMaster): - setup_onroad_alert(click, pm, AlertSize.full, "DISENGAGE IMMEDIATELY", "Driver Distracted", AlertStatus.critical) - - -def setup_onroad_full_alert_multiline(click, pm: PubMaster): - setup_onroad_alert(click, pm, AlertSize.full, "Reverse\nGear", "", AlertStatus.normal) - - -def setup_onroad_full_alert_long_text(click, pm: PubMaster): - setup_onroad_alert(click, pm, AlertSize.full, "TAKE CONTROL IMMEDIATELY", "Calibration Invalid: Remount Device & Recalibrate", AlertStatus.userPrompt) - - -CASES = { - "homescreen": setup_homescreen, - "homescreen_paired": setup_homescreen, - "homescreen_prime": setup_homescreen, - "homescreen_update_available": setup_homescreen_update_available, - "homescreen_unifont": setup_homescreen, - "settings_device": setup_settings, - "settings_network": setup_settings_network, - "settings_network_advanced": setup_settings_network_advanced, - "settings_toggles": setup_settings_toggles, - "settings_software": setup_settings_software, - "settings_software_download": setup_settings_software_download, - "settings_software_release_notes": setup_settings_software_release_notes, - "settings_software_branch_switcher": setup_settings_software_branch_switcher, - "settings_firehose": setup_settings_firehose, - "settings_developer": setup_settings_developer, - "keyboard": setup_keyboard, - "pair_device": setup_pair_device, - "offroad_alert": setup_offroad_alert, - "confirmation_dialog": setup_confirmation_dialog, - "experimental_mode_description": setup_experimental_mode_description, - "openpilot_long_confirmation_dialog": setup_openpilot_long_confirmation_dialog, - "onroad": setup_onroad, - "onroad_sidebar": setup_onroad_sidebar, - "onroad_small_alert": setup_onroad_small_alert, - "onroad_medium_alert": setup_onroad_medium_alert, - "onroad_full_alert": setup_onroad_full_alert, - "onroad_full_alert_multiline": setup_onroad_full_alert_multiline, - "onroad_full_alert_long_text": setup_onroad_full_alert_long_text, -} - - -class TestUI: - def __init__(self): - os.environ["SCALE"] = os.getenv("SCALE", "1") - os.environ["BIG"] = "1" - sys.modules["mouseinfo"] = False - - def setup(self): - # Seed minimal offroad state - self.pm = PubMaster(["deviceState", "pandaStates", "driverStateV2", "selfdriveState"]) - ds = messaging.new_message('deviceState') - ds.deviceState.networkType = log.DeviceState.NetworkType.wifi - for _ in range(5): - self.pm.send('deviceState', ds) - ds.clear_write_flag() - time.sleep(0.05) - time.sleep(0.5) - try: - self.ui = pywinctl.getWindowsWithTitle("UI")[0] - except Exception as e: - print(f"failed to find ui window, assuming that it's in the top left (for Xvfb) {e}") - self.ui = namedtuple("bb", ["left", "top", "width", "height"])(0, 0, 2160, 1080) - - def screenshot(self, name: str): - full_screenshot = pyautogui.screenshot() - cropped = full_screenshot.crop((self.ui.left, self.ui.top, self.ui.left + self.ui.width, self.ui.top + self.ui.height)) - cropped.save(SCREENSHOTS_DIR / f"{name}.png") - - def click(self, x: int, y: int, *args, **kwargs): - pyautogui.mouseDown(self.ui.left + x, self.ui.top + y, *args, **kwargs) - time.sleep(0.01) - pyautogui.mouseUp(self.ui.left + x, self.ui.top + y, *args, **kwargs) - - @with_processes(["ui"]) - def test_ui(self, name, setup_case): - self.setup() - time.sleep(UI_DELAY) # wait for UI to start - setup_case(self.click, self.pm) - self.screenshot(name) - - -def create_screenshots(): - if TEST_OUTPUT_DIR.exists(): - shutil.rmtree(TEST_OUTPUT_DIR) - SCREENSHOTS_DIR.mkdir(parents=True) - - t = TestUI() - for name, setup in CASES.items(): - with OpenpilotPrefix(): - params = Params() - params.put("DongleId", "123456789012345") - - # Set branch name - params.put("UpdaterCurrentDescription", VERSION) - params.put("UpdaterNewDescription", VERSION) - - # Set terms and training version (to skip onboarding) - params.put("HasAcceptedTerms", terms_version) - params.put("CompletedTrainingVersion", training_version) - - if name == "homescreen_paired": - params.put("PrimeType", 0) # NONE - elif name == "homescreen_prime": - params.put("PrimeType", 2) # LITE - elif name == "homescreen_unifont": - params.put("LanguageSetting", "zh-CHT") # Traditional Chinese - - t.test_ui(name, setup) - - -if __name__ == "__main__": - create_screenshots() diff --git a/selfdrive/ui/tests/test_ui/template.html b/selfdrive/ui/tests/test_ui/template.html deleted file mode 100644 index 68df5879e6692c..00000000000000 --- a/selfdrive/ui/tests/test_ui/template.html +++ /dev/null @@ -1,34 +0,0 @@ - - - - -{% for name, (image, ref_image) in cases.items() %} - -

    {{name}}

    -
    -
    - -
    -
    - -
    - -{% endfor %} - \ No newline at end of file diff --git a/selfdrive/ui/text b/selfdrive/ui/text new file mode 100755 index 00000000000000..2577e3006bf14f --- /dev/null +++ b/selfdrive/ui/text @@ -0,0 +1,8 @@ +#!/bin/sh + +if [ -f /TICI ] && [ ! -f qt/text ]; then + cp qt/text_larch64 qt/text +fi + +export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" +exec ./qt/text "$1" diff --git a/selfdrive/ui/translations/README.md b/selfdrive/ui/translations/README.md index 433eb7d64a009a..91a6c3ca2f40c4 100644 --- a/selfdrive/ui/translations/README.md +++ b/selfdrive/ui/translations/README.md @@ -1,3 +1,60 @@ # Multilanguage [![languages](https://raw.githubusercontent.com/commaai/openpilot/badges/translation_badge.svg)](#) + +## Contributing + +Before getting started, make sure you have set up the openpilot Ubuntu development environment by reading the [tools README.md](/tools/README.md). + +### Policy + +Most of the languages supported by openpilot come from and are maintained by the community via pull requests. A pull request likely to be merged is one that [fixes a translation or adds missing translations.](https://github.com/commaai/openpilot/blob/master/selfdrive/ui/translations/README.md#improving-an-existing-language) + +We also generally merge pull requests adding support for a new language if there are community members willing to maintain it. Maintaining a language is ensuring quality and completion of translations before each openpilot release. + +comma may remove or hide language support from releases depending on translation quality and completeness. + +### Adding a New Language + +openpilot provides a few tools to help contributors manage their translations and to ensure quality. To get started: + +1. Add your new language to [languages.json](/selfdrive/ui/translations/languages.json) with the appropriate [language code](https://en.wikipedia.org/wiki/List_of_ISO_639-1_codes) and the localized language name (Traditional Chinese is `中文（繁體）`). +2. Generate the XML translation file (`*.ts`): + ```shell + selfdrive/ui/update_translations.py + ``` +3. Edit the translation file, marking each translation as completed: + ```shell + linguist selfdrive/ui/translations/your_language_file.ts + ``` +4. View your finished translations by compiling and starting the UI, then find it in the language selector: + ```shell + scons -j$(nproc) selfdrive/ui && selfdrive/ui/ui + ``` + +### Improving an Existing Language + +Follow step 3. above, you can review existing translations and add missing ones. Once you're done, just open a pull request to openpilot. + +### Updating the UI + +Any time you edit source code in the UI, you need to update the translations to ensure the line numbers and contexts are up to date (first step above). + +### Testing + +openpilot has a few unit tests to make sure all translations are up-to-date and that all strings are wrapped in a translation marker. They are run in CI, but you can also run them locally. + +Tests translation files up to date: + +```shell +selfdrive/ui/tests/test_translations.py +``` + +Tests all static source strings are wrapped: + +```shell +selfdrive/ui/tests/create_test_translations.sh && selfdrive/ui/tests/test_translations +``` + +--- +![multilanguage_onroad](https://user-images.githubusercontent.com/25857203/178912800-2c798af8-78e3-498e-9e19-35906e0bafff.png) diff --git a/selfdrive/ui/translations/app.pot b/selfdrive/ui/translations/app.pot deleted file mode 100644 index abb6940a549b3a..00000000000000 --- a/selfdrive/ui/translations/app.pot +++ /dev/null @@ -1,1130 +0,0 @@ -# SOME DESCRIPTIVE TITLE. -# Copyright (C) YEAR THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# FIRST AUTHOR , YEAR. -# -#, fuzzy -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:51-0700\n" -"PO-Revision-Date: YEAR-MO-DA HO:MI+ZONE\n" -"Last-Translator: FULL NAME \n" -"Language-Team: LANGUAGE \n" -"Language: \n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=INTEGER; plural=EXPRESSION;\n" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "" - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "" - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "" - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "" - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "" diff --git a/selfdrive/ui/translations/app_ar.po b/selfdrive/ui/translations/app_ar.po deleted file mode 100644 index 608389fc07d907..00000000000000 --- a/selfdrive/ui/translations/app_ar.po +++ /dev/null @@ -1,1218 +0,0 @@ -# Arabic translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: ar\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=6; plural=n==0?0:n==1?1:n==2?2:(n%100>=3 && " -"n%100<=10)?3:(n%100>=11 && n%100<=99)?4:5;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " اكتملت معايرة استجابة عزم التوجيه." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " اكتملت معايرة استجابة عزم التوجيه بنسبة {}٪." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " جهازك موجه بمقدار {:.1f}° {} و {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "سنة واحدة من تخزين القيادة" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "اتصال LTE على مدار الساعة" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"تحذير: التحكم الطولي لـ openpilot في مرحلة ألفا لهذه السيارة وسيُعطّل نظام " -"الكبح التلقائي في حالات الطوارئ (AEB).

    في هذه السيارة، يعتمد " -"openpilot افتراضياً على نظام ACC المدمج بدلاً من التحكم الطولي لـ openpilot. " -"فعّل هذا الخيار للتبديل إلى التحكم الطولي لـ openpilot. يُنصح بتمكين وضع " -"التجربة عند تفعيل نسخة ألفا من التحكم الطولي. تغيير هذا الإعداد سيعيد تشغيل " -"openpilot إذا كانت السيارة قيد التشغيل." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    اكتملت معايرة تأخر التوجيه." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    اكتملت معايرة تأخر التوجيه بنسبة {}٪." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "نشط" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"يتيح ADB (Android Debug Bridge) الاتصال بجهازك عبر USB أو عبر الشبكة. راجع " -"https://docs.comma.ai/how-to/connect-to-comma لمزيد من المعلومات." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "إضافة" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "إعداد APN" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "تأكيد التشغيل المفرط" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "متقدم" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "عدواني" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "موافقة" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "مراقبة السائق دائماً" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"يمكن اختبار نسخة ألفا من التحكم الطولي لـ openpilot، مع وضع التجربة، على " -"الفروع غير الإصدارية." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "هل أنت متأكد أنك تريد إيقاف التشغيل؟" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "هل أنت متأكد أنك تريد إعادة التشغيل؟" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "هل أنت متأكد أنك تريد إعادة ضبط المعايرة؟" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "هل أنت متأكد أنك تريد إلغاء التثبيت؟" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "رجوع" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "انضم إلى comma prime عبر connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "ثبّت connect.comma.ai على شاشتك الرئيسية لاستخدامه كتطبيق" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "تغيير" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "تحقق" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "وضع الهدوء مُفعل" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECT" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "CONNECTING..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "إلغاء" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "خلوي بتعرفة محدودة" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "تغيير اللغة" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -"سيؤدي تغيير هذا الإعداد إلى إعادة تشغيل openpilot إذا كانت السيارة قيد " -"التشغيل." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "اضغط \"إضافة جهاز جديد\" ثم امسح رمز QR على اليمين" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "إغلاق" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "الإصدار الحالي" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "تنزيل" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "رفض" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "رفض، وإلغاء تثبيت openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "المطور" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "الجهاز" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "فصل عند الضغط على دواسة الوقود" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "افصل لإيقاف التشغيل" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "افصل لإعادة التشغيل" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "افصل لإعادة ضبط المعايرة" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "عرض السرعة بالكيلومتر/ساعة بدلاً من الميل/ساعة." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "معرّف الدونجل" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "تنزيل" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "كاميرا السائق" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "شخصية القيادة" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "تعديل" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "خطأ" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "وضع التجربة مُفعل" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "تمكين" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "تمكين ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "تمكين تحذيرات مغادرة المسار" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "تمكين التجوال" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "تمكين SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "تمكين الربط" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "تمكين مراقبة السائق حتى عندما لا يكون openpilot مُشغلاً." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "تمكين openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "فعّل تبديل التحكم الطولي (ألفا) لـ openpilot للسماح بوضع التجربة." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "أدخل APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "أدخل SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "أدخل كلمة مرور الربط الجديدة" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "أدخل كلمة المرور" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "أدخل اسم مستخدم GitHub الخاص بك" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "خطأ" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "وضع التجربة" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"وضع التجربة غير متاح حالياً في هذه السيارة لأن نظام ACC الأصلي يُستخدم للتحكم " -"الطولي." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "جارٍ النسيان..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "إنهاء الإعداد" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "وضع Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"لأقصى فعالية، أحضر جهازك إلى الداخل واتصل بمحوّل USB‑C جيد وبشبكة Wi‑Fi " -"أسبوعياً.\n" -"\n" -"يمكن أن يعمل وضع Firehose أيضاً أثناء القيادة إذا كنت متصلاً بنقطة اتصال أو " -"بشريحة غير محدودة.\n" -"\n" -"\n" -"الأسئلة الشائعة\n" -"\n" -"هل يهم كيف أو أين أقود؟ لا، قد بقدر المعتاد.\n" -"\n" -"هل يتم سحب كل مقاطعي في وضع Firehose؟ لا، نقوم بسحب مجموعة فرعية من " -"المقاطع.\n" -"\n" -"ما هو محول USB‑C الجيد؟ أي شاحن هاتف أو حاسب محمول سريع سيكون مناسباً.\n" -"\n" -"هل يهم أي برنامج أشغّل؟ نعم، فقط openpilot الأصلي (وبعض التفرعات المحددة) " -"يمكن استخدامه للتدريب." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "نسيان" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "هل تريد نسيان شبكة Wi‑Fi \"{}\"؟" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "جيد" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "اذهب إلى https://connect.comma.ai على هاتفك" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "مرتفع" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "شبكة مخفية" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "غير نشط: اتصل بشبكة غير محدودة التعرفة" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "تثبيت" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "عنوان IP" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "تثبيت التحديث" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "وضع تصحيح عصا التحكم" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "جارٍ التحميل" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "وضع المناورة الطولية" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "أقصى" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "زد من تحميل بيانات التدريب لتحسين نماذج قيادة openpilot." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "غير متوفر" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "لا" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "الشبكة" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "لم يتم العثور على مفاتيح SSH" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "لم يتم العثور على مفاتيح SSH للمستخدم '{}'" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "لا توجد ملاحظات إصدار متاحة." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "غير متصل" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "موافق" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "متصل" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "فتح" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "إقران" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "معاينة" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "ميزات PRIME:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "إقران الجهاز" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "إقران الجهاز" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "قم بإقران جهازك بحساب comma" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"أقرِن جهازك مع comma connect (connect.comma.ai) واحصل على عرض comma prime." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "يرجى الاتصال بشبكة Wi‑Fi لإكمال الاقتران الأولي" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "إيقاف التشغيل" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "منع رفع البيانات الكبيرة عند الاتصال بشبكة Wi‑Fi محدودة التعرفة" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "منع رفع البيانات الكبيرة عند الاتصال الخلوي محدود التعرفة" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"عاين كاميرا مواجهة السائق للتأكد من أن مراقبة السائق تتم برؤية جيدة. (يجب أن " -"تكون المركبة متوقفة)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "خطأ في رمز QR" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "إزالة" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "إعادة ضبط" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "مراجعة" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "إعادة التشغيل" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "إعادة تشغيل الجهاز" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "إعادة التشغيل والتحديث" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"استقبال تنبيهات للتوجيه للعودة إلى المسار عند انحراف المركبة فوق خط المسار " -"المُكتشف بدون إشارة انعطاف مفعّلة أثناء القيادة فوق 31 ميل/س (50 كم/س)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "تسجيل ورفع فيديو كاميرا السائق" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "تسجيل ورفع صوت الميكروفون" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"تسجيل وتخزين صوت الميكروفون أثناء القيادة. سيُدرج الصوت في فيديو الكاميرا " -"الأمامية في comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "لوائح" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "مسترخٍ" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "وصول عن بُعد" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "لقطات عن بُعد" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "انتهت مهلة الطلب" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "إعادة ضبط" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "إعادة ضبط المعايرة" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "مراجعة دليل التدريب" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "مراجعة قواعد وميزات وحدود openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "اختيار" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "مفاتيح SSH" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "جارٍ مسح شبكات Wi‑Fi..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "اختيار" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "اختر فرعاً" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "اختر لغة" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "الرقم التسلسلي" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "تأجيل التحديث" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "البرمجيات" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "قياسي" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"يوصى بالوضع القياسي. في الوضع العدواني، سيتبع openpilot السيارات الأمامية عن " -"قرب وسيكون أكثر شدة في الوقود والفرامل. في الوضع المسترخي سيبقى بعيداً أكثر " -"عن السيارات الأمامية. في السيارات المدعومة، يمكنك التنقل بين هذه الشخصيات " -"بزر مسافة المقود." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "النظام لا يستجيب" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "تولَّ السيطرة فوراً" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "الحرارة" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "الفرع المستهدف" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "كلمة مرور الربط" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "مفاتيح التبديل" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "إلغاء التثبيت" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "تحديث" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "إلغاء التثبيت" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "غير معروف" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "يتم تنزيل التحديثات فقط عندما تكون السيارة متوقفة." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "الترقية الآن" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"ارفع بيانات من كاميرا مواجهة السائق وساعد في تحسين خوارزمية مراقبة السائق." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "استخدام النظام المتري" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"استخدم نظام openpilot للتحكم الذكي بالسرعة والمساعدة على البقاء داخل المسار. " -"يتطلب استخدام هذه الميزة انتباهك الكامل في جميع الأوقات." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "المركبة" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "عرض" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "بانتظار البدء" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"تحذير: يمنح هذا وصول SSH إلى جميع المفاتيح العامة في إعدادات GitHub الخاصة " -"بك. لا تُدخل مطلقاً اسم مستخدم GitHub غير اسمك. لن يطلب منك موظف في comma أبداً " -"إضافة اسم مستخدمهم." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "مرحباً بك في openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "عند التمكين، سيؤدي الضغط على دواسة الوقود إلى فصل openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "شبكة Wi‑Fi محدودة التعرفة" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "كلمة مرور خاطئة" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "يجب عليك قبول الشروط والأحكام لاستخدام openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"يجب عليك قبول الشروط والأحكام لاستخدام openpilot. اقرأ أحدث الشروط على " -"https://comma.ai/terms قبل المتابعة." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "بدء تشغيل الكاميرا" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "افتراضي" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "أسفل" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "فشل التحقق من وجود تحديث" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "لـ \"{}\"" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "كم/س" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "اتركه فارغاً للإعداد التلقائي" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "يسار" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "محدود التعرفة" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "ميل/س" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "أبداً" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "الآن" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "التحكم الطولي لـ openpilot (ألفا)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot غير متاح" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"يعمل openpilot افتراضياً في وضع الهدوء. يفعّل وضع التجربة ميزات بمستوى ألفا " -"غير الجاهزة لوضع الهدوء. الميزات التجريبية مدرجة أدناه:

    التحكم الطولي " -"من طرف لطرف


    دع نموذج القيادة يتحكم في الوقود والفرامل. سيقود " -"openpilot كما يظن أن الإنسان سيقود، بما في ذلك التوقف عند الإشارات الحمراء " -"وعلامات التوقف. بما أن نموذج القيادة يقرر السرعة، فإن السرعة المضبوطة تعمل " -"كحد أعلى فقط. هذه ميزة بجودة ألفا؛ يُتوقع حدوث أخطاء.

    تصوير قيادة " -"جديد


    سينتقل عرض القيادة إلى الكاميرا الواسعة المواجهة للطريق عند " -"السرعات المنخفضة لإظهار بعض المنعطفات بشكل أفضل. كما سيظهر شعار وضع التجربة " -"في الزاوية العلوية اليمنى." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"يقوم openpilot بالمعايرة بشكل مستمر، ونادراً ما تتطلب إعادة الضبط. ستؤدي " -"إعادة ضبط المعايرة إلى إعادة تشغيل openpilot إذا كانت السيارة قيد التشغيل." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"يتعلم openpilot القيادة بمشاهدة البشر، مثلك، يقودون.\n" -"\n" -"يتيح وضع Firehose زيادة تحميل بيانات التدريب لتحسين نماذج قيادة openpilot. " -"المزيد من البيانات يعني نماذج أكبر، مما يعني وضع تجربة أفضل." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "قد يأتي التحكم الطولي لـ openpilot في تحديث مستقبلي." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"يتطلب openpilot تركيب الجهاز ضمن 4° يساراً أو يميناً وضمن 5° للأعلى أو 9° " -"للأسفل." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "يمين" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "غير محدود" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "أعلى" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "محدّث، آخر تحقق: أبداً" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "محدّث، آخر تحقق {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "تحديث متاح" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} تنبيه" -msgstr[1] "{} تنبيه" -msgstr[2] "{} تنبيهان" -msgstr[3] "{} تنبيهات" -msgstr[4] "{} تنبيهات" -msgstr[5] "{} تنبيه" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "قبل {} يوم" -msgstr[1] "قبل {} يوم" -msgstr[2] "قبل {} يومين" -msgstr[3] "قبل {} أيام" -msgstr[4] "قبل {} أيام" -msgstr[5] "قبل {} يوم" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "قبل {} ساعة" -msgstr[1] "قبل {} ساعة" -msgstr[2] "قبل {} ساعتين" -msgstr[3] "قبل {} ساعات" -msgstr[4] "قبل {} ساعات" -msgstr[5] "قبل {} ساعة" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "قبل {} دقيقة" -msgstr[1] "قبل {} دقيقة" -msgstr[2] "قبل {} دقيقتين" -msgstr[3] "قبل {} دقائق" -msgstr[4] "قبل {} دقائق" -msgstr[5] "قبل {} دقيقة" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "{} مقطع من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." -msgstr[1] "{} مقطع من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." -msgstr[2] "{} مقطعان من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." -msgstr[3] "{} مقاطع من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." -msgstr[4] "{} مقاطع من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." -msgstr[5] "{} مقطع من قيادتك ضمن مجموعة بيانات التدريب حتى الآن." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ مشترك" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 وضع Firehose 🔥" diff --git a/selfdrive/ui/translations/app_de.po b/selfdrive/ui/translations/app_de.po deleted file mode 100644 index f32c27a9efb198..00000000000000 --- a/selfdrive/ui/translations/app_de.po +++ /dev/null @@ -1,1221 +0,0 @@ -# German translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-20 16:35-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: de\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n != 1);\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " Die Lenkmoment-Reaktionskalibrierung ist abgeschlossen." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " Die Lenkmoment-Reaktionskalibrierung ist zu {}% abgeschlossen." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Ihr Gerät ist um {:.1f}° {} und {:.1f}° {} ausgerichtet." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 Jahr Fahrtdatenspeicherung" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "24/7 LTE‑Verbindung" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"WARNUNG: Die Längsregelung von openpilot befindet sich für dieses " -"Fahrzeug in der Alpha-Phase und deaktiviert das automatische Notbremssystem " -"(AEB).

    Auf diesem Fahrzeug verwendet openpilot standardmäßig den " -"integrierten ACC statt der openpilot-Längsregelung. Aktivieren Sie dies, um " -"auf die openpilot-Längsregelung umzuschalten. Das Aktivieren des " -"Experimentalmodus wird empfohlen, wenn Sie die openpilot-Längsregelung " -"(Alpha) aktivieren." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    Kalibrierung der Lenkverzögerung abgeschlossen." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    Kalibrierung der Lenkverzögerung zu {}% abgeschlossen." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "AKTIV" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) ermöglicht die Verbindung mit Ihrem Gerät über " -"USB oder über das Netzwerk. Siehe https://docs.comma.ai/how-to/connect-to-" -"comma für weitere Informationen." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "HINZUFÜGEN" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN‑Einstellung" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Übermäßige Betätigung bestätigen" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "Erweitert" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Aggressiv" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Zustimmen" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Immer aktive Fahrerüberwachung" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"Eine Alpha-Version der openpilot-Längsregelung kann zusammen mit dem " -"Experimentalmodus auf Nicht-Release-Zweigen getestet werden." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Sind Sie sicher, dass Sie ausschalten möchten?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Sind Sie sicher, dass Sie neu starten möchten?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Sind Sie sicher, dass Sie die Kalibrierung zurücksetzen möchten?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Sind Sie sicher, dass Sie deinstallieren möchten?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Zurück" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Werden Sie comma prime Mitglied auf connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" -"Fügen Sie connect.comma.ai Ihrem Startbildschirm hinzu, um es wie eine App " -"zu verwenden" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "ÄNDERN" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "PRÜFEN" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "CHILL‑MODUS AKTIV" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "VERBINDUNG" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "VERBINDUNG" - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "Abbrechen" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "Getaktete Mobilfunkverbindung" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Sprache ändern" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -" Durch Ändern dieser Einstellung wird openpilot neu gestartet, wenn das Auto " -"eingeschaltet ist." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "Klicken Sie auf \"add new device\" und scannen Sie den QR‑Code rechts" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Schließen" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Aktuelle Version" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "HERUNTERLADEN" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Ablehnen" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Ablehnen, openpilot deinstallieren" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Entwickler" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Gerät" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Beim Gaspedal deaktivieren" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Zum Ausschalten deaktivieren" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Zum Neustart deaktivieren" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Zum Zurücksetzen der Kalibrierung deaktivieren" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Geschwindigkeit in km/h statt mph anzeigen." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "Dongle-ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Herunterladen" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Fahrerkamera" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Fahrstil" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "BEARBEITEN" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "FEHLER" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "EXPERIMENTALMODUS AKTIV" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Aktivieren" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "ADB aktivieren" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Spurverlassenswarnungen aktivieren" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "openpilot aktivieren" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "SSH aktivieren" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Spurverlassenswarnungen aktivieren" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "Fahrerüberwachung auch aktivieren, wenn openpilot nicht aktiv ist." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "openpilot aktivieren" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Den Schalter für die openpilot-Längsregelung (Alpha) aktivieren, um den " -"Experimentalmodus zu erlauben." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "APN eingeben" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "SSID eingeben" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "Neues Tethering‑Passwort eingeben" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "Passwort eingeben" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Geben Sie Ihren GitHub‑Benutzernamen ein" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "Fehler" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Experimentalmodus" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"Der Experimentalmodus ist derzeit auf diesem Fahrzeug nicht verfügbar, da " -"der serienmäßige ACC für die Längsregelung verwendet wird." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "WIRD VERGESSEN..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Einrichtung abschließen" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehose‑Modus" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Für maximale Wirksamkeit bringen Sie Ihr Gerät regelmäßig ins Haus und " -"verbinden es wöchentlich mit einem guten USB‑C‑Adapter und WLAN.\n" -"\n" -"Der Firehose‑Modus kann auch während der Fahrt funktionieren, wenn eine " -"Verbindung zu einem Hotspot oder einer unbegrenzten SIM besteht.\n" -"\n" -"\n" -"Häufig gestellte Fragen\n" -"\n" -"Spielt es eine Rolle, wie oder wo ich fahre? Nein, fahren Sie einfach wie " -"gewöhnlich.\n" -"\n" -"Werden alle meine Segmente im Firehose‑Modus abgeholt? Nein, wir ziehen " -"selektiv eine Teilmenge Ihrer Segmente.\n" -"\n" -"Was ist ein guter USB‑C‑Adapter? Jeder schnelle Telefon‑ oder Laptoplader " -"sollte ausreichen.\n" -"\n" -"Spielt es eine Rolle, welche Software ich verwende? Ja, nur " -"Upstream‑openpilot (und bestimmte Forks) können für das Training verwendet " -"werden." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "Vergessen" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "WLAN‑Netz „{}“ vergessen?" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "GUT" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Gehen Sie auf Ihrem Telefon zu https://connect.comma.ai" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "HOCH" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Netzwerk" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "INAKTIV: Mit einem unlimitierten Netzwerk verbinden" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "INSTALLIEREN" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP‑Adresse" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Update installieren" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Joystick‑Debugmodus" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "LADEN" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Längsmanövermodus" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MAX" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Maximieren Sie Ihre Trainingsdaten‑Uploads, um die Fahrmodelle von openpilot " -"zu verbessern." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "k. A." - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "KEIN" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Netzwerk" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "Keine SSH‑Schlüssel gefunden" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "Keine SSH‑Schlüssel für Benutzer '{username}' gefunden" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "Keine Versionshinweise verfügbar." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "OFFLINE" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "ONLINE" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Öffnen" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "KOPPELN" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "VORSCHAU" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "PRIME‑FUNKTIONEN:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Gerät koppeln" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Gerät koppeln" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Koppeln Sie Ihr Gerät mit Ihrem comma‑Konto" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Koppeln Sie Ihr Gerät mit comma connect (connect.comma.ai) und lösen Sie Ihr " -"comma‑prime‑Angebot ein." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Bitte mit WLAN verbinden, um das erste Koppeln abzuschließen" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Ausschalten" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Vorschau der Fahrer‑Kamera, um sicherzustellen, dass die Fahrerüberwachung " -"gute Sicht hat. (Fahrzeug muss ausgeschaltet sein)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QR‑Code‑Fehler" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "ENTFERNEN" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "ZURÜCKSETZEN" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "ANSEHEN" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Neustart" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Gerät neu starten" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Neustarten und aktualisieren" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Erhalten Sie Warnungen, um zurück in die Spur zu lenken, wenn Ihr Fahrzeug " -"ohne Blinker über eine erkannte Spurlinie driftet und über 31 mph (50 km/h) " -"fährt." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Fahrerkamera aufzeichnen und hochladen" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Mikrofonton aufzeichnen und hochladen" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Mikrofonton während der Fahrt aufzeichnen und speichern. Die Audiospur wird " -"im Dashcam‑Video in comma connect enthalten sein." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Vorschriften" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Entspannt" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Fernzugriff" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Remote‑Schnappschüsse" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Zeitüberschreitung bei der Anfrage" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Zurücksetzen" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Kalibrierung zurücksetzen" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Trainingsanleitung ansehen" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "" -"Überprüfen Sie die Regeln, Funktionen und Einschränkungen von openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH‑Schlüssel" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "WLAN‑Netzwerke werden gesucht..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "Auswählen" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Sprache auswählen" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Seriennummer" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Update verschieben" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Software" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Standard" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Standard wird empfohlen. Im aggressiven Modus folgt openpilot " -"vorausfahrenden Fahrzeugen näher und ist beim Gasgeben und Bremsen " -"aggressiver. Im entspannten Modus bleibt openpilot weiter entfernt. Bei " -"unterstützten Fahrzeugen können Sie mit der Abstandstaste am Lenkrad " -"zwischen diesen Profilen wechseln." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "System reagiert nicht" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "SOFORT DIE KONTROLLE ÜBERNEHMEN" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMP" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "Tethering‑Passwort" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Schalter" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "DEINSTALLIEREN" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "UPDATE" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Deinstallieren" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Unbekannt" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Updates werden nur heruntergeladen, wenn das Auto aus ist." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Jetzt abonnieren" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Daten von der Fahrer‑Kamera hochladen und den Fahrerüberwachungs‑Algorithmus " -"verbessern." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Metersystem verwenden" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Verwenden Sie openpilot für adaptive Geschwindigkeitsregelung und " -"Spurhalteassistenz. Ihre Aufmerksamkeit ist jederzeit erforderlich, um diese " -"Funktion zu nutzen." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "FAHRZEUG" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "ANSEHEN" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Warten auf Start" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Warnung: Dies gewährt SSH‑Zugriff auf alle öffentlichen Schlüssel in Ihren " -"GitHub‑Einstellungen. Geben Sie niemals einen anderen GitHub‑Benutzernamen " -"als Ihren eigenen ein. Ein comma‑Mitarbeiter wird Sie NIEMALS bitten, seinen " -"GitHub‑Benutzernamen hinzuzufügen." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Willkommen bei openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "Wenn aktiviert, deaktiviert das Drücken des Gaspedals openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "WLAN" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Getaktetes WLAN‑Netzwerk" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "Falsches Passwort" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "" -"Sie müssen die Nutzungsbedingungen akzeptieren, um openpilot zu verwenden." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"Sie müssen die Nutzungsbedingungen akzeptieren, um openpilot zu verwenden. " -"Lesen Sie die aktuellen Bedingungen unter https://comma.ai/terms, bevor Sie " -"fortfahren." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "Kamera startet" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "Standard" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "unten" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "Überprüfung auf Updates fehlgeschlagen" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "für „{}“" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "für automatische Konfiguration leer lassen" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "links" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "getaktet" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "nie" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "jetzt" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot Längsregelung (Alpha)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot nicht verfügbar" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot fährt standardmäßig im Chill‑Modus. Der Experimentalmodus " -"aktiviert Funktionen im Alpha‑Status, die für den Chill‑Modus noch nicht " -"bereit sind. Die experimentellen Funktionen sind unten aufgeführt:" -"

    End-to‑End‑Längsregelung


    Das Fahrmodell steuert Gas und " -"Bremse. openpilot fährt so, wie es einen Menschen einschätzt, einschließlich " -"Anhalten an roten Ampeln und Stoppschildern. Da das Modell die " -"Geschwindigkeit bestimmt, dient die eingestellte Geschwindigkeit nur als " -"Obergrenze. Dies ist eine Alpha‑Funktion; Fehler sind zu erwarten." -"

    Neue Fahrvisualisierung


    Die Visualisierung wechselt bei " -"niedriger Geschwindigkeit auf die nach vorn gerichtete Weitwinkelkamera, um " -"manche Kurven besser zu zeigen. Das Experimentalmodus‑Logo wird außerdem " -"oben rechts angezeigt." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -" Durch Ändern dieser Einstellung wird openpilot neu gestartet, wenn das Auto " -"eingeschaltet ist." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot lernt das Fahren, indem es Menschen wie Sie beobachtet.\n" -"\n" -"Der Firehose‑Modus ermöglicht es Ihnen, Ihre Trainingsdaten‑Uploads zu " -"maximieren, um die Fahrmodelle von openpilot zu verbessern. Mehr Daten " -"bedeuten größere Modelle – und damit einen besseren Experimentalmodus." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "Die openpilot‑Längsregelung könnte in einem zukünftigen Update kommen." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilot erfordert, dass das Gerät innerhalb von 4° nach links oder rechts " -"und innerhalb von 5° nach oben oder 9° nach unten montiert ist." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "rechts" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "unbegrenzt" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "oben" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "Aktuell, zuletzt geprüft: nie" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "Aktuell, zuletzt geprüft: {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "Update verfügbar" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} WARNUNG" -msgstr[1] "{} WARNUNGEN" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "vor {} Tag" -msgstr[1] "vor {} Tagen" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "vor {} Stunde" -msgstr[1] "vor {} Stunden" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "vor {} Minute" -msgstr[1] "vor {} Minuten" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "{} Segment Ihrer Fahrten ist bisher im Trainingsdatensatz." -msgstr[1] "{} Segmente Ihrer Fahrten sind bisher im Trainingsdatensatz." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ ABONNIERT" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehose‑Modus 🔥" diff --git a/selfdrive/ui/translations/app_en.po b/selfdrive/ui/translations/app_en.po deleted file mode 100644 index 6fbb537aff4532..00000000000000 --- a/selfdrive/ui/translations/app_en.po +++ /dev/null @@ -1,1207 +0,0 @@ -# English translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-21 18:18-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: en\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n != 1);\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " Steering torque response calibration is complete." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " Steering torque response calibration is {}% complete." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Your device is pointed {:.1f}° {} and {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 year of drive storage" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "24/7 LTE connectivity" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    Steering lag calibration is complete." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    Steering lag calibration is {}% complete." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "ACTIVE" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "ADD" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN Setting" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Acknowledge Excessive Actuation" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "Advanced" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Aggressive" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Agree" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Always-On Driver Monitoring" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Are you sure you want to power off?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Are you sure you want to reboot?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Are you sure you want to reset calibration?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Are you sure you want to uninstall?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Back" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Become a comma prime member at connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "Bookmark connect.comma.ai to your home screen to use it like an app" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "CHANGE" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "CHECK" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "CHILL MODE ON" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECT" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "CONNECTING..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "Cancel" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "Cellular Metered" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Change Language" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "Changing this setting will restart openpilot if the car is powered on." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "Click \"add new device\" and scan the QR code on the right" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Close" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Current Version" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "DOWNLOAD" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Decline" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Decline, uninstall openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Developer" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Device" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Disengage on Accelerator Pedal" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Disengage to Power Off" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Disengage to Reboot" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Disengage to Reset Calibration" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Display speed in km/h instead of mph." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "Dongle ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Download" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Driver Camera" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Driving Personality" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "EDIT" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "ERROR" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "EXPERIMENTAL MODE ON" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Enable" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "Enable ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Enable Lane Departure Warnings" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "Enable Roaming" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "Enable SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Enable Tethering" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "Enable driver monitoring even when openpilot is not engaged." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "Enable openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "Enter APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "Enter SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "Enter new tethering password" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "Enter password" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Enter your GitHub username" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "Error" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Experimental Mode" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "FORGETTING..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Finish Setup" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehose Mode" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "Forget" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Forget Wi-Fi Network \"{}\"?" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "GOOD" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Go to https://connect.comma.ai on your phone" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "HIGH" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Hidden Network" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "INACTIVE: connect to an unmetered network" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "INSTALL" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP Address" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Install Update" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Joystick Debug Mode" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "LOADING" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Longitudinal Maneuver Mode" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MAX" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Maximize your training data uploads to improve openpilot's driving models." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "N/A" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "NO" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Network" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "No SSH keys found" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "No SSH keys found for user '{}'" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "No release notes available." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "OFFLINE" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "ONLINE" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Open" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "PAIR" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "PREVIEW" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "PRIME FEATURES:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Pair Device" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Pair device" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Pair your device to your comma account" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Please connect to Wi-Fi to complete initial pairing" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Power Off" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "Prevent large data uploads when on a metered Wi-Fi connection" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "Prevent large data uploads when on a metered cellular connection" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QR Code Error" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "REMOVE" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "RESET" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "REVIEW" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Reboot" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Reboot Device" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Reboot and Update" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Record and Upload Driver Camera" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Record and Upload Microphone Audio" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Regulatory" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Relaxed" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Remote access" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Remote snapshots" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Request timed out" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Reset" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Reset Calibration" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Review Training Guide" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "Review the rules, features, and limitations of openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH Keys" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Scanning Wi-Fi networks..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "Select" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Select a language" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Serial" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Snooze Update" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Software" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Standard" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "System Unresponsive" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "TAKE CONTROL IMMEDIATELY" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMP" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "Tethering Password" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Toggles" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "UNINSTALL" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "UPDATE" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Uninstall" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Unknown" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Updates are only downloaded while the car is off." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Upgrade Now" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Use Metric System" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "VEHICLE" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "VIEW" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Waiting to start" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Welcome to openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "When enabled, pressing the accelerator pedal will disengage openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi-Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Wi-Fi Network Metered" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "Wrong password" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "You must accept the Terms and Conditions in order to use openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "camera starting" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "default" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "down" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "failed to check for update" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "for \"{}\"" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "leave blank for automatic configuration" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "left" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "metered" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "never" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "now" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot Longitudinal Control (Alpha)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot Unavailable" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilot longitudinal control may come in a future update." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "right" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "unmetered" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "up" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "up to date, last checked never" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "up to date, last checked {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "update available" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} ALERT" -msgstr[1] "{} ALERTS" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} day ago" -msgstr[1] "{} days ago" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} hour ago" -msgstr[1] "{} hours ago" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} minute ago" -msgstr[1] "{} minutes ago" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "{} segment of your driving is in the training dataset so far." -msgstr[1] "{} segments of your driving is in the training dataset so far." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ SUBSCRIBED" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehose Mode 🔥" diff --git a/selfdrive/ui/translations/app_es.po b/selfdrive/ui/translations/app_es.po deleted file mode 100644 index 59b9e6dfdbecd0..00000000000000 --- a/selfdrive/ui/translations/app_es.po +++ /dev/null @@ -1,1225 +0,0 @@ -# Spanish translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-20 16:35-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: es\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n != 1);\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " La calibración de respuesta de par de dirección está completa." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " La calibración de respuesta de par de dirección está {}% completa." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Tu dispositivo está orientado {:.1f}° {} y {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 año de almacenamiento de conducción" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "Conectividad LTE 24/7" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"ADVERTENCIA: el control longitudinal de openpilot está en alpha para este " -"coche y deshabilitará el Frenado Automático de Emergencia (AEB).

    En este coche, openpilot usa por defecto el ACC integrado del " -"coche en lugar del control longitudinal de openpilot. Activa esto para " -"cambiar al control longitudinal de openpilot. Se recomienda activar el modo " -"Experimental al habilitar el control longitudinal de openpilot (alpha)." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "ACTIVO" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) permite conectar tu dispositivo por USB o por la " -"red. Consulta https://docs.comma.ai/how-to/connect-to-comma para más " -"información." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "AÑADIR" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Reconocer actuación excesiva" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Agresivo" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Aceptar" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Supervisión del conductor siempre activa" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"Se puede probar una versión alpha del control longitudinal de openpilot, " -"junto con el modo Experimental, en ramas que no son de lanzamiento." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "¿Seguro que quieres apagar?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "¿Seguro que quieres reiniciar?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "¿Seguro que quieres restablecer la calibración?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "¿Seguro que quieres desinstalar?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Atrás" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Hazte miembro de comma prime en connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" -"Añade connect.comma.ai a tu pantalla de inicio para usarlo como una app" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "CAMBIAR" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "COMPROBAR" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "MODO CHILL ACTIVADO" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONECTAR" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "CONECTAR" - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Cambiar idioma" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -" Cambiar esta configuración reiniciará openpilot si el coche está encendido." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "" -"Haz clic en \"añadir nuevo dispositivo\" y escanea el código QR de la derecha" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Cerrar" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Versión actual" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "DESCARGAR" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Rechazar" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Rechazar, desinstalar openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Desarrollador" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Dispositivo" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Desactivar con el pedal del acelerador" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Desactivar para apagar" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Desactivar para reiniciar" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Desactivar para restablecer la calibración" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Mostrar la velocidad en km/h en lugar de mph." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "ID del dongle" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Descargar" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Cámara del conductor" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Estilo de conducción" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "ERROR" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "MODO EXPERIMENTAL ACTIVADO" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Activar" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "Activar ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Activar advertencias de salida de carril" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "Activar openpilot" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "Activar SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Activar advertencias de salida de carril" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "" -"Activar la supervisión del conductor incluso cuando openpilot no esté " -"activado." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "Activar openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Activa el interruptor de control longitudinal de openpilot (alpha) para " -"permitir el modo Experimental." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Introduce tu nombre de usuario de GitHub" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Modo experimental" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"El modo experimental no está disponible actualmente en este coche, ya que se " -"usa el ACC de fábrica para el control longitudinal." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Finalizar configuración" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Modo Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Para la máxima efectividad, lleva tu dispositivo al interior y conéctalo " -"semanalmente a un buen adaptador USB‑C y Wi‑Fi.\n" -"\n" -"El Modo Firehose también puede funcionar mientras conduces si está conectado " -"a un hotspot o a una SIM ilimitada.\n" -"\n" -"\n" -"Preguntas frecuentes\n" -"\n" -"¿Importa cómo o dónde conduzco? No, conduce como normalmente lo harías.\n" -"\n" -"¿Se suben todos mis segmentos en el Modo Firehose? No, seleccionamos un " -"subconjunto de tus segmentos.\n" -"\n" -"¿Qué es un buen adaptador USB‑C? Cualquier cargador rápido de teléfono o " -"laptop sirve.\n" -"\n" -"¿Importa qué software ejecuto? Sí, solo openpilot upstream (y forks " -"particulares) pueden usarse para entrenamiento." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "BUENO" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Ve a https://connect.comma.ai en tu teléfono" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "ALTO" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Red" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "INACTIVO: conéctate a una red sin límites" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "INSTALAR" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Instalar actualización" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Modo de depuración de joystick" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "CARGANDO" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Modo de maniobra longitudinal" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MÁX" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Maximiza tus cargas de datos de entrenamiento para mejorar los modelos de " -"conducción de openpilot." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "NO" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Red" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "No se encontraron claves SSH" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "No se encontraron claves SSH para el usuario '{username}'" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "No hay notas de versión disponibles." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "SIN CONEXIÓN" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "EN LÍNEA" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Abrir" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "EMPAREJAR" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "VISTA PREVIA" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "FUNCIONES PRIME:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Emparejar dispositivo" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Emparejar dispositivo" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Empareja tu dispositivo con tu cuenta de comma" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Empareja tu dispositivo con comma connect (connect.comma.ai) y reclama tu " -"oferta de comma prime." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Conéctate a Wi‑Fi para completar el emparejamiento inicial" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Apagar" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Previsualiza la cámara hacia el conductor para asegurarte de que la " -"supervisión del conductor tenga buena visibilidad. (el vehículo debe estar " -"apagado)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "Error de código QR" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "ELIMINAR" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "RESTABLECER" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "REVISAR" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Reiniciar" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Reiniciar dispositivo" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Reiniciar y actualizar" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Recibe alertas para volver al carril cuando tu vehículo se desvíe sobre una " -"línea de carril detectada sin la direccional activada mientras conduces a " -"más de 31 mph (50 km/h)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Grabar y subir cámara del conductor" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Grabar y subir audio del micrófono" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Grabar y almacenar audio del micrófono mientras conduces. El audio se " -"incluirá en el video de la dashcam en comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Reglamentario" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Relajado" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Acceso remoto" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Capturas remotas" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Se agotó el tiempo de espera de la solicitud" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Restablecer" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Restablecer calibración" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Revisar guía de entrenamiento" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "Revisa las reglas, funciones y limitaciones de openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Selecciona un idioma" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Número de serie" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Posponer actualización" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Software" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Estándar" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Se recomienda Estándar. En modo agresivo, openpilot seguirá más de cerca a " -"los coches delanteros y será más agresivo con el acelerador y el freno. En " -"modo relajado, openpilot se mantendrá más lejos de los coches delanteros. En " -"coches compatibles, puedes cambiar entre estas personalidades con el botón " -"de distancia del volante." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "Sistema sin respuesta" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "TOME EL CONTROL INMEDIATAMENTE" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMP" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Interruptores" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "DESINSTALAR" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "ACTUALIZAR" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Desinstalar" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Desconocido" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Las actualizaciones solo se descargan cuando el coche está apagado." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Mejorar ahora" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Sube datos de la cámara orientada al conductor y ayuda a mejorar el " -"algoritmo de supervisión del conductor." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Usar sistema métrico" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Usa el sistema openpilot para control de crucero adaptativo y asistencia de " -"mantenimiento de carril. Tu atención se requiere en todo momento para usar " -"esta función." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "VEHÍCULO" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "VER" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Esperando para iniciar" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Advertencia: Esto otorga acceso SSH a todas las claves públicas en tu " -"configuración de GitHub. Nunca introduzcas un nombre de usuario de GitHub " -"que no sea el tuyo. Un empleado de comma NUNCA te pedirá que agregues su " -"nombre de usuario de GitHub." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Bienvenido a openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" -"Cuando está activado, al presionar el pedal del acelerador se desactivará " -"openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "Debes aceptar los Términos y Condiciones para poder usar openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"Debes aceptar los Términos y Condiciones para usar openpilot. Lee los " -"términos más recientes en https://comma.ai/terms antes de continuar." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "iniciando cámara" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "abajo" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "Error al buscar actualizaciones" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "izquierda" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "nunca" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "ahora" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "Control longitudinal de openpilot (Alpha)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot no disponible" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot conduce por defecto en modo chill. El modo Experimental habilita " -"funciones de nivel alpha que no están listas para el modo chill. Las " -"funciones experimentales se enumeran a continuación:

    Control " -"longitudinal de extremo a extremo


    Deja que el modelo de conducción " -"controle el acelerador y los frenos. openpilot conducirá como piensa que lo " -"haría un humano, incluyendo detenerse en luces rojas y señales de alto. Dado " -"que el modelo decide la velocidad a la que conducir, la velocidad " -"establecida solo actuará como límite superior. Esta es una función de " -"calidad alpha; se deben esperar errores.

    Nueva visualización de " -"conducción


    La visualización de conducción hará la transición a la " -"cámara gran angular orientada a la carretera a bajas velocidades para " -"mostrar mejor algunos giros. El logotipo del modo Experimental también se " -"mostrará en la esquina superior derecha." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -" Cambiar esta configuración reiniciará openpilot si el coche está encendido." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot aprende a conducir observando a humanos, como tú, conducir.\n" -"\n" -"El Modo Firehose te permite maximizar tus cargas de datos de entrenamiento " -"para mejorar los modelos de conducción de openpilot. Más datos significan " -"modelos más grandes, lo que significa un mejor Modo Experimental." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "" -"El control longitudinal de openpilot podría llegar en una actualización " -"futura." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilot requiere que el dispositivo esté montado dentro de 4° a izquierda " -"o derecha y dentro de 5° hacia arriba o 9° hacia abajo." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "derecha" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "arriba" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "actualizado, última comprobación: nunca" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "actualizado, última comprobación: {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "actualización disponible" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} ALERTA" -msgstr[1] "{} ALERTAS" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "hace {} día" -msgstr[1] "hace {} días" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "hace {} hora" -msgstr[1] "hace {} horas" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "hace {} minuto" -msgstr[1] "hace {} minutos" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -"{} segmento de tu conducción está en el conjunto de entrenamiento hasta " -"ahora." -msgstr[1] "" -"{} segmentos de tu conducción están en el conjunto de entrenamiento hasta " -"ahora." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ SUSCRITO" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Modo Firehose 🔥" diff --git a/selfdrive/ui/translations/app_fr.po b/selfdrive/ui/translations/app_fr.po deleted file mode 100644 index 409761588e9652..00000000000000 --- a/selfdrive/ui/translations/app_fr.po +++ /dev/null @@ -1,1236 +0,0 @@ -# French translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: \n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2026-01-24 12:37+0100\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: fr\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n > 1);\n" -"X-Generator: Poedit 3.8\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " L'étalonnage de la réponse du couple de direction est terminé." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " L'étalonnage de la réponse du couple de direction est terminé à {}%." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Votre appareil est orienté {:.1f}° {} et {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 an de stockage de trajets" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "Connexion LTE 24/7" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"ATTENTION : le contrôle longitudinal openpilot est en alpha pour cette " -"voiture et désactivera le freinage d'urgence automatique (AEB).

    Sur cette voiture, openpilot utilise par défaut le régulateur de " -"vitesse adaptatif intégré au véhicule plutôt que le contrôle longitudinal " -"d'openpilot. Activez ceci pour passer au contrôle longitudinal openpilot. Il " -"est recommandé d'activer le mode expérimental lors de l'activation du " -"contrôle longitudinal openpilot alpha." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    L'étalonnage du délai de réponse de la direction est terminé." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "" -"

    L'étalonnage du délai de réponse de la direction est terminé à {}%." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "ACTIF" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) permet de connecter votre appareil via USB ou via " -"le réseau. Voir https://docs.comma.ai/how-to/connect-to-comma pour plus " -"d'informations." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "AJOUTER" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "Paramètres APN" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Accuser réception d'actionnement excessif" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "Avancé" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Agressif" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Accepter" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Surveillance continue du conducteur" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"Une version alpha du contrôle longitudinal openpilot peut être testée, avec " -"le mode expérimental, sur des branches non publiées." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Êtes-vous sûr de vouloir éteindre ?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Êtes-vous sûr de vouloir redémarrer ?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Êtes-vous sûr de vouloir réinitialiser la calibration ?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Êtes-vous sûr de vouloir désinstaller ?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Retour" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Devenez membre comma prime sur connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" -"Ajoutez connect.comma.ai à votre écran d'accueil pour l'utiliser comme une " -"application" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "CHANGER" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "VÉRIFIER" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "MODE CHILL ACTIVÉ" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECTER" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "CONNECTER..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "Annuler" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "Données cellulaire limitées" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Changer la langue" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -"La modification de ce réglage redémarrera openpilot si la voiture est sous " -"tension." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "Cliquez sur \"add new device\" et scannez le code QR à droite" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Fermer" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Version actuelle" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "TÉLÉCHARGER" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Refuser" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Refuser, désinstaller openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Développeur" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Appareil" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Désengager à l'appui sur l'accélérateur" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Désengager pour éteindre" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Désengager pour redémarrer" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Désengager pour réinitialiser la calibration" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Afficher la vitesse en km/h au lieu de mph." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "ID du dongle" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Télécharger" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Caméra conducteur" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Personnalité de conduite" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "EDITER" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "ERREUR" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "MODE EXPÉRIMENTAL ACTIVÉ" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Activer" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "Activer ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Activer les alertes de sortie de voie" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "Activer openpilot" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "Activer SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Activer les alertes de sortie de voie" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "" -"Activer la surveillance du conducteur même lorsque openpilot n'est pas " -"engagé." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "Activer openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Activez l'option de contrôle longitudinal openpilot (alpha) pour autoriser " -"le mode expérimental." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "Saisir l'APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "Entrer le SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "Saisir le mot de passe du partage de connexion" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "Saisir le mot de passe" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Entrez votre nom d'utilisateur GitHub" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "Erreur" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Mode expérimental" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"Le mode expérimental est actuellement indisponible sur cette voiture car " -"l'ACC d'origine est utilisé pour le contrôle longitudinal." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "OUBLIER..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Terminer la configuration" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Mode Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Pour une efficacité maximale, rentrez votre appareil et connectez-le chaque " -"semaine à un bon adaptateur USB-C et au Wi‑Fi.\n" -"\n" -"Le Mode Firehose peut aussi fonctionner pendant que vous conduisez si vous " -"êtes connecté à un hotspot ou à une carte SIM illimitée.\n" -"\n" -"\n" -"Foire aux questions\n" -"\n" -"Est-ce que la manière ou l'endroit où je conduis compte ? Non, conduisez " -"normalement.\n" -"\n" -"Tous mes segments sont-ils récupérés en Mode Firehose ? Non, nous récupérons " -"de façon sélective un sous-ensemble de vos segments.\n" -"\n" -"Quel est un bon adaptateur USB-C ? Tout chargeur rapide de téléphone ou " -"d'ordinateur portable convient.\n" -"\n" -"Le logiciel utilisé importe-t-il ? Oui, seul openpilot amont (et certains " -"forks) peut être utilisé pour l'entraînement." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "Oublier" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Oublier le réseau Wi-Fi \"{}\" ?" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "BON" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Allez sur https://connect.comma.ai sur votre téléphone" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "ÉLEVÉ" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Réseau" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "INACTIF : connectez-vous à un réseau non limité" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "INSTALLER" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "Adresse IP" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Installer la mise à jour" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Mode débogage joystick" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "CHARGEMENT" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Mode de manœuvre longitudinale" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MAX" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Maximisez vos envois de données d'entraînement pour améliorer les modèles de " -"conduite d'openpilot." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "NC" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "NON" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Réseau" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "Aucune clé SSH trouvée" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "Aucune clé SSH trouvée pour l'utilisateur '{}'" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "Aucune note de version disponible." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "HORS LIGNE" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "EN LIGNE" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Ouvrir" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "ASSOCIER" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "APERÇU" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "FONCTIONNALITÉS PRIME :" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Associer l'appareil" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Associer l'appareil" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Associez votre appareil à votre compte comma" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Associez votre appareil à comma connect (connect.comma.ai) et réclamez votre " -"offre comma prime." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Veuillez vous connecter au Wi‑Fi pour terminer l'association initiale" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Éteindre" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" -"Eviter les transferts de données volumineux lorsque vous êtes connecté à un " -"réseau Wi-Fi limité" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" -"Eviter les transferts de données volumineux lors d'une connexion à un réseau " -"cellulaire limité" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Prévisualisez la caméra orientée conducteur pour vous assurer que la " -"surveillance du conducteur a une bonne visibilité. (le véhicule doit être " -"éteint)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "Erreur de code QR" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "SUPPRIMER" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "RÉINITIALISER" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "CONSULTER" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Redémarrer" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Redémarrer l'appareil" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Redémarrer et mettre à jour" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Recevez des alertes pour revenir dans la voie lorsque votre véhicule dépasse " -"une ligne de voie détectée sans clignotant activé en roulant au-delà de 31 " -"mph (50 km/h)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Enregistrer et téléverser la caméra conducteur" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Enregistrer et téléverser l'audio du microphone" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Enregistrer et stocker l'audio du microphone pendant la conduite. L'audio " -"sera inclus dans la vidéo dashcam dans comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Réglementaire" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Détendu" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Accès à distance" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Captures à distance" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Délai de la requête dépassé" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Réinitialiser" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Réinitialiser la calibration" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Consulter le guide d'entraînement" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "Consultez les règles, fonctionnalités et limitations d'openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "SELECTIONNER" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "Clefs SSH" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Analyse des réseaux Wi-Fi..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "Sélectionner" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "Sélectionner une branche" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Sélectionner un langage" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Numéro de série" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Reporter la mise à jour" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Logiciel" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Standard" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Le mode standard est recommandé. En mode agressif, openpilot suivra les " -"véhicules de tête de plus près et sera plus agressif avec l'accélérateur et " -"le frein. En mode détendu, openpilot restera plus éloigné des véhicules de " -"tête. Sur les voitures compatibles, vous pouvez parcourir ces personnalités " -"avec le bouton de distance du volant." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "Système non réactif" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "REPRENEZ IMMÉDIATEMENT LE CONTRÔLE" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMPÉRATURE" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "Branche cible" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "Mot de passe du partage de connexion" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Options" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "DÉSINSTALLER" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "METTRE À JOUR" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Désinstaller" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Inconnu" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "" -"Les mises à jour ne sont téléchargées que lorsque la voiture est éteinte." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Mettre à niveau maintenant" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Téléverser les données de la caméra orientée conducteur et aider à améliorer " -"l'algorithme de surveillance du conducteur." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Utiliser le système métrique" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Utilisez le système openpilot pour l'ACC et l'assistance au maintien de " -"voie. Votre attention est requise en permanence pour utiliser cette " -"fonctionnalité." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "VÉHICULE" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "VOIR" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "En attente de démarrage" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Avertissement : Ceci accorde un accès SSH à toutes les clés publiques dans " -"vos paramètres GitHub. N'entrez jamais un nom d'utilisateur GitHub autre que " -"le vôtre. Un employé comma ne vous demandera JAMAIS d'ajouter son nom " -"d'utilisateur GitHub." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Bienvenue sur openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" -"Lorsque activé, appuyer sur la pédale d'accélérateur désengagera openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Réseau Wi-Fi limité" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "Mauvais mot de passe" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "Vous devez accepter les conditions générales pour utiliser openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"Vous devez accepter les conditions générales pour utiliser openpilot. Lisez " -"les dernières conditions sur https://comma.ai/terms avant de continuer." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "démarrage de la caméra" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "défaut" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "bas" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "échec de la vérification de mise à jour" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "pour \"{}\"" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "ne pas remplir pour une configuration automatique" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "gauche" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "limité" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "jamais" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "maintenant" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "Contrôle longitudinal openpilot (Alpha)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot indisponible" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot roule par défaut en mode chill. Le mode expérimental active des " -"fonctionnalités de niveau alpha qui ne sont pas prêtes pour le mode chill. " -"Les fonctionnalités expérimentales sont listées ci‑dessous:

    Contrôle " -"longitudinal de bout en bout


    Laissez le modèle de conduite contrôler " -"l'accélérateur et les freins. openpilot conduira comme il pense qu'un humain " -"le ferait, y compris s'arrêter aux feux rouges et aux panneaux stop. Comme " -"le modèle décide de la vitesse à adopter, la vitesse réglée n'agira que " -"comme une limite supérieure. C'est une fonctionnalité de qualité alpha ; des " -"erreurs sont à prévoir.

    Nouvelle visualisation de conduite


    La " -"visualisation passera à la caméra grand angle orientée route à basse vitesse " -"pour mieux montrer certains virages. Le logo du mode expérimental sera " -"également affiché en haut à droite." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"La modification de ce réglage redémarrera openpilot si la voiture est sous " -"tension." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot apprend à conduire en regardant des humains, comme vous, " -"conduire.\n" -"\n" -"Le Mode Firehose vous permet de maximiser vos envois de données " -"d'entraînement pour améliorer les modèles de conduite d'openpilot. Plus de " -"données signifie des modèles plus grands, ce qui signifie un meilleur Mode " -"expérimental." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "" -"Le contrôle longitudinal openpilot pourra arriver dans une future mise à " -"jour." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilot exige que l'appareil soit monté à moins de 4° à gauche ou à droite " -"et à moins de 5° vers le haut ou 9° vers le bas." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "droite" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "non limité" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "haut" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "à jour, dernière vérification jamais" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "à jour, dernière vérification {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "mise à jour disponible" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} ALERTE" -msgstr[1] "{} ALERTES" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "il y a {} jour" -msgstr[1] "il y a {} jours" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "il y a {} heure" -msgstr[1] "il y a {} heures" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "il y a {} minute" -msgstr[1] "il y a {} minutes" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -"{} segment de votre conduite est dans l'ensemble d'entraînement jusqu'à " -"présent." -msgstr[1] "" -"{} segments de votre conduite sont dans l'ensemble d'entraînement jusqu'à " -"présent." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ ABONNÉ" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Mode Firehose 🔥" diff --git a/selfdrive/ui/translations/app_ja.po b/selfdrive/ui/translations/app_ja.po deleted file mode 100644 index ca8aac1515a2ff..00000000000000 --- a/selfdrive/ui/translations/app_ja.po +++ /dev/null @@ -1,1197 +0,0 @@ -# Japanese translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: ja\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=1; plural=0;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " ステアリングトルク応答のキャリブレーションが完了しました。" - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " ステアリングトルク応答のキャリブレーションは{}%完了しました。" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " デバイスは{:.1f}°{}、{:.1f}°{}の向きです。" - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "走行データを1年間保存" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "24時間365日のLTE接続" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"警告: この車におけるopenpilotの縦制御はアルファ版であり、自動緊急ブレーキ" -"（AEB）を無効にします。

    この車では、openpilotは縦制御として" -"openpilotではなく車両の内蔵ACCを既定で使用します。openpilotの縦制御に切り替え" -"るにはこの設定を有効にしてください。openpilot縦制御アルファを有効にする場合は" -"実験モードの有効化を推奨します。この設定を変更すると、車が起動中の場合は" -"openpilotが再起動します。" - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    ステアリング遅延のキャリブレーションが完了しました。" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    ステアリング遅延のキャリブレーションは{}%完了しました。" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "アクティブ" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB（Android Debug Bridge）を使用すると、USBまたはネットワーク経由でデバイス" -"に接続できます。詳しくは https://docs.comma.ai/how-to/connect-to-comma を参照" -"してください。" - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "追加" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN設定" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "過度な作動を承認" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "詳細設定" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "アグレッシブ" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "同意する" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "常時ドライバーモニタリング" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"openpilotの縦制御アルファ版は、実験モードと併せて非リリースブランチでテストで" -"きます。" - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "本当に電源をオフにしますか？" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "本当に再起動しますか？" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "本当にキャリブレーションをリセットしますか？" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "本当にアンインストールしますか？" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "戻る" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "connect.comma.aiで comma prime に加入" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "connect.comma.aiをホーム画面に追加してアプリのように使いましょう" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "変更" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "確認" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "チルモードON" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "接続" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "接続中..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "キャンセル" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "従量課金の携帯回線" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "言語を変更" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "車が起動中の場合、この設定を変更するとopenpilotが再起動します。" - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "\"add new device\"を押して右側のQRコードをスキャン" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "閉じる" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "現在のバージョン" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "ダウンロード" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "拒否する" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "拒否してopenpilotをアンインストール" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "開発者" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "デバイス" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "アクセルで解除" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "解除して電源オフ" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "解除して再起動" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "解除してキャリブレーションをリセット" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "速度をmphではなくkm/hで表示します。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "ドングルID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "ダウンロード" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "ドライバーカメラ" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "走行性格" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "編集" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "エラー" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "実験モードON" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "有効化" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "ADBを有効化" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "車線逸脱警報を有効化" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "ローミングを有効化" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "SSHを有効化" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "テザリングを有効化" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "openpilotが未作動でもドライバーモニタリングを有効にします。" - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "openpilotを有効化" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"openpilot縦制御（アルファ）のトグルを有効にすると実験モードが使用できます。" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "APNを入力" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "SSIDを入力" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "新しいテザリングのパスワードを入力" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "パスワードを入力" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "GitHubユーザー名を入力" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "エラー" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "実験モード" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"この車では縦制御に純正ACCを使用するため、現在実験モードは利用できません。" - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "削除中..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "セットアップを完了" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehoseモード" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"最大限の効果を得るため、デバイスを屋内に持ち込み、週に一度は品質の良いUSB-Cア" -"ダプターとWi‑Fiに接続してください。\n" -"\n" -"Firehoseモードは、ホットスポットや無制限SIMに接続していれば走行中でも動作しま" -"す。\n" -"\n" -"\n" -"よくある質問\n" -"\n" -"運転の仕方や場所は関係ありますか？ いいえ。普段どおりに運転してください。\n" -"\n" -"Firehoseモードではすべてのセグメントが取得されますか？ いいえ。セグメントの一" -"部を選択的に取得します。\n" -"\n" -"良いUSB‑Cアダプターとは？ 高速なスマホまたはノートPC用充電器で問題ありませ" -"ん。\n" -"\n" -"どのソフトウェアを使うかは重要ですか？ はい。学習に使えるのは上流のopenpilot" -"（および特定のフォーク）のみです。" - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "削除" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Wi‑Fiネットワーク「{}」を削除しますか？" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "良好" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "スマートフォンで https://connect.comma.ai にアクセス" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "高温" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "非公開ネットワーク" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "非アクティブ：非従量のネットワークに接続してください" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "インストール" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IPアドレス" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "アップデートをインストール" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "ジョイスティックデバッグモード" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "読み込み中" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "縦制御マヌーバーモード" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "最大" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"学習データのアップロードを最大化してopenpilotの運転モデルを改善しましょう。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "該当なし" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "いいえ" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "ネットワーク" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "SSH鍵が見つかりません" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "ユーザー'{}'のSSH鍵が見つかりません" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "リリースノートはありません。" - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "オフライン" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "オンライン" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "開く" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "ペアリング" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "プレビュー" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "prime の特典:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "デバイスをペアリング" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "デバイスをペアリング" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "デバイスをあなたの comma アカウントにペアリング" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"デバイスを comma connect（connect.comma.ai）とペアリングして、comma prime 特" -"典を受け取りましょう。" - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "初回ペアリングを完了するにはWi‑Fiに接続してください" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "電源オフ" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "従量課金のWi‑Fi接続時は大きなデータのアップロードを抑制" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "従量課金の携帯回線接続時は大きなデータのアップロードを抑制" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"ドライバー向きカメラのプレビューでモニタリングの視界を確認します。（車両は停" -"止状態である必要があります）" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QRコードエラー" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "削除" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "リセット" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "確認" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "再起動" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "デバイスを再起動" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "再起動して更新" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"時速31mph（50km/h）を超えて走行中にウインカーを出さず検出された車線を外れた場" -"合、車線内に戻るよう警告を受け取ります。" - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "ドライバーカメラを記録してアップロード" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "マイク音声を記録してアップロード" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"走行中にマイク音声を記録・保存します。音声は comma connect のドライブレコー" -"ダー動画に含まれます。" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "規制情報" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "リラックス" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "リモートアクセス" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "リモートスナップショット" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "リクエストがタイムアウトしました" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "リセット" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "キャリブレーションをリセット" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "トレーニングガイドを確認" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "openpilotのルール、機能、制限を確認" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "選択" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH鍵" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Wi‑Fiネットワークを検索中..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "選択" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "ブランチを選択" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "言語を選択" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "シリアル" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "更新を後で通知" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "ソフトウェア" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "スタンダード" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"標準を推奨します。アグレッシブでは前走車に近づき、加減速も積極的になります。" -"リラックスでは前走車との距離を保ちます。対応車種ではステアリングの車間ボタン" -"でこれらの性格を切り替えられます。" - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "システムが応答しません" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "すぐに手動介入してください" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "温度" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "対象ブランチ" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "テザリングのパスワード" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "トグル" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "アンインストール" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "更新" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "アンインストール" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "不明" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "アップデートは車両の電源が切れている間のみダウンロードされます。" - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "今すぐアップグレード" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"ドライバー向きカメラのデータをアップロードしてモニタリングアルゴリズムの改善" -"に協力してください。" - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "メートル法を使用" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"ACCと車線維持支援にopenpilotを使用します。本機能の使用中は常に注意が必要で" -"す。" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "車両" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "表示" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "開始待機中" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"警告: これはGitHub設定内のすべての公開鍵にSSHアクセスを与えます。自分以外の" -"GitHubユーザー名を絶対に入力しないでください。comma の従業員が自分のGitHub" -"ユーザー名を追加するよう求めることは決してありません。" - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "openpilotへようこそ" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "有効にすると、アクセルを踏むとopenpilotが解除されます。" - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Wi‑Fiネットワーク（従量課金）" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "パスワードが違います" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "openpilotを使用するには、利用規約に同意する必要があります。" - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"openpilotを使用するには利用規約に同意する必要があります。続行する前に " -"https://comma.ai/terms の最新の規約をお読みください。" - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "カメラを起動中" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "既定" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "下" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "アップデートの確認に失敗しました" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "「{}」向け" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "自動設定の場合は空欄のままにしてください" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "左" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "従量" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "なし" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "今" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot 縦制御（アルファ）" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilotは利用できません" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilotは既定でチルモードで走行します。実験モードでは、チルモードにはまだ準" -"備ができていないアルファレベルの機能が有効になります。実験的な機能は以下のと" -"おりです:

    エンドツーエンド縦制御


    運転モデルがアクセルとブレー" -"キを制御します。openpilotは人間のように走行し、赤信号や一時停止でも停止しま" -"す。走行速度は運転モデルが決めるため、設定速度は上限としてのみ機能します。こ" -"れはアルファ品質の機能であり、誤動作が発生する可能性があります。

    新し" -"い運転ビジュアライゼーション


    低速時には道路向きの広角カメラに切り替わ" -"り、一部の曲がりをより良く表示します。画面右上には実験モードのロゴも表示され" -"ます。" - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilotは継続的にキャリブレーションを行っており、リセットが必要になることは" -"稀です。車が起動中にキャリブレーションをリセットするとopenpilotが再起動しま" -"す。" - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilotは、あなたのような人間の運転を見て運転を学習します。\n" -"\n" -"Firehoseモードを使うと、学習データのアップロードを最大化してopenpilotの運転モ" -"デルを改善できます。データが増えるほどモデルが大きくなり、実験モードがより良" -"くなります。" - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilotの縦制御は将来のアップデートで提供される可能性があります。" - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilotでは、デバイスの取り付け角度が左右±4°、上方向5°以内、下方向9°以内で" -"ある必要があります。" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "右" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "非従量" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "上" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "最新です。最終確認: なし" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "最新です。最終確認: {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "更新があります" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{}件のアラート" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{}日前" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{}時間前" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{}分前" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -"これまでにあなたの走行の{}セグメントが学習データセットに含まれています。" - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ 登録済み" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehoseモード 🔥" diff --git a/selfdrive/ui/translations/app_ko.po b/selfdrive/ui/translations/app_ko.po deleted file mode 100644 index f12aebaeb3bebe..00000000000000 --- a/selfdrive/ui/translations/app_ko.po +++ /dev/null @@ -1,1190 +0,0 @@ -# Korean translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: ko\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=1; plural=0;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " 스티어링 토크 응답 보정이 완료되었습니다." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " 스티어링 토크 응답 보정이 {}% 완료되었습니다." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " 장치는 {:.1f}° {} 및 {:.1f}° {} 방향을 가리키고 있습니다." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "주행 데이터 1년 보관" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "연중무휴 LTE 연결" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"경고: 이 차량에서 openpilot의 롱컨 제어는 알파 버전이며 자동 긴급 제동" -"(AEB)을 비활성화합니다.

    이 차량에서는 openpilot 롱컨 제어 대신 " -"차량 내장 ACC가 기본으로 사용됩니다. openpilot 롱컨 제어로 전환하려면 이 설" -"정을 켜세요. 롱컨 제어 알파를 켤 때는 실험 모드 사용을 권장합니다. 차량 전" -"원이 켜져 있는 경우 이 설정을 변경하면 openpilot이 재시작됩니다." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    스티어링 지연 보정이 완료되었습니다." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    스티어링 지연 보정이 {}% 완료되었습니다." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "활성" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB(Android Debug Bridge)를 사용하면 USB 또는 네트워크로 장치에 연결할 수 있" -"습니다. 자세한 내용은 https://docs.comma.ai/how-to/connect-to-comma 를 참고하" -"세요." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "추가" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN 설정" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "과도한 작동을 확인" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "고급" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "공격적" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "동의" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "운전자 모니터링 항상 켜짐" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"openpilot 롱컨 제어 알파 버전은 실험 모드와 함께 비릴리스 브랜치에서 테스트" -"할 수 있습니다." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "정말 전원을 끄시겠습니까?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "정말 재시작하시겠습니까?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "정말 보정을 재설정하시겠습니까?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "정말 제거하시겠습니까?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "뒤로" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "connect.comma.ai에서 comma prime 회원이 되세요" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "connect.comma.ai를 홈 화면에 추가하여 앱처럼 사용하세요" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "변경" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "확인" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "안정적 모드 켜짐" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "연결" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "연결 중..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "취소" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "종량제 셀룰러" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "언어 변경" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "차량 전원이 켜져 있으면 이 설정을 변경할 때 openpilot이 재시작됩니다." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "\"add new device\"를 눌러 오른쪽의 QR 코드를 스캔하세요" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "닫기" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "현재 버전" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "다운로드" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "거부" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "거부하고 openpilot 제거" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "개발자" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "장치" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "가속 페달로 해제" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "해제 후 전원 끄기" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "해제 후 재시작" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "해제 후 캘리브레이션 재설정" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "속도를 mph 대신 km/h로 표시합니다." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "동글 ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "다운로드" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "운전자 카메라" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "주행 성향" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "편집" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "오류" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "실험 모드 켜짐" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "사용" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "ADB 사용" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "차선 이탈 경고 사용" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "로밍 사용" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "SSH 사용" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "테더링 사용" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "openpilot이 작동 중이 아닐 때도 운전자 모니터링을 사용합니다." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "openpilot 사용" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "실험 모드를 사용하려면 openpilot 롱컨 제어(알파) 토글을 켜세요." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "APN 입력" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "SSID 입력" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "새 테더링 비밀번호 입력" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "비밀번호 입력" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "GitHub 사용자 이름 입력" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "오류" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "실험 모드" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"이 차량은 롱컨 제어에 순정 ACC를 사용하므로 현재 실험 모드를 사용할 수 없습" -"니다." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "삭제 중..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "설정 완료" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "파이어호스" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "파이어호스 모드" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"최대의 효과를 위해 주 1회는 장치를 실내로 가져와 품질 좋은 USB‑C 어댑터와 " -"Wi‑Fi에 연결하세요.\n" -"\n" -"핫스팟이나 무제한 SIM에 연결되어 있다면 주행 중에도 파이어호스 모드가 동작합니" -"다.\n" -"\n" -"\n" -"자주 묻는 질문\n" -"\n" -"어떻게, 어디서 운전하는지가 중요한가요? 아니요. 평소처럼 운전하세요.\n" -"\n" -"파이어호스 모드에서 모든 구간을 가져가지나요? 아니요. 일부 구간만 선택" -"적으로 가져갑니다.\n" -"\n" -"좋은 USB‑C 어댑터는 무엇인가요? 빠른 휴대폰 또는 노트북 충전기면 충분합니" -"다.\n" -"\n" -"어떤 소프트웨어를 실행하는지가 중요한가요? 예. 학습에는 업스트림 " -"openpilot(및 일부 포크)만 사용할 수 있습니다." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "삭제" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Wi‑Fi 네트워크 \"{}\"를 삭제하시겠습니까?" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "양호" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "휴대폰에서 https://connect.comma.ai 에 접속하세요" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "높음" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "숨겨진 네트워크" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "비활성: 비종량제 네트워크에 연결하세요" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "설치" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP 주소" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "업데이트 설치" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "조이스틱 디버그 모드" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "로딩 중" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "롱컨 기동 모드" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "최대" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "학습 데이터 업로드를 최대화하여 openpilot의 주행 모델을 개선하세요." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "해당 없음" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "아니오" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "네트워크" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "SSH 키를 찾을 수 없습니다" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "사용자 '{}'의 SSH 키를 찾을 수 없습니다" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "릴리스 노트가 없습니다." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "오프라인" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "확인" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "온라인" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "열기" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "페어링" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "미리보기" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "프라임 기능:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "장치 페어링" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "장치 페어링" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "장치를 귀하의 comma 계정에 페어링하세요" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"장치를 comma connect(connect.comma.ai)와 페어링하고 comma 프라임 혜택을 받으세" -"요." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "초기 페어링을 완료하려면 Wi‑Fi에 연결하세요" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "전원 끄기" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "종량제 Wi‑Fi 연결 시 대용량 업로드 방지" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "종량제 셀룰러 연결 시 대용량 업로드 방지" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"운전자 모니터링의 가시성을 확인하기 위해 운전자 카메라를 미리 봅니다. (차량" -"은 꺼져 있어야 합니다)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QR 코드 오류" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "제거" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "재설정" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "검토" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "재시작" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "장치 재시작" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "재시작 및 업데이트" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"시속 31mph(50km/h) 이상에서 방향지시등 없이 감지된 차선 밖으로 벗어나면 차선" -"으로 복귀하라는 경고를 받습니다." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "운전자 카메라 기록 및 업로드" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "마이크 오디오 기록 및 업로드" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"주행 중 마이크 오디오를 기록하고 저장합니다. 오디오는 comma connect의 대시캠 " -"영상에 포함됩니다." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "규제 정보" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "편안한" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "원격 액세스" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "원격 스냅샷" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "요청 시간이 초과되었습니다" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "재설정" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "캘리브레이션 재설정" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "학습 가이드 검토" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "openpilot의 규칙, 기능 및 제한을 검토" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "선택" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH 키" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Wi‑Fi 네트워크 검색 중..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "선택" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "브랜치 선택" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "언어 선택" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "시리얼" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "업데이트 나중에 알림" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "소프트웨어" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "표준" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"표준을 권장합니다. 공격적 모드에서는 앞차를 더 가깝게 따라가고 가감속이 더 적" -"극적입니다. 편안한 모드에서는 앞차와 거리를 더 둡니다. 지원 차량에서는 스티어" -"링의 차간 버튼으로 이 성향들을 전환할 수 있습니다." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "시스템 응답 없음" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "즉시 수동 조작하세요" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "온도" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "대상 브랜치" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "테더링 비밀번호" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "토글" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "제거" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "업데이트" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "제거" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "알수없음" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "업데이트는 차량 전원이 꺼져 있을 때만 다운로드됩니다." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "지금 업그레이드" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"운전자 방향 카메라 데이터를 업로드하여 운전자 모니터링 알고리즘 개선에 도움" -"을 주세요." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "미터법 사용" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"ACC 및 차선 유지 보조에 openpilot을 사용합니다. 이 기능을 사용할 때는 항상 주" -"의가 필요합니다." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "차량" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "보기" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "시작 대기 중" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"경고: 이는 GitHub 설정의 모든 공개 키에 SSH 액세스를 부여합니다. 자신의 것이 " -"아닌 GitHub 사용자 이름을 절대 입력하지 마세요. comma 직원이 본인의 GitHub 사" -"용자 이름 추가를 요구하는 일은 결코 없습니다." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "openpilot에 오신 것을 환영합니다" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "이 옵션을 켜면 가속 페달을 밟을 때 openpilot이 해제됩니다." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Wi‑Fi 네트워크 종량제" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "비밀번호가 올바르지 않습니다" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "openpilot을 사용하려면 약관에 동의해야 합니다." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"openpilot을 사용하려면 약관에 동의해야 합니다. 계속하기 전에 https://comma." -"ai/terms 에서 최신 약관을 읽어주세요." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "카메라 시작 중" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma 프라임" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "기본값" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "아래" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "업데이트 확인 실패" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "\"{}\"용" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "자동 구성을 사용하려면 비워 두세요" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "왼쪽" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "종량제" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "없음" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "지금" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot 롱컨 제어(알파)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot 사용 불가" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot은 기본적으로 안정적 모드로 주행합니다. 실험 모드를 사용하면 안정적 모드에 " -"아직 준비되지 않은 알파 수준의 기능이 활성화됩니다. 실험 기능은 아래와 같습니" -"다:

    엔드투엔드 롱컨 제어


    주행 모델이 가속과 제동을 제어합니" -"다. openpilot은 빨간 신호 및 정지 표지에서의 정지를 포함해 사람이 운전한다고 " -"판단하는 방식으로 주행합니다. 주행 속도는 모델이 결정하므로 설정 속도는 상한" -"으로만 동작합니다. 알파 품질 기능이므로 오작동이 발생할 수 있습니다.

    " -"새로운 주행 시각화


    저속에서는 도로 방향의 광각 카메라로 전환되어 일" -"부 회전을 더 잘 보여줍니다. 화면 오른쪽 위에는 실험 모드 로고도 표시됩니다." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilot은 지속적으로 보정을 진행하므로 재설정이 필요한 경우는 드뭅니다. 차" -"량 전원이 켜져 있을 때 보정을 재설정하면 openpilot이 재시작됩니다." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot은 당신과 같은 사람의 운전을 보며 운전을 학습합니다.\n" -"\n" -"Firehose 모드는 학습 데이터 업로드를 최대화하여 openpilot의 주행 모델을 개선" -"할 수 있게 해줍니다. 데이터가 많을수록 모델은 커지고, 실험 모드는 더 좋아집니" -"다." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilot 롱컨 제어는 향후 업데이트에서 제공될 수 있습니다." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "openpilot은 장치를 좌우 4°, 위쪽 5°, 아래쪽 9° 이내로 장착해야 합니다." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "오른쪽" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "비종량제" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "위" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "최신입니다. 마지막 확인: 없음" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "최신입니다. 마지막 확인: {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "업데이트 가능" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{}건의 알림" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{}일 전" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{}시간 전" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{}분 전" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "현재까지 귀하의 주행 {}구간이 학습 데이터셋에 포함되었습니다." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ 구독됨" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 파이어호스 모드 🔥" diff --git a/selfdrive/ui/translations/app_pt-BR.po b/selfdrive/ui/translations/app_pt-BR.po deleted file mode 100644 index 84b53c6e8d9428..00000000000000 --- a/selfdrive/ui/translations/app_pt-BR.po +++ /dev/null @@ -1,1220 +0,0 @@ -# Language pt-BR translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-21 00:00-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: pt-BR\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n > 1);\n" -"X-Language: pt_BR\n" -"X-Source-Language: C\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " A calibração da resposta de torque da direção foi concluída." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " A calibração da resposta de torque da direção está {}% concluída." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Seu dispositivo está apontado {:.1f}° {} e {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 ano de armazenamento de condução" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "Conectividade LTE 24/7" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"AVISO: o controle longitudinal do openpilot está em alpha para este carro " -"e desativará a Frenagem Automática de Emergência (AEB).

    Neste " -"carro, o openpilot usa por padrão o ACC integrado do carro em vez do " -"controle longitudinal do openpilot. Ative isto para alternar para o controle " -"longitudinal do openpilot. Recomenda-se ativar o Modo Experimental ao ativar " -"o controle longitudinal do openpilot em alpha." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    A calibração da latência da direção está concluída." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    A calibração da latência da direção está {}% concluída." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "ATIVO" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) permite conectar ao seu dispositivo via USB ou " -"pela rede. Veja https://docs.comma.ai/how-to/connect-to-comma para mais " -"informações." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "ADICIONAR" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "Configuração de APN" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Reconhecer Atuação Excessiva" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "Avançado" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Agressivo" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Concordo" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Monitoramento de Motorista Sempre Ativo" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"Uma versão alpha do controle longitudinal do openpilot pode ser testada, " -"junto com o Modo Experimental, em ramificações fora de release." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Tem certeza de que deseja desligar?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Tem certeza de que deseja reiniciar?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Tem certeza de que deseja redefinir a calibração?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Tem certeza de que deseja desinstalar?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Voltar" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Torne-se membro comma prime em connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "Adicione connect.comma.ai à tela inicial para usá-lo como um app" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "ALTERAR" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "VERIFICAR" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "MODO CHILL ATIVO" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONECTAR" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "CONECTANDO..." - -#: system/ui/widgets/confirm_dialog.py:23 -#: system/ui/widgets/option_dialog.py:35 system/ui/widgets/keyboard.py:81 -#: system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "Cancelar" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "Dados móveis limitados" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Alterar Idioma" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -"Alterar esta configuração reiniciará o openpilot se o carro estiver ligado." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "Toque em \"adicionar novo dispositivo\" e escaneie o QR code à direita" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Fechar" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Versão Atual" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "BAIXAR" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Recusar" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Recusar, desinstalar o openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Desenvolv" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Dispositivo" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Desativar ao pressionar o acelerador" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Desativar para Desligar" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Desativar para Reiniciar" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Desativar para Redefinir Calibração" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Exibir velocidade em km/h em vez de mph." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "ID do Dongle" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Baixar" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Câmera do Motorista" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Personalidade" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "EDITAR" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "ERRO" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "MODO EXPERIMENTAL ATIVO" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Ativar" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "Ativar ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Ativar alertas de saída de faixa" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "Ativar openpilot" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "Ativar SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Ativar alertas de saída de faixa" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "" -"Ativar monitoramento do motorista mesmo quando o openpilot não está engajado." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "Ativar openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Ative a opção de controle longitudinal do openpilot (alpha) para permitir o " -"Modo Experimental." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "Digite APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "Digite SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "Digite nova senha tethering" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "Digite a senha" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Digite seu nome de usuário do GitHub" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "Erro" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Modo Experimental" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"O Modo Experimental está indisponível neste carro pois o ACC original do " -"carro é usado para controle longitudinal." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "ESQUECENDO..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Configure" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Modo Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Para máxima efetividade, leve seu dispositivo para dentro e conecte a um bom " -"adaptador USB-C e Wi‑Fi semanalmente.\n" -"\n" -"O Modo Firehose também pode funcionar enquanto você dirige se estiver " -"conectado a um hotspot ou a um SIM ilimitado.\n" -"\n" -"\n" -"Perguntas Frequentes\n" -"\n" -"Importa como ou onde eu dirijo? Não, apenas dirija como normalmente.\n" -"\n" -"Todos os meus segmentos são puxados no Modo Firehose? Não, puxamos " -"seletivamente um subconjunto dos seus segmentos.\n" -"\n" -"Qual é um bom adaptador USB‑C? Qualquer carregador rápido de telefone ou " -"laptop serve.\n" -"\n" -"Importa qual software eu executo? Sim, apenas o openpilot upstream (e forks " -"específicos) podem ser usados para treinamento." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "Esquecer" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Esquecer rede Wi-Fi \"{}\"?" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "BOM" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Acesse https://connect.comma.ai no seu telefone" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "ALTO" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Rede" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "INATIVO: conecte a uma rede sem franquia" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "INSTALAR" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "Endereço IP" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Instalar Atualização" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Modo de Depuração do Joystick" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "CARREGANDO" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Modo de Manobra Longitudinal" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MÁX" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Maximize seus envios de dados de treinamento para melhorar os modelos de " -"condução do openpilot." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "N/A" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "NÃO" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Rede" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "Nenhuma chave SSH encontrada" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "Nenhuma chave SSH encontrada para o usuário '{username}'" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "Sem notas de versão disponíveis." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "OFFLINE" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "ONLINE" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Abrir" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "EMPARELHAR" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "PRÉVIA" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "RECURSOS PRIME:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Emparelhar Dispositivo" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Emparelhar dispositivo" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Emparelhe seu dispositivo à sua conta comma" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Emparelhe seu dispositivo com o comma connect (connect.comma.ai) e resgate " -"sua oferta comma prime." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Conecte-se ao Wi‑Fi para concluir o emparelhamento inicial" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Desligar" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "Evitar uploads grandes de dados em conexões Wi-Fi limitadas" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "Evitar uploads grandes de dados em conexões móveis limitadas" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Pré-visualize a câmera voltada para o motorista para garantir que o " -"monitoramento do motorista tenha boa visibilidade. (veículo deve estar " -"desligado)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "Erro no QR Code" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "REMOVER" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "REDEFINIR" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "REVISAR" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Reiniciar" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Reiniciar Dispositivo" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Reiniciar e Atualizar" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Receba alertas para voltar à faixa quando seu veículo cruzar uma linha de " -"faixa detectada sem seta ativada ao dirigir acima de 31 mph (50 km/h)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Gravar e Enviar Câmera do Motorista" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Gravar e Enviar Áudio do Microfone" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Grave e armazene o áudio do microfone enquanto dirige. O áudio será incluído " -"no vídeo da dashcam no comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Regulatório" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Relaxado" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Acesso remoto" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Capturas remotas" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Tempo da solicitação esgotado" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Redefinir" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Redefinir Calibração" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Revisar Guia de Treinamento" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "Revise as regras, recursos e limitações do openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "SELECIONAR" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "Chaves SSH" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Procurando redes Wi-Fi..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "Selecione" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "Selecione uma branch" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Selecione um idioma" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Serial" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Adiar Atualização" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Software" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Padrão" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Padrão é recomendado. No modo agressivo, o openpilot seguirá veículos à " -"frente mais de perto e será mais agressivo com acelerador e freio. No modo " -"relaxado, o openpilot ficará mais longe dos veículos à frente. Em carros " -"compatíveis, você pode alternar essas personalidades com o botão de " -"distância do volante." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "Sistema sem resposta" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "ASSUMA O CONTROLE IMEDIATAMENTE" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMP" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "Branch Alvo" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "Senha Tethering" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Toggles" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "DESINSTALAR" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "ATUALIZAR" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Desinstalar" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Desconhecido" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Atualizações são baixadas apenas com o carro desligado." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Atualizar Agora" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Envie dados da câmera voltada para o motorista e ajude a melhorar o " -"algoritmo de monitoramento do motorista." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Usar Sistema Métrico" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Use o sistema openpilot para controle de cruzeiro adaptativo e assistência " -"de permanência em faixa. Sua atenção é necessária o tempo todo para usar " -"este recurso." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "VEÍCULO" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "VER" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Aguardando para iniciar" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Aviso: Isso concede acesso SSH a todas as chaves públicas nas suas " -"configurações do GitHub. Nunca informe um nome de usuário do GitHub que não " -"seja o seu. Um funcionário da comma NUNCA pedirá para você adicionar o nome " -"de usuário do GitHub dele." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Bem-vindo ao openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" -"Quando ativado, pressionar o pedal do acelerador desengajará o openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Rede Wi-Fi limitada" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "Senha errada" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "Você deve aceitar os Termos e Condições para usar o openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"Você deve aceitar os Termos e Condições para usar o openpilot. Leia os " -"termos mais recentes em https://comma.ai/terms antes de continuar." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "câmera iniciando" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "default" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "para baixo" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "falha ao verificar atualização" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "para \"{}\"" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "deixe em branco para configuração automática" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "à esquerda" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "limitados" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "nunca" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "agora" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "Controle Longitudinal do openpilot (Alpha)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot Indisponível" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables " -"alpha-level features that aren't ready for chill mode. Experimental features " -"are listed below:

    End-to-End Longitudinal Control


    Let the " -"driving model control the gas and brakes. openpilot will drive as it thinks " -"a human would, including stopping for red lights and stop signs. Since the " -"driving model decides the speed to drive, the set speed will only act as an " -"upper bound. This is an alpha quality feature; mistakes should be " -"expected.

    New Driving Visualization


    The driving visualization " -"will transition to the road-facing wide-angle camera at low speeds to better " -"show some turns. The Experimental mode logo will also be shown in the top " -"right corner." -msgstr "" -"o openpilot dirige por padrão no modo chill. O Modo Experimental habilita " -"recursos em nível alpha que não estão prontos para o modo chill. Os recursos " -"experimentais são listados abaixo:

    Controle Longitudinal " -"End-to-End


    Permita que o modelo de condução controle o acelerador e " -"os freios. O openpilot dirigirá como acha que um humano faria, incluindo " -"parar em sinais e semáforos vermelhos. Como o modelo decide a velocidade, a " -"velocidade definida atuará apenas como limite superior. Este é um recurso de " -"qualidade alpha; erros devem ser esperados.

    Nova Visualização de " -"Condução


    A visualização de condução mudará para a câmera " -"grande-angular voltada para a estrada em baixas velocidades para mostrar " -"melhor algumas curvas. O logotipo do Modo Experimental também será exibido " -"no canto superior direito." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"O openpilot está continuamente calibrando, resetar é raramente solicitado. " -"Alterar esta configuração reiniciará o openpilot se o carro estiver ligado." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"o openpilot aprende a dirigir observando humanos, como você, dirigirem.\n" -"\n" -"O Modo Firehose permite maximizar seus envios de dados de treinamento para " -"melhorar os modelos de condução do openpilot. Mais dados significam modelos " -"maiores, o que significa um Modo Experimental melhor." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "" -"o controle longitudinal do openpilot pode vir em uma atualização futura." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"o openpilot requer que o dispositivo seja montado dentro de 4° para a " -"esquerda ou direita e dentro de 5° para cima ou 9° para baixo." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "à direita" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "ilimitados" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "para cima" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "atualizado, última verificação: nunca" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "atualizado, última verificação: {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "atualização disponível" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} ALERTA" -msgstr[1] "{} ALERTAS" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} dia atrás" -msgstr[1] "{} dias atrás" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} hora atrás" -msgstr[1] "{} horas atrás" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} minuto atrás" -msgstr[1] "{} minutos atrás" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -"{} segmento da sua condução está no conjunto de treinamento até agora." -msgstr[1] "" -"{} segmentos da sua condução estão no conjunto de treinamento até agora." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ ASSINADO" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Modo Firehose 🔥" diff --git a/selfdrive/ui/translations/app_th.po b/selfdrive/ui/translations/app_th.po deleted file mode 100644 index f2e56f2882c6a1..00000000000000 --- a/selfdrive/ui/translations/app_th.po +++ /dev/null @@ -1,1129 +0,0 @@ -# Thai translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: th\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=1; plural=0;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "" - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "" - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -msgstr[1] "" - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "" diff --git a/selfdrive/ui/translations/app_tr.po b/selfdrive/ui/translations/app_tr.po deleted file mode 100644 index 10191234a1ff72..00000000000000 --- a/selfdrive/ui/translations/app_tr.po +++ /dev/null @@ -1,1210 +0,0 @@ -# Turkish translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-20 18:19-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: tr\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=2; plural=(n != 1);\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " Direksiyon tork tepkisi kalibrasyonu tamamlandı." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " Direksiyon tork tepkisi kalibrasyonu {}% tamamlandı." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Cihazınız {:.1f}° {} ve {:.1f}° {} yönünde konumlandırılmış." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 yıl sürüş depolaması" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "7/24 LTE bağlantısı" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"UYARI: Bu araç için openpilot boylamsal kontrolü alfa aşamasındadır ve " -"Otomatik Acil Frenlemeyi (AEB) devre dışı bırakacaktır.

    Bu araçta " -"openpilot, openpilot'un boylamsal kontrolü yerine aracın yerleşik ACC'sini " -"varsayılan olarak kullanır. openpilot boylamsal kontrolüne geçmek için bunu " -"etkinleştirin. openpilot boylamsal kontrol alfayı etkinleştirirken Deneysel " -"modu etkinleştirmeniz önerilir." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "AKTİF" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge), cihazınıza USB veya ağ üzerinden bağlanmayı " -"sağlar. Daha fazla bilgi için https://docs.comma.ai/how-to/connect-to-comma " -"adresine bakın." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "EKLE" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Aşırı Müdahaleyi Onayla" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Agresif" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Kabul et" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Sürekli Sürücü İzleme" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"openpilot boylamsal kontrolünün alfa sürümü, Deneysel mod ile birlikte, " -"yayın dışı dallarda test edilebilir." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Kapatmak istediğinizden emin misiniz?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Yeniden başlatmak istediğinizden emin misiniz?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Kalibrasyonu sıfırlamak istediğinizden emin misiniz?" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Kaldırmak istediğinizden emin misiniz?" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Geri" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "connect.comma.ai adresinde comma prime üyesi olun" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" -"connect.comma.ai'yi ana ekranınıza ekleyerek bir uygulama gibi kullanın" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "DEĞİŞTİR" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "KONTROL ET" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "CHILL MODU AÇIK" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "BAĞLAN" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "BAĞLAN" - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Dili Değiştir" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -" Bu ayarı değiştirmek, araç çalışıyorsa openpilot'u yeniden başlatacaktır." - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "\"yeni cihaz ekle\"ye tıklayın ve sağdaki QR kodunu tarayın" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Kapat" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Geçerli Sürüm" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "İNDİR" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Reddet" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Reddet, openpilot'u kaldır" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "Geliştirici" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "Cihaz" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Gaz Pedalında Devreden Çık" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Kapatmak için Devreden Çıkın" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Yeniden Başlatmak için Devreden Çıkın" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Kalibrasyonu Sıfırlamak için Devreden Çıkın" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Hızı mph yerine km/h olarak göster." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "Dongle ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "İndir" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Sürücü Kamerası" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Sürüş Kişiliği" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "HATA" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "DENEYSEL MOD AÇIK" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "Etkinleştir" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "ADB'yi Etkinleştir" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Şerit Terk Uyarılarını Etkinleştir" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "openpilot'u etkinleştir" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "SSH'yi Etkinleştir" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Şerit Terk Uyarılarını Etkinleştir" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "openpilot devrede değilken bile sürücü izlemesini etkinleştir." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "openpilot'u etkinleştir" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Deneysel modu etkinleştirmek için openpilot boylamsal kontrolünü (alfa) açın." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "GitHub kullanıcı adınızı girin" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Deneysel Mod" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"Bu araçta boylamsal kontrol için stok ACC kullanıldığından şu anda Deneysel " -"mod kullanılamıyor." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "" - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Kurulumu Bitir" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehose Modu" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Maksimum verim için cihazınızı içeri alın ve haftalık olarak iyi bir USB-C " -"adaptörüne ve Wi‑Fi'a bağlayın.\n" -"\n" -"Firehose Modu, bir hotspot'a veya sınırsız SIM karta bağlıyken sürüş " -"sırasında da çalışabilir.\n" -"\n" -"\n" -"Sıkça Sorulan Sorular\n" -"\n" -"Nasıl veya nerede sürdüğüm önemli mi? Hayır, normalde nasıl sürüyorsanız " -"öyle sürün.\n" -"\n" -"Firehose Modu'nda tüm segmentlerim çekiliyor mu? Hayır, segmentlerinizin bir " -"alt kümesini seçerek çekiyoruz.\n" -"\n" -"İyi bir USB‑C adaptörü nedir? Hızlı bir telefon veya dizüstü şarj cihazı " -"uygundur.\n" -"\n" -"Hangi yazılımı çalıştırdığım önemli mi? Evet, yalnızca upstream openpilot " -"(ve bazı fork'lar) eğitim için kullanılabilir." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "İYİ" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Telefonunuzda https://connect.comma.ai adresine gidin" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "YÜKSEK" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Ağ" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "PASİF: sınırsız bir ağa bağlanın" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "YÜKLE" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Güncellemeyi Yükle" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Joystick Hata Ayıklama Modu" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "YÜKLENİYOR" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Boylamsal Manevra Modu" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "MAKS" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"openpilot'un sürüş modellerini iyileştirmek için eğitim veri yüklemelerinizi " -"en üst düzeye çıkarın." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "HAYIR" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "Ağ" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "SSH anahtarı bulunamadı" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "'{username}' için SSH anahtarı bulunamadı" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "Sürüm notu mevcut değil." - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "ÇEVRİMDIŞI" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "ÇEVRİMİÇİ" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "Aç" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "EŞLE" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "ÖNİZLEME" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "PRIME ÖZELLİKLERİ:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Cihazı Eşle" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Cihazı eşle" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Cihazınızı comma hesabınızla eşleştirin" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Cihazınızı comma connect (connect.comma.ai) ile eşleştirin ve comma prime " -"teklifinizi alın." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "İlk eşleştirmeyi tamamlamak için lütfen Wi‑Fi'a bağlanın" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Kapat" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Sürücü izleme görünürlüğünün iyi olduğundan emin olmak için sürücüye bakan " -"kamerayı önizleyin. (araç kapalı olmalıdır)" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QR Kod Hatası" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "KALDIR" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "SIFIRLA" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "GÖZDEN GEÇİR" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Yeniden Başlat" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Cihazı Yeniden Başlat" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Yeniden Başlat ve Güncelle" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Araç 31 mph (50 km/h) üzerindeyken sinyal verilmeden algılanan şerit " -"çizgisini aştığınızda şeride geri dönmeniz için uyarılar alın." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Sürücü Kamerasını Kaydet ve Yükle" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Mikrofon Sesini Kaydet ve Yükle" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Sürüş sırasında mikrofon sesini kaydedip saklayın. Ses, comma connect'teki " -"ön kamera videosuna dahil edilecektir." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Mevzuat" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Rahat" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Uzaktan erişim" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Uzaktan anlık görüntüler" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "İstek zaman aşımına uğradı" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Sıfırla" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Kalibrasyonu Sıfırla" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Eğitim Kılavuzunu İncele" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "" -"openpilot'un kurallarını, özelliklerini ve sınırlamalarını gözden geçirin" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Bir dil seçin" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Seri" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Güncellemeyi Ertele" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "Yazılım" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Standart" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Standart önerilir. Agresif modda openpilot öndeki aracı daha yakından takip " -"eder ve gaz/fren kullanımında daha ataktır. Rahat modda openpilot öndeki " -"araçlardan daha uzak durur. Desteklenen araçlarda bu kişilikler arasında " -"direksiyon mesafe düğmesiyle geçiş yapabilirsiniz." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "Sistem Yanıt Vermiyor" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "HEMEN KONTROLÜ DEVRALIN" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "TEMP" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "Seçenekler" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "KALDIR" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "GÜNCELLE" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "Kaldır" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Bilinmiyor" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Güncellemeler yalnızca araç kapalıyken indirilir." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Şimdi Yükselt" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Sürücüye bakan kameradan veri yükleyin ve sürücü izleme algoritmasını " -"geliştirmeye yardımcı olun." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Metrik Sistemi Kullan" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Uyarlanabilir hız sabitleyici ve şerit koruma sürücü yardımında openpilot " -"sistemini kullanın. Bu özelliği kullanırken her zaman dikkatli olmanız " -"gerekir." - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "ARAÇ" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "GÖRÜNTÜLE" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Başlatma bekleniyor" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Uyarı: Bu, GitHub ayarlarınızdaki tüm açık anahtarlara SSH erişimi verir. " -"Kendi adınız dışında asla bir GitHub kullanıcı adı girmeyin. Bir comma " -"çalışanı sizden asla GitHub kullanıcı adlarını eklemenizi İSTEMEZ." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "openpilot'a hoş geldiniz" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "" -"Etkinleştirildiğinde, gaz pedalına basmak openpilot'u devreden çıkarır." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "openpilot'u kullanmak için Şartlar ve Koşulları kabul etmelisiniz." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"openpilot'u kullanmak için Şartlar ve Koşulları kabul etmelisiniz. Devam " -"etmeden önce en güncel şartları https://comma.ai/terms adresinde okuyun." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "kamera başlatılıyor" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "aşağı" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "güncelleme kontrolü başarısız" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "sol" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "asla" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "şimdi" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot Boylamsal Kontrol (Alfa)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot Kullanılamıyor" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot varsayılan olarak chill modunda sürer. Deneysel mod, chill moduna " -"hazır olmayan alfa seviyesindeki özellikleri etkinleştirir. Deneysel " -"özellikler aşağıda listelenmiştir:

    Uçtan Uca Boylamsal Kontrol
    Sürüş modelinin gaz ve frenleri kontrol etmesine izin verin. " -"openpilot, kırmızı ışıklarda ve dur işaretlerinde durmak dahil, bir insan " -"nasıl sürer diye düşündüğüne göre sürer. Hızı sürüş modeli belirlediğinden, " -"ayarlanan hız yalnızca üst sınır olarak işlev görür. Bu bir alfa kalitesinde " -"özelliktir; hatalar beklenmelidir.

    Yeni Sürüş Görselleştirmesi
    Sürüş görselleştirmesi, düşük hızlarda bazı dönüşleri daha iyi " -"göstermek için yola bakan geniş açılı kameraya geçer. Deneysel mod logosu " -"sağ üst köşede de gösterilecektir." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -" Bu ayarı değiştirmek, araç çalışıyorsa openpilot'u yeniden başlatacaktır." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot, sizin gibi insanların nasıl sürdüğünü izleyerek sürmeyi öğrenir.\n" -"\n" -"Firehose Modu, openpilot'un sürüş modellerini geliştirmek için eğitim veri " -"yüklemelerinizi en üst düzeye çıkarmanıza olanak tanır. Daha fazla veri, " -"daha büyük modeller demektir; bu da daha iyi Deneysel Mod anlamına gelir." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilot boylamsal kontrolü gelecekteki bir güncellemede gelebilir." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"openpilot, cihazın sağa/sola 4° ve yukarı 5° veya aşağı 9° içinde monte " -"edilmesini gerektirir." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "sağ" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "yukarı" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "güncel, son kontrol asla" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "güncel, son kontrol {}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "güncelleme mevcut" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} UYARI" -msgstr[1] "{} UYARILAR" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} gün önce" -msgstr[1] "{} gün önce" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} saat önce" -msgstr[1] "{} saat önce" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} dakika önce" -msgstr[1] "{} dakika önce" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "{} segment sürüşünüz eğitim veri setinde." -msgstr[1] "{} segment sürüşünüz eğitim veri setinde." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ ABONE" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehose Modu 🔥" diff --git a/selfdrive/ui/translations/app_uk.po b/selfdrive/ui/translations/app_uk.po deleted file mode 100644 index cf78fb5a330d05..00000000000000 --- a/selfdrive/ui/translations/app_uk.po +++ /dev/null @@ -1,1258 +0,0 @@ -# Ukrainian translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: \n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-11-19 12:21+0200\n" -"PO-Revision-Date: 2025-11-19 13:27+0200\n" -"Last-Translator: KeeFeeRe \n" -"Language-Team: none\n" -"Language: uk\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=3; plural=(n%10==1 && n%100!=11 ? 0 : n%10>=2 && " -"n%10<=4 && (n%100<10 || n%100>=20) ? 1 : 2);\n" -"X-Generator: Poedit 3.8\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " Калібрування реакції крутного моменту керма завершено." - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr "Калібрування реакції крутного моменту керма завершено на {}%." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " Ваш пристрій нахилено на {:.1f}° {} та {:.1f}° {}." - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 рік зберігання поїздок" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "Підключення LTE 24/7" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"ПОПЕРЕДЖЕННЯ: поздовжнє керування openpilot для цього автомобіля знаходиться " -"в стадії альфа-тестування і вимкне автоматичне екстрене гальмування (AEB)." - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    Калібрування затримки кермування завершено." - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    Калібрування затримки кермування завершено на {}%." - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "АКТИВНИЙ" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) дозволяє підключатися до вашого пристрою через " -"USB або мережу. Дивіться https://docs.comma.ai/how-to/connect-to-comma для " -"отримання додаткової інформації." - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "ДОДАТИ" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "Налаштування APN" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "Визнайте надмірне спрацьовування" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "Розширені" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "Агресивн." - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "Погодитися" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "Постійний моніторинг водія" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "" -"Альфа-версію поздовжнього керування openpilot можна протестувати разом з " -"експериментальним режимом на нерелізних гілках." - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "Ви впевнені, що хочете вимкнути?" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "Ви впевнені, що хочете перезавантажити?" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "Ви впевнені, що хочете скинути калібрування?" - -#: selfdrive/ui/layouts/settings/software.py:171 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "Ви впевнені, що хочете видалити?" - -#: system/ui/widgets/network.py:99 -#: selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "Назад" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "Станьте членом comma prime на connect.comma.ai" - -#: selfdrive/ui/widgets/pairing_dialog.py:119 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "" -"Додайте connect.comma.ai до головного екрану, щоб використовувати його як " -"додаток." - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "ЗМІНИТИ" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:115 -#: selfdrive/ui/layouts/settings/software.py:126 -#: selfdrive/ui/layouts/settings/software.py:155 -#, python-format -msgid "CHECK" -msgstr "ПЕРЕВІРИТИ" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "СПОКІЙНИЙ РЕЖИМ" - -#: system/ui/widgets/network.py:155 -#: selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 -#: selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECT" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "ПІДКЛЮЧА..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/network.py:318 system/ui/widgets/keyboard.py:81 -#, python-format -msgid "Cancel" -msgstr "Скасувати" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "Лімітне стільникове з'єднання" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "Змінити мову" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "" -"Зміна цього параметра призведе до перезапуску openpilot, якщо автомобіль " -"увімкнено." - -#: selfdrive/ui/widgets/pairing_dialog.py:118 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "Натисніть «додати новий пристрій» і відскануйте QR-код праворуч." - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "Закрити" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "Поточна версія" - -#: selfdrive/ui/layouts/settings/software.py:118 -#, python-format -msgid "DOWNLOAD" -msgstr "ВАНТАЖ" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "Відхилити" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "Відхилити, видалити openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Developer" -msgstr "Розробник" - -#: selfdrive/ui/layouts/settings/settings.py:59 -msgid "Device" -msgstr "Пристрій" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "Вимкнення при натисканні на педаль газу" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "Вимкніть openpilot, щоб вимкнути пристрій" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "Вимкніть openpilot, щоб перезавантажити" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "Деактивуйте для скидання калібрування" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "Відображати швидкість у км/год замість миль/год." - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "ID ключа" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "Завантажити" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "Камера водія" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "Стиль водіння" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "РЕДАГ." - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "ПОМИЛКА" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "ЕКСПЕРИМЕНТ. РЕЖИМ" - -#: selfdrive/ui/layouts/settings/toggles.py:228 -#: selfdrive/ui/layouts/settings/developer.py:166 -#, python-format -msgid "Enable" -msgstr "Увімкнути" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "Увімкнути ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "Увімкнути попередження про виїзд зі смуги" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "Увімкнути роумінг" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "Увімкнути SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "Увімкнути точку доступу" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "Увімкнути моніторинг водія, навіть коли openpilot не ввімкнено." - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "Увімкнути openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "" -"Увімкніть перемикач поздовжнього керування openpilot (альфа), щоб увімкнути " -"експериментальний режим." - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "Введіть APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "Введіть SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "Введіть новий пароль для модему" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "Введіть пароль" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "Введіть ваш логін GitHub" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "Помилка" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "Експериментальний режим" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "" -"Експериментальний режим наразі недоступний для цього автомобіля, оскільки " -"для поздовжнього керування використовується штатний адаптивний круїз-" -"контроль (ACC)." - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "ЗАБУВАЮ..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "Завершити налаштування" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Firehose" -msgstr "Злива" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Режим зливи" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"Для максимальної ефективності щотижня заносьте пристрій у приміщення та " -"підключайте його до якісного адаптера USB-C і Wi-Fi.\n" -"\n" -"Режим Зливи також може працювати під час руху, якщо пристрій підключено до " -"точки доступу або SIM-картки з необмеженим трафіком.\n" -"\n" -"\n" -"Поширені запитання\n" -"\n" -"Чи має значення, як і де я їду? Ні, просто їдьте, як зазвичай.\n" -"\n" -"Чи всі мої сегменти потрапляють у режим Зливи? Ні, ми вибірково вибираємо " -"підмножину ваших сегментів.\n" -"\n" -"Що таке хороший адаптер USB-C? Будь-який швидкий зарядний пристрій для " -"телефону або ноутбука підійде.\n" -"\n" -"Чи має значення, яке програмне забезпечення я використовую? Так, для " -"навчання можна використовувати тільки upstream openpilot (і певні його " -"форки)." - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "Заб-и" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "Забути мережу Wi-Fi \"{}\"?" - -#: selfdrive/ui/layouts/sidebar.py:71 -#: selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "ДОБРА" - -#: selfdrive/ui/widgets/pairing_dialog.py:117 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "Перейдіть на сайт https://connect.comma.ai на своєму телефоні." - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "ВИСОКА" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "Прихована мережа" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "НЕАКТИВНО: підключення до мережі без ліміту трафіку" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:144 -#, python-format -msgid "INSTALL" -msgstr "ВСТАНОВ." - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP-адреса" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "Встановити оновлення" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "Режим зневадження джойстика" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "ЗАВАНТАЖЕННЯ" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "Режим поздовжнього маневрування" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "МАКС" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "" -"Максимізуйте завантаження навчальних даних, щоб поліпшити моделі openpilot." - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "Н/Д" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "НЕМАЄ" - -#: selfdrive/ui/layouts/settings/settings.py:60 -msgid "Network" -msgstr "Мережа" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "Не знайдено ключів SSH" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "Користувач '{}' не має ключів на GitHub" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "Інформація про випуск відсутня." - -#: selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "ОФЛАЙН" - -#: system/ui/widgets/confirm_dialog.py:93 system/ui/widgets/html_render.py:263 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "OK" - -#: selfdrive/ui/layouts/sidebar.py:72 -#: selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "ОНЛАЙН" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "ВІДКРИТИ" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "ПІДКЛЮЧИТИ" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "ПОКАЖИ" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "XАРАКТЕРИСТИКИ PRIME:" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "Підключити пристрій" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "Підключити пристрій" - -#: selfdrive/ui/widgets/pairing_dialog.py:92 -#, python-format -msgid "Pair your device to your comma account" -msgstr "Підключіть свій пристрій до обліковки comma connect" - -#: selfdrive/ui/widgets/setup.py:48 -#: selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"Підключіть свій пристрій до comma connect (connect.comma.ai) і отримайте " -"свою пропозицію comma prime." - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "Будь ласка, підключіться до Wi-Fi, щоб завершити початкове сполучення." - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "Вимкнути" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "" -"Запобігайте завантаженню великих обсягів даних під час використання Wi-Fi-" -"з'єднання з обмеженим трафіком" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "" -"Запобігати великим завантаженням даних під час лімітного стільникового " -"з'єднання" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "" -"Попередньо перегляньте камеру, спрямовану на водія, щоб переконатися, що " -"система моніторингу водія має добру видимість. (автомобіль повинен бути " -"вимкнений)" - -#: selfdrive/ui/widgets/pairing_dialog.py:150 -#, python-format -msgid "QR Code Error" -msgstr "Помилка QR-коду" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "ВИДАЛИТИ" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "Скинути" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "ДИВИТИСЬ" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "Перезавантажити" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "Перезавантажте пристрій" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "Перезавантажити та оновити" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"Отримувати попередження про необхідність повернутися в смугу, коли ваш " -"автомобіль перетинає виявлену лінію розмітки без увімкненого сигналу " -"повороту під час руху зі швидкістю понад 31 миль/год (50 км/год)." - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "Писати та вантажити відео з камери водія" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "Запис та завантаження аудіо з мікрофона" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"Записуйте та зберігайте аудіо з мікрофона під час руху. Аудіо буде включено " -"до відео з відеореєстратора в comma connect." - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "Нормативні документи" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "Спокійний" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "Віддалений доступ" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "Віддалені знімки" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "Час запиту вичерпано" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "Скинути" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "Скинути калібрування" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "Переглянути посібник з навчання" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "Перегляньте правила, функції та обмеження openpilot" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "ВИБРАТИ" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH ключі" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "Пошук мереж..." - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "Вибрати" - -#: selfdrive/ui/layouts/settings/software.py:191 -#, python-format -msgid "Select a branch" -msgstr "Виберіть гілку" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "Виберіть мову" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "Серійний номер" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "Відкласти оновлення" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Software" -msgstr "Програма" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "Стандарт" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"Рекомендується стандартний режим. В агресивному режимі openpilot буде " -"триматися ближче до автомобілів попереду і більш агресивно використовувати " -"газ і гальма. У спокійному режимі openpilot буде триматися на більшій " -"відстані від автомобілів попереду. На підтримуваних автомобілях ви можете " -"перемикатися між цими режимами за допомогою кнопки дистанції на кермі." - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "Система не реагує" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "КЕРМУЙТЕ НЕГАЙНО" - -#: selfdrive/ui/layouts/sidebar.py:71 -#: selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "ТЕМП" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "Цільова гілка" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "Пароль для точки доступу" - -#: selfdrive/ui/layouts/settings/settings.py:61 -msgid "Toggles" -msgstr "Перемикачі" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "ВИДАЛИТИ" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "ОНОВИТИ" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:171 -#, python-format -msgid "Uninstall" -msgstr "Видалити" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "Невідомо" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "Оновлення завантажуються лише тоді, коли автомобіль вимкнено." - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "Оновити зараз" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "" -"Завантажуйте дані з камери, спрямованої на водія, та допоможіть покращити " -"алгоритм моніторингу водія." - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "Використовувати метричну систему" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"Використовуйте систему openpilot для адаптивного круїз-контролю та допомоги " -"в утриманні смуги руху. Ваша увага потрібна постійно при використанні цієї " -"функції. Зміна цього налаштування набуває чинності після вимкнення живлення " -"автомобіля." - -#: selfdrive/ui/layouts/sidebar.py:72 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "АВТО" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "ДИВИСЬ" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "Очікування початку" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"Попередження: це надає доступ по SSH до всіх публічних ключів у ваших " -"налаштуваннях GitHub. Ніколи не вводьте ім'я користувача GitHub, окрім " -"вашого власного. Співробітник comma НІКОЛИ не попросить вас додати його ім'я " -"користувача GitHub." - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "Ласкаво просимо до openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "Якщо увімкнено, натискання на педаль акселератора вимкне openpilot." - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi-Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Трафік Wi-Fi" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "Невірний пароль" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "Ви повинні прийняти Умови та положення, щоб користуватися openpilot." - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"Ви повинні прийняти Умови використання, щоб користуватися openpilot. Перед " -"тим, як продовжити, ознайомтеся з останніми умовами на сайті https://" -"comma.ai/terms." - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "запуск камери" - -#: selfdrive/ui/layouts/settings/software.py:105 -#, python-format -msgid "checking..." -msgstr "перевіряю..." - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "замовч." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "вниз" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "downloading..." -msgstr "завантажую..." - -#: selfdrive/ui/layouts/settings/software.py:114 -#, python-format -msgid "failed to check for update" -msgstr "не вдалося перевірити оновлення" - -#: selfdrive/ui/layouts/settings/software.py:107 -#, python-format -msgid "finalizing update..." -msgstr "завершую..." - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "для \"{}\"" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "км/год" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "залиште порожнім для автоматичного налаштування" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "вліво" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "обмеж." - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "миль/год" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "ніколи" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "зараз" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "Поздовжнє керування openpilot (Альфа)" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot Недоступний" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot за замовчуванням працює в режимі спокій. Експериментальний режим " -"увімкне функції альфа-рівня, які ще не готові для режиму спокій. " -"Експериментальні функції перелічені нижче:

    Кінцевий поздовжній " -"контроль


    Дозвольте моделі водіння контролювати газ і гальма. " -"openpilot буде керувати автомобілем так, як це робив би людина, включаючи " -"зупинку на червоне світло і знаки зупинки. Оскільки модель водіння визначає " -"швидкість руху, задана швидкість буде діяти лише як верхня межа. Це функція " -"альфа-рівня; слід очікувати помилок.

    Нова візуалізація водіння
    Візуалізація водіння перейде на ширококутну камеру, спрямовану на " -"дорогу, при низьких швидкостях, щоб краще показувати деякі повороти. Логотип " -"експериментального режиму також буде показаний у верхньому правому куті." - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilot постійно калібрується, скидання рідко потрібне. Скидання " -"калібрування призведе до перезапуску openpilot, якщо автомобіль увімкнено." - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot вчиться керувати автомобілем, спостерігаючи за тим, як це роблять " -"люди, такі як ви.\n" -"\n" -"Режим зливи дозволяє максимально збільшити обсяг завантажуваних навчальних " -"даних, щоб поліпшити моделі керування автомобілем openpilot. Більше даних " -"означає більші моделі, а це означає кращий експериментальний режим." - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "Поздовжнє керування openpilot може з'явитися в майбутньому оновленні." - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "" -"Для роботи openpilot потрібно, щоб пристрій був встановлений з нахилом не " -"більше 4° вліво або вправо та не більше 5° вгору або 9° вниз. openpilot " -"постійно калібрується, тому скидання калібрування потрібне рідко." - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "вправо" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "необмеж." - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "вгору" - -#: selfdrive/ui/layouts/settings/software.py:125 -#, python-format -msgid "up to date, last checked never" -msgstr "оновлено, ніколи не перевірялось" - -#: selfdrive/ui/layouts/settings/software.py:123 -#, python-format -msgid "up to date, last checked {}" -msgstr "оновлено, перевірив {}" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "update available" -msgstr "доступне оновлення" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} СПОВІЩЕННЯ" -msgstr[1] "{} СПОВІЩЕННЯ" -msgstr[2] "{} СПОВІЩЕНЬ" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} день тому" -msgstr[1] "{} дні тому" -msgstr[2] "{} днів тому" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} година тому" -msgstr[1] "{} години тому" -msgstr[2] "{} годин тому" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} хвилина тому" -msgstr[1] "{} хвилини тому" -msgstr[2] "{} хвилин тому" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "" -"{} сегмент вашого водіння на даний момент містяться в тренувальному наборі " -"даних." -msgstr[1] "" -"{} сегменти вашого водіння на даний момент містяться в тренувальному наборі " -"даних." -msgstr[2] "" -"{} сегментів вашого водіння на даний момент містяться в тренувальному наборі " -"даних." - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ ПІДПИСАНО" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🌧️ Режим зливи 🌧️" diff --git a/selfdrive/ui/translations/app_zh-CHS.po b/selfdrive/ui/translations/app_zh-CHS.po deleted file mode 100644 index 2400b6f44aa097..00000000000000 --- a/selfdrive/ui/translations/app_zh-CHS.po +++ /dev/null @@ -1,1174 +0,0 @@ -# Language zh-CHS translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: zh-CHS\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=1; plural=0;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " 转向扭矩响应校准完成。" - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " 转向扭矩响应校准已完成 {}%。" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " 您的设备朝向 {:.1f}° {} 与 {:.1f}° {}。" - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 年行驶数据存储" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "全天候 LTE 连接" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"警告：此车型的 openpilot 纵向控制仍为 alpha，将会停用自动紧急制动 (AEB)。" -"

    在此车型上，openpilot 默认使用车载 ACC，而非 openpilot 的纵向控" -"制。启用此选项可切换为 openpilot 纵向控制。建议同时启用实验模式。若车辆通电，" -"更改此设置将会重启 openpilot。" - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    转向延迟校准完成。" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    转向延迟校准已完成 {}%。" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "已启用" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB（Android 调试桥）可通过 USB 或网络连接到您的设备。详见 https://docs." -"comma.ai/how-to/connect-to-comma。" - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "添加" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN 设置" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "确认过度作动" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "高级" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "激进" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "同意" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "始终启用驾驶员监控" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "openpilot 纵向控制的 alpha 版本可在非发布分支搭配实验模式进行测试。" - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "确定要关机吗？" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "确定要重启吗？" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "确定要重置校准吗？" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "确定要卸载吗？" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "返回" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "前往 connect.comma.ai 成为 comma prime 会员" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "将 connect.comma.ai 添加到主屏幕，像应用一样使用" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "更改" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "检查" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "安稳模式已开启" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECT" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "连接中..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "取消" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "蜂窝计量" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "更改语言" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "若车辆通电，更改此设置将重启 openpilot。" - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "点击“添加新设备”，扫描右侧二维码" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "关闭" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "当前版本" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "下载" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "拒绝" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "拒绝并卸载 openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "开发者" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "设备" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "踩下加速踏板时脱离" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "脱离以关机" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "脱离以重启" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "脱离以重置校准" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "以 km/h 显示速度（非 mph）。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "Dongle ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "下载" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "车内摄像头" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "驾驶风格" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "编辑" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "错误" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "实验模式已开启" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "启用" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "启用 ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "启用车道偏离警示" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "启用漫游" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "启用 SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "启用网络共享" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "即使未启用 openpilot 也启用驾驶员监控。" - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "启用 openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "启用 openpilot 纵向控制（alpha）开关，以使用实验模式。" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "输入 APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "输入 SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "输入新的网络共享密码" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "输入密码" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "输入您的 GitHub 用户名" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "错误" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "实验模式" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "此车型当前无法使用实验模式，因为纵向控制使用的是原厂 ACC。" - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "正在遗忘..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "完成设置" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehose 模式" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"为达到最佳效果，请将设备带到室内，并每周连接优质 USB‑C 充电器与 Wi‑Fi。\n" -"\n" -"若连接热点或不限流量卡，行车中也可使用 Firehose 模式。\n" -"\n" -"\n" -"常见问题\n" -"\n" -"我怎么开、在哪开有区别吗？没有，平常怎么开就怎么开。\n" -"\n" -"Firehose 模式会拉取我所有片段吗？不会，我们会选择性拉取部分片段。\n" -"\n" -"什么是好的 USB‑C 充电器？任何快速的手机或笔电充电器都可以。\n" -"\n" -"我跑什么软件有区别吗？有，只有上游 openpilot（及特定分支）可用于训练。" - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "忘记" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "要忘记 Wi‑Fi 网络“{}”吗？" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "良好" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "在手机上前往 https://connect.comma.ai" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "高" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "隐藏网络" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "未启用：请连接不限流量网络" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "安装" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP 地址" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "安装更新" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "摇杆调试模式" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "加载中" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "纵向操作模式" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "最大" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "最大化上传训练数据，以改进 openpilot 的驾驶模型。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "无" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "否" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "网络" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "未找到 SSH 密钥" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "未找到用户“{}”的 SSH 密钥" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "暂无发行说明。" - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "离线" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "确定" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "在线" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "打开" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "配对" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "预览" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "PRIME 功能：" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "配对设备" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "配对设备" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "将设备配对到您的 comma 账号" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"将设备与 comma connect（connect.comma.ai）配对，领取您的 comma prime 优惠。" - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "请连接 Wi‑Fi 以完成初始配对" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "关机" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "在计量制 Wi‑Fi 连接时避免大量上传" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "在计量制蜂窝网络时避免大量上传" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "预览车内摄像头以确保驾驶员监控视野良好。（车辆必须熄火）" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "二维码错误" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "移除" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "重置" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "查看" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "重启" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "重启设备" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "重启并更新" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"当车辆以超过 31 mph（50 km/h）行驶且未打转向灯越过检测到的车道线时，接收引导" -"回车道的警报。" - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "录制并上传车内摄像头" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "录制并上传麦克风音频" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"行驶时录制并保存麦克风音频。音频将包含在 comma connect 的行车记录视频中。" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "法规" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "从容" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "远程访问" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "远程快照" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "请求超时" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "重置" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "重置校准" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "查看训练指南" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "查看 openpilot 的规则、功能与限制" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "选择" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH 密钥" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "正在扫描 Wi‑Fi 网络…" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "选择" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "选择分支" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "选择语言" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "序列号" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "延后更新" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "软件" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "标准" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"建议使用标准模式。激进模式下，openpilot 会更贴近前车，油门与刹车更为激进；从" -"容模式下，会与前车保持更远距离。在支持的车型上，可用方向盘距离按钮切换这些风" -"格。" - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "系统无响应" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "请立即接管控制" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "温度" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "目标分支" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "网络共享密码" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "切换" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "卸载" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "更新" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "卸载" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "未知" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "仅在车辆熄火时下载更新。" - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "立即升级" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "上传车内摄像头数据，帮助改进驾驶员监控算法。" - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "使用公制" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"使用 openpilot 进行自适应巡航与车道保持辅助。使用此功能时，您必须始终保持专" -"注。" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "车辆" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "查看" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "等待开始" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"警告：这将授予对您 GitHub 设置中所有公钥的 SSH 访问权限。请勿输入非您本人的 " -"GitHub 用户名。comma 员工绝不会要求您添加他们的用户名。" - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "欢迎使用 openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "启用后，踩下加速踏板将会脱离 openpilot。" - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Wi‑Fi 计量网络" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "密码错误" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "您必须接受条款与条件才能使用 openpilot。" - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"您必须接受条款与条件才能使用 openpilot。继续前请阅读 https://comma.ai/terms " -"上的最新条款。" - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "相机启动中" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "默认" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "下" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "检查更新失败" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "用于“{}”" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "留空以自动配置" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "左" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "计量" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "从不" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "现在" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot 纵向控制（Alpha）" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot 无法使用" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot 默认以安稳模式行驶。实验模式会启用尚未准备好用于安稳模式的 Alpha 级" -"功能。实验功能如下：

    端到端纵向控制


    让驾驶模型控制油门与刹车。" -"openpilot 会像人类一样驾驶，包括在红灯与停牌前停车。由于驾驶模型决定行驶速" -"度，设定速度仅作为上限。这是 Alpha 质量功能；预期会有错误。

    全新驾驶可" -"视化


    在低速时，驾驶可视化将切换至面向道路的广角摄像头以更好显示部分转" -"弯。右上角也会显示实验模式图标。" - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilot 持续进行校准，通常无需重置。若车辆通电，重置校准将会重启 " -"openpilot。" - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot 通过观察人类（例如您）的驾驶来学习。\n" -"\n" -"Firehose 模式可让您最大化上传训练数据，以改进 openpilot 的驾驶模型。更多数据" -"意味着更大的模型，也意味着更好的实验模式。" - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilot 纵向控制可能会在未来更新中提供。" - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "openpilot 要求设备安装在左右 4°、上 5° 或下 9° 以内。" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "右" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "不限流量" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "上" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "已是最新，最后检查：从未" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "已是最新，最后检查：{}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "有可用更新" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} 条警报" -msgstr[1] "{} 条警报" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} 天前" -msgstr[1] "{} 天前" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} 小时前" -msgstr[1] "{} 小时前" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} 分钟前" -msgstr[1] "{} 分钟前" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "目前已有 {} 个您的驾驶片段被纳入训练数据集。" -msgstr[1] "目前已有 {} 个您的驾驶片段被纳入训练数据集。" - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ 已订阅" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehose 模式 🔥" diff --git a/selfdrive/ui/translations/app_zh-CHT.po b/selfdrive/ui/translations/app_zh-CHT.po deleted file mode 100644 index f4d5e0a4ed2e0c..00000000000000 --- a/selfdrive/ui/translations/app_zh-CHT.po +++ /dev/null @@ -1,1173 +0,0 @@ -# Language zh-CHT translations for PACKAGE package. -# Copyright (C) 2025 THE PACKAGE'S COPYRIGHT HOLDER -# This file is distributed under the same license as the PACKAGE package. -# Automatically generated, 2025. -# -msgid "" -msgstr "" -"Project-Id-Version: PACKAGE VERSION\n" -"Report-Msgid-Bugs-To: \n" -"POT-Creation-Date: 2025-10-23 00:50-0700\n" -"PO-Revision-Date: 2025-10-22 16:32-0700\n" -"Last-Translator: Automatically generated\n" -"Language-Team: none\n" -"Language: zh-CHT\n" -"MIME-Version: 1.0\n" -"Content-Type: text/plain; charset=UTF-8\n" -"Content-Transfer-Encoding: 8bit\n" -"Plural-Forms: nplurals=1; plural=0;\n" - -#: selfdrive/ui/layouts/settings/device.py:160 -#, python-format -msgid " Steering torque response calibration is complete." -msgstr " 轉向扭矩回應校正完成。" - -#: selfdrive/ui/layouts/settings/device.py:158 -#, python-format -msgid " Steering torque response calibration is {}% complete." -msgstr " 轉向扭矩回應校正已完成 {}%。" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid " Your device is pointed {:.1f}° {} and {:.1f}° {}." -msgstr " 您的裝置朝向 {:.1f}° {} 與 {:.1f}° {}。" - -#: selfdrive/ui/layouts/sidebar.py:43 -msgid "--" -msgstr "--" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "1 year of drive storage" -msgstr "1 年行駛資料儲存" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "24/7 LTE connectivity" -msgstr "全年無休 LTE 連線" - -#: selfdrive/ui/layouts/sidebar.py:46 -msgid "2G" -msgstr "2G" - -#: selfdrive/ui/layouts/sidebar.py:47 -msgid "3G" -msgstr "3G" - -#: selfdrive/ui/layouts/sidebar.py:49 -msgid "5G" -msgstr "5G" - -#: selfdrive/ui/layouts/settings/developer.py:23 -msgid "" -"WARNING: openpilot longitudinal control is in alpha for this car and will " -"disable Automatic Emergency Braking (AEB).

    On this car, openpilot " -"defaults to the car's built-in ACC instead of openpilot's longitudinal " -"control. Enable this to switch to openpilot longitudinal control. Enabling " -"Experimental mode is recommended when enabling openpilot longitudinal " -"control alpha. Changing this setting will restart openpilot if the car is " -"powered on." -msgstr "" -"警告：此車款的 openpilot 縱向控制仍為 alpha，將會停用自動緊急煞車 (AEB)。" -"

    在此車款上，openpilot 預設使用車載 ACC，而非 openpilot 的縱向控" -"制。啟用此選項可切換為 openpilot 縱向控制。建議同時啟用實驗模式。若車輛通電，" -"變更此設定將會重新啟動 openpilot。" - -#: selfdrive/ui/layouts/settings/device.py:148 -#, python-format -msgid "

    Steering lag calibration is complete." -msgstr "

    轉向延遲校正完成。" - -#: selfdrive/ui/layouts/settings/device.py:146 -#, python-format -msgid "

    Steering lag calibration is {}% complete." -msgstr "

    轉向延遲校正已完成 {}%。" - -#: selfdrive/ui/layouts/settings/firehose.py:138 -#, python-format -msgid "ACTIVE" -msgstr "啟用" - -#: selfdrive/ui/layouts/settings/developer.py:15 -msgid "" -"ADB (Android Debug Bridge) allows connecting to your device over USB or over " -"the network. See https://docs.comma.ai/how-to/connect-to-comma for more info." -msgstr "" -"ADB (Android Debug Bridge) 可透過 USB 或網路連線至您的裝置。詳見 https://" -"docs.comma.ai/how-to/connect-to-comma。" - -#: selfdrive/ui/widgets/ssh_key.py:30 -msgid "ADD" -msgstr "新增" - -#: system/ui/widgets/network.py:139 -#, python-format -msgid "APN Setting" -msgstr "APN 設定" - -#: selfdrive/ui/widgets/offroad_alerts.py:109 -#, python-format -msgid "Acknowledge Excessive Actuation" -msgstr "確認過度作動" - -#: system/ui/widgets/network.py:74 system/ui/widgets/network.py:95 -#, python-format -msgid "Advanced" -msgstr "進階" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Aggressive" -msgstr "積極" - -#: selfdrive/ui/layouts/onboarding.py:116 -#, python-format -msgid "Agree" -msgstr "同意" - -#: selfdrive/ui/layouts/settings/toggles.py:70 -#, python-format -msgid "Always-On Driver Monitoring" -msgstr "持續啟用駕駛監控" - -#: selfdrive/ui/layouts/settings/toggles.py:186 -#, python-format -msgid "" -"An alpha version of openpilot longitudinal control can be tested, along with " -"Experimental mode, on non-release branches." -msgstr "openpilot 縱向控制的 alpha 版本可於非發行分支搭配實驗模式進行測試。" - -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Are you sure you want to power off?" -msgstr "確定要關機嗎？" - -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Are you sure you want to reboot?" -msgstr "確定要重新啟動嗎？" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Are you sure you want to reset calibration?" -msgstr "確定要重設校正嗎？" - -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Are you sure you want to uninstall?" -msgstr "確定要解除安裝嗎？" - -#: system/ui/widgets/network.py:99 selfdrive/ui/layouts/onboarding.py:147 -#, python-format -msgid "Back" -msgstr "返回" - -#: selfdrive/ui/widgets/prime.py:38 -#, python-format -msgid "Become a comma prime member at connect.comma.ai" -msgstr "前往 connect.comma.ai 成為 comma prime 會員" - -#: selfdrive/ui/widgets/pairing_dialog.py:130 -#, python-format -msgid "Bookmark connect.comma.ai to your home screen to use it like an app" -msgstr "將 connect.comma.ai 加到主畫面，像 App 一樣使用" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "CHANGE" -msgstr "變更" - -#: selfdrive/ui/layouts/settings/software.py:50 -#: selfdrive/ui/layouts/settings/software.py:107 -#: selfdrive/ui/layouts/settings/software.py:118 -#: selfdrive/ui/layouts/settings/software.py:147 -#, python-format -msgid "CHECK" -msgstr "檢查" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "CHILL MODE ON" -msgstr "安穩模式已開啟" - -#: system/ui/widgets/network.py:155 selfdrive/ui/layouts/sidebar.py:73 -#: selfdrive/ui/layouts/sidebar.py:134 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:138 -#, python-format -msgid "CONNECT" -msgstr "CONNECT" - -#: system/ui/widgets/network.py:369 -#, python-format -msgid "CONNECTING..." -msgstr "連線中..." - -#: system/ui/widgets/confirm_dialog.py:23 system/ui/widgets/option_dialog.py:35 -#: system/ui/widgets/keyboard.py:81 system/ui/widgets/network.py:318 -#, python-format -msgid "Cancel" -msgstr "取消" - -#: system/ui/widgets/network.py:134 -#, python-format -msgid "Cellular Metered" -msgstr "行動網路計量" - -#: selfdrive/ui/layouts/settings/device.py:68 -#, python-format -msgid "Change Language" -msgstr "變更語言" - -#: selfdrive/ui/layouts/settings/toggles.py:125 -#, python-format -msgid "Changing this setting will restart openpilot if the car is powered on." -msgstr "若車輛通電，變更此設定將重新啟動 openpilot。" - -#: selfdrive/ui/widgets/pairing_dialog.py:129 -#, python-format -msgid "Click \"add new device\" and scan the QR code on the right" -msgstr "點選「新增裝置」，掃描右側 QR 碼" - -#: selfdrive/ui/widgets/offroad_alerts.py:104 -#, python-format -msgid "Close" -msgstr "關閉" - -#: selfdrive/ui/layouts/settings/software.py:49 -#, python-format -msgid "Current Version" -msgstr "目前版本" - -#: selfdrive/ui/layouts/settings/software.py:110 -#, python-format -msgid "DOWNLOAD" -msgstr "下載" - -#: selfdrive/ui/layouts/onboarding.py:115 -#, python-format -msgid "Decline" -msgstr "拒絕" - -#: selfdrive/ui/layouts/onboarding.py:148 -#, python-format -msgid "Decline, uninstall openpilot" -msgstr "拒絕並解除安裝 openpilot" - -#: selfdrive/ui/layouts/settings/settings.py:67 -msgid "Developer" -msgstr "開發人員" - -#: selfdrive/ui/layouts/settings/settings.py:62 -msgid "Device" -msgstr "裝置" - -#: selfdrive/ui/layouts/settings/toggles.py:58 -#, python-format -msgid "Disengage on Accelerator Pedal" -msgstr "踩下加速踏板時脫離" - -#: selfdrive/ui/layouts/settings/device.py:184 -#, python-format -msgid "Disengage to Power Off" -msgstr "脫離以關機" - -#: selfdrive/ui/layouts/settings/device.py:172 -#, python-format -msgid "Disengage to Reboot" -msgstr "脫離以重新啟動" - -#: selfdrive/ui/layouts/settings/device.py:103 -#, python-format -msgid "Disengage to Reset Calibration" -msgstr "脫離以重設校正" - -#: selfdrive/ui/layouts/settings/toggles.py:32 -msgid "Display speed in km/h instead of mph." -msgstr "以 km/h 顯示速度（非 mph）。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#, python-format -msgid "Dongle ID" -msgstr "Dongle ID" - -#: selfdrive/ui/layouts/settings/software.py:50 -#, python-format -msgid "Download" -msgstr "下載" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "Driver Camera" -msgstr "車內鏡頭" - -#: selfdrive/ui/layouts/settings/toggles.py:96 -#, python-format -msgid "Driving Personality" -msgstr "駕駛風格" - -#: system/ui/widgets/network.py:123 system/ui/widgets/network.py:139 -#, python-format -msgid "EDIT" -msgstr "編輯" - -#: selfdrive/ui/layouts/sidebar.py:138 -msgid "ERROR" -msgstr "錯誤" - -#: selfdrive/ui/layouts/sidebar.py:45 -msgid "ETH" -msgstr "ETH" - -#: selfdrive/ui/widgets/exp_mode_button.py:50 -#, python-format -msgid "EXPERIMENTAL MODE ON" -msgstr "實驗模式已開啟" - -#: selfdrive/ui/layouts/settings/developer.py:166 -#: selfdrive/ui/layouts/settings/toggles.py:228 -#, python-format -msgid "Enable" -msgstr "啟用" - -#: selfdrive/ui/layouts/settings/developer.py:39 -#, python-format -msgid "Enable ADB" -msgstr "啟用 ADB" - -#: selfdrive/ui/layouts/settings/toggles.py:64 -#, python-format -msgid "Enable Lane Departure Warnings" -msgstr "啟用偏離車道警示" - -#: system/ui/widgets/network.py:129 -#, python-format -msgid "Enable Roaming" -msgstr "啟用漫遊" - -#: selfdrive/ui/layouts/settings/developer.py:48 -#, python-format -msgid "Enable SSH" -msgstr "啟用 SSH" - -#: system/ui/widgets/network.py:120 -#, python-format -msgid "Enable Tethering" -msgstr "啟用網路共享" - -#: selfdrive/ui/layouts/settings/toggles.py:30 -msgid "Enable driver monitoring even when openpilot is not engaged." -msgstr "即使未啟動 openpilot 亦啟用駕駛監控。" - -#: selfdrive/ui/layouts/settings/toggles.py:46 -#, python-format -msgid "Enable openpilot" -msgstr "啟用 openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:189 -#, python-format -msgid "" -"Enable the openpilot longitudinal control (alpha) toggle to allow " -"Experimental mode." -msgstr "啟用 openpilot 縱向控制（alpha）切換，以使用實驗模式。" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "Enter APN" -msgstr "輸入 APN" - -#: system/ui/widgets/network.py:241 -#, python-format -msgid "Enter SSID" -msgstr "輸入 SSID" - -#: system/ui/widgets/network.py:254 -#, python-format -msgid "Enter new tethering password" -msgstr "輸入新的網路共享密碼" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "Enter password" -msgstr "輸入密碼" - -#: selfdrive/ui/widgets/ssh_key.py:89 -#, python-format -msgid "Enter your GitHub username" -msgstr "輸入您的 GitHub 使用者名稱" - -#: system/ui/widgets/list_view.py:123 system/ui/widgets/list_view.py:160 -#, python-format -msgid "Error" -msgstr "錯誤" - -#: selfdrive/ui/layouts/settings/toggles.py:52 -#, python-format -msgid "Experimental Mode" -msgstr "實驗模式" - -#: selfdrive/ui/layouts/settings/toggles.py:181 -#, python-format -msgid "" -"Experimental mode is currently unavailable on this car since the car's stock " -"ACC is used for longitudinal control." -msgstr "此車款目前無法使用實驗模式，因為縱向控制使用的是原廠 ACC。" - -#: system/ui/widgets/network.py:373 -#, python-format -msgid "FORGETTING..." -msgstr "正在遺忘..." - -#: selfdrive/ui/widgets/setup.py:44 -#, python-format -msgid "Finish Setup" -msgstr "完成設定" - -#: selfdrive/ui/layouts/settings/settings.py:66 -msgid "Firehose" -msgstr "Firehose" - -#: selfdrive/ui/layouts/settings/firehose.py:18 -msgid "Firehose Mode" -msgstr "Firehose 模式" - -#: selfdrive/ui/layouts/settings/firehose.py:25 -msgid "" -"For maximum effectiveness, bring your device inside and connect to a good " -"USB-C adapter and Wi-Fi weekly.\n" -"\n" -"Firehose Mode can also work while you're driving if connected to a hotspot " -"or unlimited SIM card.\n" -"\n" -"\n" -"Frequently Asked Questions\n" -"\n" -"Does it matter how or where I drive? Nope, just drive as you normally " -"would.\n" -"\n" -"Do all of my segments get pulled in Firehose Mode? No, we selectively pull a " -"subset of your segments.\n" -"\n" -"What's a good USB-C adapter? Any fast phone or laptop charger should be " -"fine.\n" -"\n" -"Does it matter which software I run? Yes, only upstream openpilot (and " -"particular forks) are able to be used for training." -msgstr "" -"為達最佳效果，請將裝置帶到室內，並每週連接優質 USB‑C 充電器與 Wi‑Fi。\n" -"\n" -"若連上熱點或吃到飽門號，行車中也可使用 Firehose 模式。\n" -"\n" -"\n" -"常見問題\n" -"\n" -"我怎麼開、在哪裡開有差嗎？沒有，平常怎麼開就怎麼開。\n" -"\n" -"Firehose 模式會拉取我所有片段嗎？不會，我們會選擇性拉取部分片段。\n" -"\n" -"什麼是好的 USB‑C 充電器？任何快速的手機或筆電充電器都可以。\n" -"\n" -"我跑什麼軟體有差嗎？有，只有上游 openpilot（及特定分支）可用於訓練。" - -#: system/ui/widgets/network.py:318 system/ui/widgets/network.py:451 -#, python-format -msgid "Forget" -msgstr "忘記" - -#: system/ui/widgets/network.py:319 -#, python-format -msgid "Forget Wi-Fi Network \"{}\"?" -msgstr "要忘記 Wi‑Fi 網路「{}」嗎？" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -msgid "GOOD" -msgstr "良好" - -#: selfdrive/ui/widgets/pairing_dialog.py:128 -#, python-format -msgid "Go to https://connect.comma.ai on your phone" -msgstr "在手機上前往 https://connect.comma.ai" - -#: selfdrive/ui/layouts/sidebar.py:129 -msgid "HIGH" -msgstr "高" - -#: system/ui/widgets/network.py:155 -#, python-format -msgid "Hidden Network" -msgstr "隱藏網路" - -#: selfdrive/ui/layouts/settings/firehose.py:140 -#, python-format -msgid "INACTIVE: connect to an unmetered network" -msgstr "未啟用：請連接不限流量網路" - -#: selfdrive/ui/layouts/settings/software.py:53 -#: selfdrive/ui/layouts/settings/software.py:136 -#, python-format -msgid "INSTALL" -msgstr "安裝" - -#: system/ui/widgets/network.py:150 -#, python-format -msgid "IP Address" -msgstr "IP 位址" - -#: selfdrive/ui/layouts/settings/software.py:53 -#, python-format -msgid "Install Update" -msgstr "安裝更新" - -#: selfdrive/ui/layouts/settings/developer.py:56 -#, python-format -msgid "Joystick Debug Mode" -msgstr "搖桿除錯模式" - -#: selfdrive/ui/widgets/ssh_key.py:29 -msgid "LOADING" -msgstr "載入中" - -#: selfdrive/ui/layouts/sidebar.py:48 -msgid "LTE" -msgstr "LTE" - -#: selfdrive/ui/layouts/settings/developer.py:64 -#, python-format -msgid "Longitudinal Maneuver Mode" -msgstr "縱向操作模式" - -#: selfdrive/ui/onroad/hud_renderer.py:148 -#, python-format -msgid "MAX" -msgstr "最大" - -#: selfdrive/ui/widgets/setup.py:75 -#, python-format -msgid "" -"Maximize your training data uploads to improve openpilot's driving models." -msgstr "最大化上傳訓練資料，以改進 openpilot 的駕駛模型。" - -#: selfdrive/ui/layouts/settings/device.py:59 -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "N/A" -msgstr "無" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "NO" -msgstr "否" - -#: selfdrive/ui/layouts/settings/settings.py:63 -msgid "Network" -msgstr "網路" - -#: selfdrive/ui/widgets/ssh_key.py:114 -#, python-format -msgid "No SSH keys found" -msgstr "找不到 SSH 金鑰" - -#: selfdrive/ui/widgets/ssh_key.py:126 -#, python-format -msgid "No SSH keys found for user '{}'" -msgstr "找不到使用者 '{}' 的 SSH 金鑰" - -#: selfdrive/ui/widgets/offroad_alerts.py:320 -#, python-format -msgid "No release notes available." -msgstr "無可用發行說明。" - -#: selfdrive/ui/layouts/sidebar.py:73 selfdrive/ui/layouts/sidebar.py:134 -msgid "OFFLINE" -msgstr "離線" - -#: system/ui/widgets/html_render.py:263 system/ui/widgets/confirm_dialog.py:93 -#: selfdrive/ui/layouts/sidebar.py:127 -#, python-format -msgid "OK" -msgstr "確定" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:136 -#: selfdrive/ui/layouts/sidebar.py:144 -msgid "ONLINE" -msgstr "線上" - -#: selfdrive/ui/widgets/setup.py:20 -#, python-format -msgid "Open" -msgstr "開啟" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "PAIR" -msgstr "配對" - -#: selfdrive/ui/layouts/sidebar.py:142 -msgid "PANDA" -msgstr "PANDA" - -#: selfdrive/ui/layouts/settings/device.py:62 -#, python-format -msgid "PREVIEW" -msgstr "預覽" - -#: selfdrive/ui/widgets/prime.py:44 -#, python-format -msgid "PRIME FEATURES:" -msgstr "PRIME 功能：" - -#: selfdrive/ui/layouts/settings/device.py:48 -#, python-format -msgid "Pair Device" -msgstr "配對裝置" - -#: selfdrive/ui/widgets/setup.py:19 -#, python-format -msgid "Pair device" -msgstr "配對裝置" - -#: selfdrive/ui/widgets/pairing_dialog.py:103 -#, python-format -msgid "Pair your device to your comma account" -msgstr "將裝置配對至您的 comma 帳號" - -#: selfdrive/ui/widgets/setup.py:48 selfdrive/ui/layouts/settings/device.py:24 -#, python-format -msgid "" -"Pair your device with comma connect (connect.comma.ai) and claim your comma " -"prime offer." -msgstr "" -"將裝置與 comma connect（connect.comma.ai）配對，領取您的 comma prime 優惠。" - -#: selfdrive/ui/widgets/setup.py:91 -#, python-format -msgid "Please connect to Wi-Fi to complete initial pairing" -msgstr "請連線至 Wi‑Fi 以完成初始化配對" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:187 -#, python-format -msgid "Power Off" -msgstr "關機" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Prevent large data uploads when on a metered Wi-Fi connection" -msgstr "在計量制 Wi‑Fi 連線時避免大量上傳" - -#: system/ui/widgets/network.py:135 -#, python-format -msgid "Prevent large data uploads when on a metered cellular connection" -msgstr "在計量制行動網路時避免大量上傳" - -#: selfdrive/ui/layouts/settings/device.py:25 -msgid "" -"Preview the driver facing camera to ensure that driver monitoring has good " -"visibility. (vehicle must be off)" -msgstr "預覽車內鏡頭以確保駕駛監控視野良好。（車輛須熄火）" - -#: selfdrive/ui/widgets/pairing_dialog.py:161 -#, python-format -msgid "QR Code Error" -msgstr "QR 碼錯誤" - -#: selfdrive/ui/widgets/ssh_key.py:31 -msgid "REMOVE" -msgstr "移除" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "RESET" -msgstr "重設" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "REVIEW" -msgstr "檢視" - -#: selfdrive/ui/layouts/settings/device.py:55 -#: selfdrive/ui/layouts/settings/device.py:175 -#, python-format -msgid "Reboot" -msgstr "重新啟動" - -#: selfdrive/ui/onroad/alert_renderer.py:66 -#, python-format -msgid "Reboot Device" -msgstr "重新啟動裝置" - -#: selfdrive/ui/widgets/offroad_alerts.py:112 -#, python-format -msgid "Reboot and Update" -msgstr "重新啟動並更新" - -#: selfdrive/ui/layouts/settings/toggles.py:27 -msgid "" -"Receive alerts to steer back into the lane when your vehicle drifts over a " -"detected lane line without a turn signal activated while driving over 31 mph " -"(50 km/h)." -msgstr "" -"當車輛以超過 31 mph（50 km/h）行駛且未打方向燈越過偵測到的車道線時，接收轉向" -"回車道的警示。" - -#: selfdrive/ui/layouts/settings/toggles.py:76 -#, python-format -msgid "Record and Upload Driver Camera" -msgstr "錄製並上傳車內鏡頭" - -#: selfdrive/ui/layouts/settings/toggles.py:82 -#, python-format -msgid "Record and Upload Microphone Audio" -msgstr "錄製並上傳麥克風音訊" - -#: selfdrive/ui/layouts/settings/toggles.py:33 -msgid "" -"Record and store microphone audio while driving. The audio will be included " -"in the dashcam video in comma connect." -msgstr "" -"行車時錄製並儲存麥克風音訊。音訊將包含在 comma connect 的行車紀錄影片中。" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "Regulatory" -msgstr "法規" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Relaxed" -msgstr "從容" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote access" -msgstr "遠端存取" - -#: selfdrive/ui/widgets/prime.py:47 -#, python-format -msgid "Remote snapshots" -msgstr "遠端擷圖" - -#: selfdrive/ui/widgets/ssh_key.py:123 -#, python-format -msgid "Request timed out" -msgstr "要求逾時" - -#: selfdrive/ui/layouts/settings/device.py:119 -#, python-format -msgid "Reset" -msgstr "重設" - -#: selfdrive/ui/layouts/settings/device.py:51 -#, python-format -msgid "Reset Calibration" -msgstr "重設校正" - -#: selfdrive/ui/layouts/settings/device.py:65 -#, python-format -msgid "Review Training Guide" -msgstr "檢視訓練指南" - -#: selfdrive/ui/layouts/settings/device.py:27 -msgid "Review the rules, features, and limitations of openpilot" -msgstr "檢視 openpilot 的規則、功能與限制" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "SELECT" -msgstr "選取" - -#: selfdrive/ui/layouts/settings/developer.py:53 -#, python-format -msgid "SSH Keys" -msgstr "SSH 金鑰" - -#: system/ui/widgets/network.py:310 -#, python-format -msgid "Scanning Wi-Fi networks..." -msgstr "正在掃描 Wi‑Fi 網路…" - -#: system/ui/widgets/option_dialog.py:36 -#, python-format -msgid "Select" -msgstr "選取" - -#: selfdrive/ui/layouts/settings/software.py:183 -#, python-format -msgid "Select a branch" -msgstr "選取分支" - -#: selfdrive/ui/layouts/settings/device.py:91 -#, python-format -msgid "Select a language" -msgstr "選取語言" - -#: selfdrive/ui/layouts/settings/device.py:60 -#, python-format -msgid "Serial" -msgstr "序號" - -#: selfdrive/ui/widgets/offroad_alerts.py:106 -#, python-format -msgid "Snooze Update" -msgstr "延後更新" - -#: selfdrive/ui/layouts/settings/settings.py:65 -msgid "Software" -msgstr "軟體" - -#: selfdrive/ui/layouts/settings/toggles.py:98 -#, python-format -msgid "Standard" -msgstr "標準" - -#: selfdrive/ui/layouts/settings/toggles.py:22 -msgid "" -"Standard is recommended. In aggressive mode, openpilot will follow lead cars " -"closer and be more aggressive with the gas and brake. In relaxed mode " -"openpilot will stay further away from lead cars. On supported cars, you can " -"cycle through these personalities with your steering wheel distance button." -msgstr "" -"建議使用標準模式。積極模式下，openpilot 會更貼近前車，油門與煞車反應更積極；" -"從容模式下，會與前車保持更遠距離。於支援車款，可用方向盤距離按鈕切換這些風" -"格。" - -#: selfdrive/ui/onroad/alert_renderer.py:59 -#: selfdrive/ui/onroad/alert_renderer.py:65 -#, python-format -msgid "System Unresponsive" -msgstr "系統無回應" - -#: selfdrive/ui/onroad/alert_renderer.py:58 -#, python-format -msgid "TAKE CONTROL IMMEDIATELY" -msgstr "請立刻接手控制" - -#: selfdrive/ui/layouts/sidebar.py:71 selfdrive/ui/layouts/sidebar.py:125 -#: selfdrive/ui/layouts/sidebar.py:127 selfdrive/ui/layouts/sidebar.py:129 -msgid "TEMP" -msgstr "溫度" - -#: selfdrive/ui/layouts/settings/software.py:61 -#, python-format -msgid "Target Branch" -msgstr "目標分支" - -#: system/ui/widgets/network.py:124 -#, python-format -msgid "Tethering Password" -msgstr "網路共享密碼" - -#: selfdrive/ui/layouts/settings/settings.py:64 -msgid "Toggles" -msgstr "切換" - -#: selfdrive/ui/layouts/settings/software.py:72 -#, python-format -msgid "UNINSTALL" -msgstr "解除安裝" - -#: selfdrive/ui/layouts/home.py:155 -#, python-format -msgid "UPDATE" -msgstr "更新" - -#: selfdrive/ui/layouts/settings/software.py:72 -#: selfdrive/ui/layouts/settings/software.py:163 -#, python-format -msgid "Uninstall" -msgstr "解除安裝" - -#: selfdrive/ui/layouts/sidebar.py:117 -msgid "Unknown" -msgstr "未知" - -#: selfdrive/ui/layouts/settings/software.py:48 -#, python-format -msgid "Updates are only downloaded while the car is off." -msgstr "僅在車輛熄火時下載更新。" - -#: selfdrive/ui/widgets/prime.py:33 -#, python-format -msgid "Upgrade Now" -msgstr "立即升級" - -#: selfdrive/ui/layouts/settings/toggles.py:31 -msgid "" -"Upload data from the driver facing camera and help improve the driver " -"monitoring algorithm." -msgstr "上傳車內鏡頭資料，協助改善駕駛監控演算法。" - -#: selfdrive/ui/layouts/settings/toggles.py:88 -#, python-format -msgid "Use Metric System" -msgstr "使用公制" - -#: selfdrive/ui/layouts/settings/toggles.py:17 -msgid "" -"Use the openpilot system for adaptive cruise control and lane keep driver " -"assistance. Your attention is required at all times to use this feature." -msgstr "" -"使用 openpilot 進行 ACC 與車道維持輔助。使用此功能時，您必須始終保持專注。" - -#: selfdrive/ui/layouts/sidebar.py:72 selfdrive/ui/layouts/sidebar.py:144 -msgid "VEHICLE" -msgstr "車輛" - -#: selfdrive/ui/layouts/settings/device.py:67 -#, python-format -msgid "VIEW" -msgstr "檢視" - -#: selfdrive/ui/onroad/alert_renderer.py:52 -#, python-format -msgid "Waiting to start" -msgstr "等待開始" - -#: selfdrive/ui/layouts/settings/developer.py:19 -msgid "" -"Warning: This grants SSH access to all public keys in your GitHub settings. " -"Never enter a GitHub username other than your own. A comma employee will " -"NEVER ask you to add their GitHub username." -msgstr "" -"警告：這將授予對您 GitHub 設定中所有公開金鑰的 SSH 存取權。請勿輸入非您本人" -"的 GitHub 帳號。comma 員工絕不會要求您新增他們的帳號。" - -#: selfdrive/ui/layouts/onboarding.py:111 -#, python-format -msgid "Welcome to openpilot" -msgstr "歡迎使用 openpilot" - -#: selfdrive/ui/layouts/settings/toggles.py:20 -msgid "When enabled, pressing the accelerator pedal will disengage openpilot." -msgstr "啟用後，踩下加速踏板將會脫離 openpilot。" - -#: selfdrive/ui/layouts/sidebar.py:44 -msgid "Wi-Fi" -msgstr "Wi‑Fi" - -#: system/ui/widgets/network.py:144 -#, python-format -msgid "Wi-Fi Network Metered" -msgstr "Wi‑Fi 計量網路" - -#: system/ui/widgets/network.py:314 -#, python-format -msgid "Wrong password" -msgstr "密碼錯誤" - -#: selfdrive/ui/layouts/onboarding.py:145 -#, python-format -msgid "You must accept the Terms and Conditions in order to use openpilot." -msgstr "您必須接受條款與細則才能使用 openpilot。" - -#: selfdrive/ui/layouts/onboarding.py:112 -#, python-format -msgid "" -"You must accept the Terms and Conditions to use openpilot. Read the latest " -"terms at https://comma.ai/terms before continuing." -msgstr "" -"您必須接受條款與細則才能使用 openpilot。繼續前請閱讀 https://comma.ai/terms " -"上的最新條款。" - -#: selfdrive/ui/onroad/driver_camera_dialog.py:34 -#, python-format -msgid "camera starting" -msgstr "相機啟動中" - -#: selfdrive/ui/widgets/prime.py:63 -#, python-format -msgid "comma prime" -msgstr "comma prime" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "default" -msgstr "預設" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "down" -msgstr "下" - -#: selfdrive/ui/layouts/settings/software.py:106 -#, python-format -msgid "failed to check for update" -msgstr "檢查更新失敗" - -#: system/ui/widgets/network.py:237 system/ui/widgets/network.py:314 -#, python-format -msgid "for \"{}\"" -msgstr "適用於「{}」" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "km/h" -msgstr "km/h" - -#: system/ui/widgets/network.py:204 -#, python-format -msgid "leave blank for automatic configuration" -msgstr "留空以自動設定" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "left" -msgstr "左" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "metered" -msgstr "計量" - -#: selfdrive/ui/onroad/hud_renderer.py:177 -#, python-format -msgid "mph" -msgstr "mph" - -#: selfdrive/ui/layouts/settings/software.py:20 -#, python-format -msgid "never" -msgstr "從不" - -#: selfdrive/ui/layouts/settings/software.py:31 -#, python-format -msgid "now" -msgstr "現在" - -#: selfdrive/ui/layouts/settings/developer.py:71 -#, python-format -msgid "openpilot Longitudinal Control (Alpha)" -msgstr "openpilot 縱向控制（Alpha）" - -#: selfdrive/ui/onroad/alert_renderer.py:51 -#, python-format -msgid "openpilot Unavailable" -msgstr "openpilot 無法使用" - -#: selfdrive/ui/layouts/settings/toggles.py:158 -#, python-format -msgid "" -"openpilot defaults to driving in chill mode. Experimental mode enables alpha-" -"level features that aren't ready for chill mode. Experimental features are " -"listed below:

    End-to-End Longitudinal Control


    Let the driving " -"model control the gas and brakes. openpilot will drive as it thinks a human " -"would, including stopping for red lights and stop signs. Since the driving " -"model decides the speed to drive, the set speed will only act as an upper " -"bound. This is an alpha quality feature; mistakes should be expected." -"

    New Driving Visualization


    The driving visualization will " -"transition to the road-facing wide-angle camera at low speeds to better show " -"some turns. The Experimental mode logo will also be shown in the top right " -"corner." -msgstr "" -"openpilot 預設以安穩模式行駛。實驗模式啟用尚未準備好進入安穩模式的 Alpha 等級" -"功能。實驗功能如下：

    端到端縱向控制


    讓駕駛模型控制油門與煞車。" -"openpilot 會如同人類駕駛般行駛，包括在紅燈與停車標誌前停車。由於駕駛模型決定" -"行駛速度，設定速度僅作為上限。此為 Alpha 品質功能；預期會有失誤。

    全新" -"駕駛視覺化


    在低速時，駕駛視覺化將切換至面向道路的廣角鏡頭以更好呈現部" -"分轉彎。右上角亦會顯示實驗模式圖示。" - -#: selfdrive/ui/layouts/settings/device.py:165 -#, python-format -msgid "" -"openpilot is continuously calibrating, resetting is rarely required. " -"Resetting calibration will restart openpilot if the car is powered on." -msgstr "" -"openpilot 會持續校正，通常不需重設。若車輛通電，重設校正將重新啟動 " -"openpilot。" - -#: selfdrive/ui/layouts/settings/firehose.py:20 -msgid "" -"openpilot learns to drive by watching humans, like you, drive.\n" -"\n" -"Firehose Mode allows you to maximize your training data uploads to improve " -"openpilot's driving models. More data means bigger models, which means " -"better Experimental Mode." -msgstr "" -"openpilot 透過觀察人類（也就是您）的駕駛方式來學習。\n" -"\n" -"Firehose 模式可讓您最大化上傳訓練資料，以改進 openpilot 的駕駛模型。更多資料" -"代表更大的模型，也就代表更好的實驗模式。" - -#: selfdrive/ui/layouts/settings/toggles.py:183 -#, python-format -msgid "openpilot longitudinal control may come in a future update." -msgstr "openpilot 縱向控制可能於未來更新提供。" - -#: selfdrive/ui/layouts/settings/device.py:26 -msgid "" -"openpilot requires the device to be mounted within 4° left or right and " -"within 5° up or 9° down." -msgstr "openpilot 要求裝置安裝在左右 4°、上 5° 或下 9° 以內。" - -#: selfdrive/ui/layouts/settings/device.py:134 -#, python-format -msgid "right" -msgstr "右" - -#: system/ui/widgets/network.py:142 -#, python-format -msgid "unmetered" -msgstr "不限流量" - -#: selfdrive/ui/layouts/settings/device.py:133 -#, python-format -msgid "up" -msgstr "上" - -#: selfdrive/ui/layouts/settings/software.py:117 -#, python-format -msgid "up to date, last checked never" -msgstr "已為最新，最後檢查：從未" - -#: selfdrive/ui/layouts/settings/software.py:115 -#, python-format -msgid "up to date, last checked {}" -msgstr "已為最新，最後檢查：{}" - -#: selfdrive/ui/layouts/settings/software.py:109 -#, python-format -msgid "update available" -msgstr "有可用更新" - -#: selfdrive/ui/layouts/home.py:169 -#, python-format -msgid "{} ALERT" -msgid_plural "{} ALERTS" -msgstr[0] "{} 則警示" -msgstr[1] "{} 則警示" - -#: selfdrive/ui/layouts/settings/software.py:40 -#, python-format -msgid "{} day ago" -msgid_plural "{} days ago" -msgstr[0] "{} 天前" -msgstr[1] "{} 天前" - -#: selfdrive/ui/layouts/settings/software.py:37 -#, python-format -msgid "{} hour ago" -msgid_plural "{} hours ago" -msgstr[0] "{} 小時前" -msgstr[1] "{} 小時前" - -#: selfdrive/ui/layouts/settings/software.py:34 -#, python-format -msgid "{} minute ago" -msgid_plural "{} minutes ago" -msgstr[0] "{} 分鐘前" -msgstr[1] "{} 分鐘前" - -#: selfdrive/ui/layouts/settings/firehose.py:111 -#, python-format -msgid "{} segment of your driving is in the training dataset so far." -msgid_plural "{} segments of your driving is in the training dataset so far." -msgstr[0] "目前已有 {} 個您的駕駛片段納入訓練資料集。" -msgstr[1] "目前已有 {} 個您的駕駛片段納入訓練資料集。" - -#: selfdrive/ui/widgets/prime.py:62 -#, python-format -msgid "✓ SUBSCRIBED" -msgstr "✓ 已訂閱" - -#: selfdrive/ui/widgets/setup.py:22 -#, python-format -msgid "🔥 Firehose Mode 🔥" -msgstr "🔥 Firehose 模式 🔥" diff --git a/selfdrive/ui/translations/auto_translate.py b/selfdrive/ui/translations/auto_translate.py deleted file mode 100755 index 9354790f94f9e5..00000000000000 --- a/selfdrive/ui/translations/auto_translate.py +++ /dev/null @@ -1,138 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -import json -import os -import pathlib -import xml.etree.ElementTree as ET -from typing import cast - -import requests - -TRANSLATIONS_DIR = pathlib.Path(__file__).resolve().parent -TRANSLATIONS_LANGUAGES = TRANSLATIONS_DIR / "languages.json" - -OPENAI_MODEL = "gpt-4" -OPENAI_API_KEY = os.environ.get("OPENAI_API_KEY") -OPENAI_PROMPT = "You are a professional translator from English to {language} (ISO 639 language code). " + \ - "The following sentence or word is in the GUI of a software called openpilot, translate it accordingly." - - -def get_language_files(languages: list[str] | None = None) -> dict[str, pathlib.Path]: - files = {} - - with open(TRANSLATIONS_LANGUAGES) as fp: - language_dict = json.load(fp) - - for filename in language_dict.values(): - path = TRANSLATIONS_DIR / f"{filename}.ts" - language = path.stem - - if languages is None or language in languages: - files[language] = path - - return files - - -def translate_phrase(text: str, language: str) -> str: - response = requests.post( - "https://api.openai.com/v1/chat/completions", - json={ - "model": OPENAI_MODEL, - "messages": [ - { - "role": "system", - "content": OPENAI_PROMPT.format(language=language), - }, - { - "role": "user", - "content": text, - }, - ], - "temperature": 0.8, - "max_tokens": 1024, - "top_p": 1, - }, - headers={ - "Authorization": f"Bearer {OPENAI_API_KEY}", - "Content-Type": "application/json", - }, - ) - - if 400 <= response.status_code < 600: - raise requests.HTTPError(f'Error {response.status_code}: {response.json()}', response=response) - - data = response.json() - - return cast(str, data["choices"][0]["message"]["content"]) - - -def translate_file(path: pathlib.Path, language: str, all_: bool) -> None: - tree = ET.parse(path) - - root = tree.getroot() - - for context in root.findall("./context"): - name = context.find("name") - if name is None: - raise ValueError("name not found") - - print(f"Context: {name.text}") - - for message in context.findall("./message"): - source = message.find("source") - translation = message.find("translation") - - if source is None or translation is None: - raise ValueError("source or translation not found") - - if not all_ and translation.attrib.get("type") != "unfinished": - continue - - llm_translation = translate_phrase(cast(str, source.text), language) - - print(f"Source: {source.text}\n" + - f"Current translation: {translation.text}\n" + - f"LLM translation: {llm_translation}") - - translation.text = llm_translation - - with path.open("w", encoding="utf-8") as fp: - fp.write('\n' + - '\n' + - ET.tostring(root, encoding="utf-8").decode()) - - -def main(): - arg_parser = argparse.ArgumentParser("Auto translate") - - group = arg_parser.add_mutually_exclusive_group(required=True) - group.add_argument("-a", "--all-files", action="store_true", help="Translate all files") - group.add_argument("-f", "--file", nargs="+", help="Translate the selected files. (Example: -f fr de)") - - arg_parser.add_argument("-t", "--all-translations", action="store_true", default=False, help="Translate all sections. (Default: only unfinished)") - - args = arg_parser.parse_args() - - if OPENAI_API_KEY is None: - print("OpenAI API key is missing. (Hint: use `export OPENAI_API_KEY=YOUR-KEY` before you run the script).\n" + - "If you don't have one go to: https://beta.openai.com/account/api-keys.") - exit(1) - - files = get_language_files(None if args.all_files else args.file) - - if args.file: - missing_files = set(args.file) - set(files) - if len(missing_files): - print(f"No language files found: {missing_files}") - exit(1) - - print(f"Translation mode: {'all' if args.all_translations else 'only unfinished'}. Files: {list(files)}") - - for lang, path in files.items(): - print(f"Translate {lang} ({path})") - translate_file(path, lang, args.all_translations) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/ui/translations/create_badges.py b/selfdrive/ui/translations/create_badges.py index 2948c4857d8c65..32904f242aa407 100755 --- a/selfdrive/ui/translations/create_badges.py +++ b/selfdrive/ui/translations/create_badges.py @@ -2,106 +2,45 @@ import json import os import requests -import xml.etree.ElementTree as ET -from openpilot.common.basedir import BASEDIR -from openpilot.selfdrive.ui.update_translations import LANGUAGES_FILE, TRANSLATIONS_DIR +from common.basedir import BASEDIR +from selfdrive.ui.update_translations import LANGUAGES_FILE, TRANSLATIONS_DIR +TRANSLATION_TAG = " 2: - has_content = True - break - # End of entry - if stripped.startswith(('msgid', '#')) or not stripped: - break - - if not has_content: - unfinished_translations += 1 - - return (total_translations, unfinished_translations) if __name__ == "__main__": - with open(LANGUAGES_FILE) as f: + with open(LANGUAGES_FILE, "r") as f: translation_files = json.load(f) - badge_svg = [] - max_badge_width = 0 # keep track of max width to set parent element + badge_svg = [f''] for idx, (name, file) in enumerate(translation_files.items()): - po_file_path = os.path.join(str(TRANSLATIONS_DIR), f"app_{file}.po") - - total_translations, unfinished_translations = parse_po_file(po_file_path) + with open(os.path.join(TRANSLATIONS_DIR, f"{file}.ts"), "r") as tr_f: + tr_file = tr_f.read() - percent_finished = int(100 - (unfinished_translations / total_translations * 100.)) if total_translations > 0 else 0 - color = f"rgb{(94, 188, 0) if percent_finished == 100 else (248, 255, 50) if percent_finished > 90 else (204, 55, 27)}" + total_translations = 0 + unfinished_translations = 0 + for line in tr_file.splitlines(): + if TRANSLATION_TAG in line: + total_translations += 1 + if UNFINISHED_TRANSLATION_TAG in line: + unfinished_translations += 1 - # Download badge - badge_label = f"LANGUAGE {name}" - badge_message = f"{percent_finished}% complete" - if unfinished_translations != 0: - badge_message += f" ({unfinished_translations} unfinished)" + percent_finished = int(100 - (unfinished_translations / total_translations * 100.)) + color = "green" if percent_finished == 100 else "orange" if percent_finished >= 70 else "red" - r = requests.get(f"{SHIELDS_URL}/{badge_label}-{badge_message}-{color}", timeout=10) + r = requests.get(f"https://img.shields.io/badge/LANGUAGE {name}-{percent_finished}%25 complete-{color}", timeout=10) assert r.status_code == 200, "Error downloading badge" content_svg = r.content.decode("utf-8") - xml = ET.fromstring(content_svg) - assert "width" in xml.attrib - max_badge_width = max(max_badge_width, int(xml.attrib["width"])) - - # Make tag ids in each badge unique to combine them into one svg + # make tag ids in each badge unique for tag in ("r", "s"): content_svg = content_svg.replace(f'id="{tag}"', f'id="{tag}{idx}"') content_svg = content_svg.replace(f'"url(#{tag})"', f'"url(#{tag}{idx})"') badge_svg.extend([f'', content_svg, ""]) - badge_svg.insert(0, '') badge_svg.append("") with open(os.path.join(BASEDIR, "translation_badge.svg"), "w") as badge_f: diff --git a/selfdrive/ui/translations/languages.json b/selfdrive/ui/translations/languages.json index 47e673ce89b692..072d320c137e23 100644 --- a/selfdrive/ui/translations/languages.json +++ b/selfdrive/ui/translations/languages.json @@ -1,15 +1,8 @@ { - "English": "en", - "Deutsch": "de", - "Français": "fr", - "Português": "pt-BR", - "Español": "es", - "Türkçe": "tr", - "Українська": "uk", - "العربية": "ar", - "ไทย": "th", - "中文（繁體）": "zh-CHT", - "中文（简体）": "zh-CHS", - "한국어": "ko", - "日本語": "ja" + "English": "main_en", + "Português": "main_pt-BR", + "中文（繁體）": "main_zh-CHT", + "中文（简体）": "main_zh-CHS", + "한국어": "main_ko", + "日本語": "main_ja" } diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts new file mode 100644 index 00000000000000..284208720b5003 --- /dev/null +++ b/selfdrive/ui/translations/main_ar.ts @@ -0,0 +1,1323 @@ + + + + + AbstractAlert + + + Close + أغلق + + + + Snooze Update + تأخير التحديث + + + + Reboot and Update + إعادة التشغيل والتحديث + + + + AdvancedNetworking + + + Back + خلف + + + + Enable Tethering + تمكين الربط + + + + Tethering Password + كلمة مرور للربط + + + + + EDIT + تعديل + + + + Enter new tethering password + أدخل كلمة مرور جديدة للربط + + + + IP Address + عنوان IP + + + + Enable Roaming + تمكين التجوال + + + + APN Setting + إعداد APN + + + + Enter APN + أدخل APN + + + + leave blank for automatic configuration + اتركه فارغا للتكوين التلقائي + + + + ConfirmationDialog + + + + Ok + موافق + + + + Cancel + إلغاء + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + يجب عليك قبول الشروط والأحكام من أجل استخدام openpilot. + + + + Back + خلف + + + + Decline, uninstall %1 + رفض ، قم بإلغاء تثبيت %1 + + + + DevicePanel + + + Dongle ID + معرف دونجل + + + + N/A + غير متاح + + + + Serial + التسلسلي + + + + Driver Camera + كاميرا السائق + + + + PREVIEW + لمح + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + قم بمعاينة الكاميرا المواجهة للسائق للتأكد من أن نظام مراقبة السائق يتمتع برؤية جيدة. (يجب أن تكون السيارة معطلة) + + + + Reset Calibration + إعادة ضبط المعايرة + + + + RESET + إعادة تعيين + + + + Are you sure you want to reset calibration? + هل أنت متأكد أنك تريد إعادة ضبط المعايرة؟ + + + + Review Training Guide + مراجعة دليل التدريب + + + + REVIEW + مراجعة + + + + Review the rules, features, and limitations of openpilot + راجع القواعد والميزات والقيود الخاصة بـ openpilot + + + + Are you sure you want to review the training guide? + هل أنت متأكد أنك تريد مراجعة دليل التدريب؟ + + + + Regulatory + تنظيمية + + + + VIEW + عرض + + + + Change Language + تغيير اللغة + + + + CHANGE + تغيير + + + + Select a language + اختر لغة + + + + Reboot + اعادة التشغيل + + + + Power Off + أطفاء + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + يتطلب openpilot أن يتم تركيب الجهاز في حدود 4 درجات يسارًا أو يمينًا و 5 درجات لأعلى أو 8 درجات لأسفل. يقوم برنامج openpilot بالمعايرة بشكل مستمر ، ونادراً ما تكون إعادة الضبط مطلوبة. + + + + Your device is pointed %1° %2 and %3° %4. + جهازك يشير %1° %2 و %3° %4. + + + + down + لأسفل + + + + up + إلى أعلى + + + + left + إلى اليسار + + + + right + إلى اليمين + + + + Are you sure you want to reboot? + هل أنت متأكد أنك تريد إعادة التشغيل؟ + + + + Disengage to Reboot + فك الارتباط لإعادة التشغيل + + + + Are you sure you want to power off? + هل أنت متأكد أنك تريد إيقاف التشغيل؟ + + + + Disengage to Power Off + فك الارتباط لإيقاف التشغيل + + + + DriveStats + + + Drives + أرقام القيادة + + + + Hours + ساعات + + + + ALL TIME + في كل وقت + + + + PAST WEEK + الأسبوع الماضي + + + + KM + كم + + + + Miles + اميال + + + + DriverViewScene + + + camera starting + بدء تشغيل الكاميرا + + + + InputDialog + + + Cancel + إلغاء + + + + Need at least %n character(s)! + + تحتاج على الأقل %n حرف! + تحتاج على الأقل %n حرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + تحتاج على الأقل %n احرف! + + + + + Installer + + + Installing... + جارٍ التثبيت ... + + + + Receiving objects: + استقبال الكائنات: + + + + Resolving deltas: + حل دلتا: + + + + Updating files: + جارٍ تحديث الملفات: + + + + MapETA + + + eta + eta + + + + min + دق + + + + hr + سع + + + + km + كم + + + + mi + مل + + + + MapInstructions + + + km + كم + + + + m + م + + + + mi + مل + + + + ft + قد + + + + MapPanel + + + Current Destination + الوجهة الحالية + + + + CLEAR + مسح + + + + Recent Destinations + الوجهات الأخيرة + + + + Try the Navigation Beta + جرب التنقل التجريبي + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + احصل على الاتجاهات خطوة بخطوة معروضة والمزيد باستخدام comma +الاشتراك الرئيسي. اشترك الآن: https://connect.comma.ai + + + + No home +location set + لم يتم تعيين +موقع المنزل + + + + No work +location set + لم يتم تعيين +موقع العمل + + + + no recent destinations + لا توجد وجهات حديثة + + + + MapWindow + + + Map Loading + تحميل الخريطة + + + + Waiting for GPS + في انتظار GPS + + + + MultiOptionDialog + + + Select + اختر + + + + Cancel + إلغاء + + + + Networking + + + Advanced + متقدم + + + + Enter password + أدخل كلمة المرور + + + + + for "%1" + ل "%1" + + + + Wrong password + كلمة مرور خاطئة + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + الأعلى + + + + + SPEED + سرعة + + + + + LIMIT + حد + + + + OffroadHome + + + UPDATE + تحديث + + + + ALERTS + تنبيهات + + + + ALERT + تنبيه + + + + PairingPopup + + + Pair your device to your comma account + قم بإقران جهازك بحساب comma الخاص بك + + + + Go to https://connect.comma.ai on your phone + اذهب إلى https://connect.comma.ai من هاتفك + + + + Click "add new device" and scan the QR code on the right + انقر على "إضافة جهاز جديد" وامسح رمز الاستجابة السريعة على اليمين + + + + Bookmark connect.comma.ai to your home screen to use it like an app + ضع إشارة مرجعية على connect.comma.ai على شاشتك الرئيسية لاستخدامه مثل أي تطبيق + + + + PrimeAdWidget + + + Upgrade Now + قم بالترقية الآن + + + + Become a comma prime member at connect.comma.ai + كن عضوًا comme prime في connect.comma.ai + + + + PRIME FEATURES: + ميزات PRIME: + + + + Remote access + الوصول عن بعد + + + + 1 year of storage + سنة واحدة من التخزين + + + + Developer perks + امتيازات المطور + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ مشترك + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA POINTS + + + + QObject + + + Reboot + اعادة التشغيل + + + + Exit + أغلق + + + + dashcam + dashcam + + + + openpilot + openpilot + + + + %n minute(s) ago + + منذ %n دقيقة + منذ %n دقيقة + منذ %n دقائق + منذ %n دقائق + منذ %n دقائق + منذ %n دقائق + + + + + %n hour(s) ago + + منذ %n ساعة + منذ %n ساعة + منذ %n ساعات + منذ %n ساعات + منذ %n ساعات + منذ %n ساعات + + + + + %n day(s) ago + + منذ %n يوم + منذ %n يوم + منذ %n ايام + منذ %n ايام + منذ %n ايام + منذ %n ايام + + + + + Reset + + + Reset failed. Reboot to try again. + فشل إعادة التعيين. أعد التشغيل للمحاولة مرة أخرى. + + + + Are you sure you want to reset your device? + هل أنت متأكد أنك تريد إعادة ضبط جهازك؟ + + + + Resetting device... + جارٍ إعادة ضبط الجهاز ... + + + + System Reset + إعادة تعيين النظام + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + تم تشغيل إعادة تعيين النظام. اضغط على تأكيد لمسح كل المحتوى والإعدادات. اضغط على إلغاء لاستئناف التمهيد. + + + + Cancel + إلغاء + + + + Reboot + اعادة التشغيل + + + + Confirm + تأكيد + + + + Unable to mount data partition. Press confirm to reset your device. + تعذر تحميل قسم البيانات. اضغط على تأكيد لإعادة ضبط جهازك. + + + + RichTextDialog + + + Ok + موافق + + + + SettingsWindow + + + × + x + + + + Device + جهاز + + + + + Network + شبكة الاتصال + + + + Toggles + التبديل + + + + Software + برمجة + + + + Navigation + ملاحة + + + + Setup + + + WARNING: Low Voltage + تحذير: الجهد المنخفض + + + + Power your device in a car with a harness or proceed at your own risk. + قم بتشغيل جهازك في سيارة باستخدام أداة تثبيت أو المضي قدمًا على مسؤوليتك الخاصة. + + + + Power off + اطفئ الجهاز + + + + + + Continue + أكمل + + + + Getting Started + ابدء + + + + Before we get on the road, let’s finish installation and cover some details. + قبل أن ننطلق على الطريق ، دعنا ننتهي من التثبيت ونغطي بعض التفاصيل. + + + + Connect to Wi-Fi + اتصل بشبكة Wi-Fi + + + + + Back + خلف + + + + Continue without Wi-Fi + استمر بدون Wi-Fi + + + + Waiting for internet + في انتظار الاتصال بالإنترنت + + + + Choose Software to Install + اختر البرنامج المراد تثبيته + + + + Dashcam + Dashcam + + + + Custom Software + برامج مخصصة + + + + Enter URL + إدخال عنوان الموقع + + + + for Custom Software + للبرامج المخصصة + + + + Downloading... + جارى التحميل... + + + + Download Failed + فشل التنزيل + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + تأكد من أن عنوان موقع الويب الذي تم إدخاله صالح ، وأن اتصال الجهاز بالإنترنت جيد. + + + + Reboot device + إعادة تشغيل الجهاز + + + + Start over + ابدأ من جديد + + + + SetupWidget + + + Finish Setup + إنهاء الإعداد + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + قم بإقران جهازك بفاصلة connect (connect.comma.ai) واطلب عرض comma prime الخاص بك. + + + + Pair device + إقران الجهاز + + + + Sidebar + + + + CONNECT + الاتصال + + + + OFFLINE + غير متصل + + + + + ONLINE + متصل + + + + ERROR + خطأ + + + + + + TEMP + درجة الحرارة + + + + HIGH + عالي + + + + GOOD + جيد + + + + OK + موافق + + + + VEHICLE + مركبة + + + + NO + لا + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + بحث + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git Branch + + + + Git Commit + Git Commit + + + + OS Version + إصدار نظام التشغيل + + + + Version + إصدار + + + + Last Update Check + التحقق من آخر تحديث + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + آخر مرة نجح برنامج openpilot في التحقق من التحديث. يعمل المحدث فقط أثناء إيقاف تشغيل السيارة. + + + + Check for Update + فحص التحديثات + + + + CHECKING + تدقيق + + + + Switch Branch + تبديل الفرع + + + + ENTER + أدخل + + + + + The new branch will be pulled the next time the updater runs. + سيتم سحب الفرع الجديد في المرة التالية التي يتم فيها تشغيل أداة التحديث. + + + + Enter branch name + أدخل اسم الفرع + + + + UNINSTALL + الغاء التثبيت + + + + Uninstall %1 + الغاء التثبيت %1 + + + + Are you sure you want to uninstall? + هل أنت متأكد أنك تريد إلغاء التثبيت؟ + + + + failed to fetch update + فشل في جلب التحديث + + + + + CHECK + تأكد الان + + + + SshControl + + + SSH Keys + SSH Keys + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + تحذير: هذا يمنح SSH الوصول إلى جميع المفاتيح العامة في إعدادات GitHub. لا تدخل أبدًا اسم مستخدم GitHub بخلاف اسم المستخدم الخاص بك. لن يطلب منك موظف comma أبدًا إضافة اسم مستخدم GitHub الخاص به. + + + + + ADD + أضف + + + + Enter your GitHub username + أدخل اسم مستخدم GitHub الخاص بك + + + + LOADING + جار التحميل + + + + REMOVE + نزع + + + + Username '%1' has no keys on GitHub + لا يحتوي اسم المستخدم '%1' على مفاتيح على GitHub + + + + Request timed out + انتهت مهلة الطلب + + + + Username '%1' doesn't exist on GitHub + اسم المستخدم '%1' غير موجود على GitHub + + + + SshToggle + + + Enable SSH + تفعيل SSH + + + + TermsPage + + + Terms & Conditions + البنود و الظروف + + + + Decline + انحدار + + + + Scroll to accept + قم بالتمرير للقبول + + + + Agree + موافق + + + + TogglesPanel + + + Enable openpilot + تمكين openpilot + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + استخدم نظام الطيار المفتوح للتحكم التكيفي في ثبات السرعة والحفاظ على مساعدة السائق. انتباهك مطلوب في جميع الأوقات لاستخدام هذه الميزة. يسري تغيير هذا الإعداد عند إيقاف تشغيل السيارة. + + + + Enable Lane Departure Warnings + قم بتمكين تحذيرات مغادرة حارة السير + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + تلقي تنبيهات للتوجه مرة أخرى إلى الحارة عندما تنجرف سيارتك فوق خط المسار المكتشف دون تنشيط إشارة الانعطاف أثناء القيادة لمسافة تزيد عن 31 ميلاً في الساعة (50 كم / ساعة). + + + + Use Metric System + استخدم النظام المتري + + + + Display speed in km/h instead of mph. + عرض السرعة بالكيلو متر في الساعة بدلاً من ميل في الساعة. + + + + Record and Upload Driver Camera + تسجيل وتحميل كاميرا السائق + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + قم بتحميل البيانات من الكاميرا المواجهة للسائق وساعد في تحسين خوارزمية مراقبة السائق. + + + + Disengage On Accelerator Pedal + فك الارتباط على دواسة التسريع + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + عند التمكين ، سيؤدي الضغط على دواسة الوقود إلى فصل الطيار المفتوح. + + + + Show ETA in 24h Format + إظهار الوقت المقدر للوصول بتنسيق 24 ساعة + + + + Use 24h format instead of am/pm + استخدم تنسيق 24 ساعة بدلاً من صباحًا / مساءً + + + + Show Map on Left Side of UI + إظهار الخريطة على الجانب الأيسر من واجهة المستخدم + + + + Show map on left side when in split screen view. + إظهار الخريطة على الجانب الأيسر عندما تكون في طريقة عرض الشاشة المنقسمة. + + + + openpilot Longitudinal Control + openpilot التحكم الطولي + + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + سوف يقوم برنامج openpilot بتعطيل رادار السيارة وسيتولى التحكم في الغاز والمكابح. تحذير: هذا يعطل AEB! + + + + Updater + + + Update Required + مطلوب التحديث + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + مطلوب تحديث نظام التشغيل. قم بتوصيل جهازك بشبكة Wi-Fi للحصول على أسرع تجربة تحديث. حجم التنزيل 1 غيغابايت تقريبًا. + + + + Connect to Wi-Fi + اتصل بشبكة Wi-Fi + + + + Install + ثبيت + + + + Back + خلف + + + + Loading... + جار التحميل... + + + + Reboot + اعادة التشغيل + + + + Update failed + فشل التحديث + + + + WifiUI + + + + Scanning for networks... + جارٍ البحث عن شبكات ... + + + + CONNECTING... + جارٍ الاتصال ... + + + + FORGET + نزع + + + + Forget Wi-Fi Network "%1"? + نزع شبكة اWi-Fi "%1"? + + + diff --git a/selfdrive/ui/translations/main_en.ts b/selfdrive/ui/translations/main_en.ts new file mode 100644 index 00000000000000..42e30a59af4890 --- /dev/null +++ b/selfdrive/ui/translations/main_en.ts @@ -0,0 +1,42 @@ + + + + + InputDialog + + + Need at least %n character(s)! + + Need at least %n character! + Need at least %n characters! + + + + + QObject + + + %n minute(s) ago + + %n minute ago + %n minutes ago + + + + + %n hour(s) ago + + %n hour ago + %n hours ago + + + + + %n day(s) ago + + %n day ago + %n days ago + + + + diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts new file mode 100644 index 00000000000000..a0b2a99866f537 --- /dev/null +++ b/selfdrive/ui/translations/main_ja.ts @@ -0,0 +1,1313 @@ + + + + + AbstractAlert + + + Close + 閉じる + + + + Snooze Update + 更新の一時停止 + + + + Reboot and Update + 再起動してアップデート + + + + AdvancedNetworking + + + Back + 戻る + + + + Enable Tethering + ﾃｻﾞﾘﾝｸﾞを有効化 + + + + Tethering Password + ﾃｻﾞﾘﾝｸﾞﾊﾟｽﾜｰﾄﾞ + + + + + EDIT + 編集 + + + + Enter new tethering password + 新しいテザリングパスワードを入力 + + + + IP Address + IP アドレス + + + + Enable Roaming + ﾛｰﾐﾝｸﾞを有効化 + + + + APN Setting + APN 設定 + + + + Enter APN + APN を入力 + + + + leave blank for automatic configuration + 空白のままにして、自動設定にします + + + + ConfirmationDialog + + + + Ok + OK + + + + Cancel + キャンセル + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + openpilot をご利用される前に、利用規約に同意する必要があります。 + + + + Back + 戻る + + + + Decline, uninstall %1 + 拒否して %1 をｱﾝｲﾝｽﾄｰﾙ + + + + DevicePanel + + + Dongle ID + ドングル番号 (Dongle ID) + + + + N/A + N/A + + + + Serial + シリアル番号 + + + + Driver Camera + 車内カメラ + + + + PREVIEW + 見る + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + 車内カメラをプレビューして、ドライバー監視システムの視界を確認ができます。(車両の電源を切る必要があります) + + + + Reset Calibration + ｷｬﾘﾌﾞﾚｰｼｮﾝをﾘｾｯﾄ + + + + RESET + ﾘｾｯﾄ + + + + Are you sure you want to reset calibration? + ｷｬﾘﾌﾞﾚｰｼｮﾝをリセットしてもよろしいですか？ + + + + Review Training Guide + 入門書を見る + + + + REVIEW + 見る + + + + Review the rules, features, and limitations of openpilot + openpilot の特徴を見る + + + + Are you sure you want to review the training guide? + 入門書を見てもよろしいですか？ + + + + Regulatory + 認証情報 + + + + VIEW + 見る + + + + Change Language + 言語を変更 + + + + CHANGE + 変更 + + + + Select a language + 言語を選択 + + + + Reboot + 再起動 + + + + Power Off + 電源を切る + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot は、左または右の4°以内、上の5°または下の8°以内にデバイスを取付ける必要があります。キャリブレーションを引き続きます、リセットはほとんど必要ありません。 + + + + Your device is pointed %1° %2 and %3° %4. + このデバイスは%2の%1°、%4の%3°に向けます。 + + + + down + 下 + + + + up + 上 + + + + left + 左 + + + + right + 右 + + + + Are you sure you want to reboot? + 再起動してもよろしいですか？ + + + + Disengage to Reboot + openpilot をキャンセルして再起動ができます + + + + Are you sure you want to power off? + シャットダウンしてもよろしいですか？ + + + + Disengage to Power Off + openpilot をキャンセルしてシャットダウンができます + + + + DriveStats + + + Drives + 運転履歴 + + + + Hours + 時間 + + + + ALL TIME + 累計 + + + + PAST WEEK + 先週 + + + + KM + km + + + + Miles + マイル + + + + DriverViewScene + + + camera starting + ｶﾒﾗを起動しています + + + + InputDialog + + + Cancel + キャンセル + + + + Need at least %n character(s)! + + %n文字以上でお願いします！ + + + + + Installer + + + Installing... + インストールしています... + + + + Receiving objects: + ｵﾌﾞｼﾞｪｸﾄをﾀﾞｳﾝﾛｰﾄﾞしています： + + + + Resolving deltas: + デルタを解決しています： + + + + Updating files: + ファイルを更新しています： + + + + MapETA + + + eta + 予定到着時間 + + + + min + 分 + + + + hr + 時間 + + + + km + ｷﾛﾒｰﾄﾙ + + + + mi + ﾏｲﾙ + + + + MapInstructions + + + km + ｷﾛﾒｰﾄﾙ + + + + m + ﾒｰﾄﾙ + + + + mi + ﾏｲﾙ + + + + ft + ﾌｨｰﾄ + + + + MapPanel + + + Current Destination + 現在の目的地 + + + + CLEAR + 削除 + + + + Recent Destinations + 最近の目的地 + + + + Try the Navigation Beta + β版ﾅﾋﾞｹﾞｰｼｮﾝを試す + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + より詳細な案内情報を得ることができます。 +詳しくはこちら：https://connect.comma.ai + + + + No home +location set + 自宅の住所はまだ +設定されていません + + + + No work +location set + 職場の住所はまだ +設定されていません + + + + no recent destinations + 最近の目的地履歴がありません + + + + MapWindow + + + Map Loading + マップを読み込んでいます + + + + Waiting for GPS + GPS信号を探しています + + + + MultiOptionDialog + + + Select + 選択 + + + + Cancel + キャンセル + + + + Networking + + + Advanced + 詳細 + + + + Enter password + パスワードを入力 + + + + + for "%1" + ﾈｯﾄﾜｰｸ名：%1 + + + + Wrong password + ﾊﾟｽﾜｰﾄﾞが間違っています + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + 最高速度 + + + + + SPEED + 速度 + + + + + LIMIT + 制限速度 + + + + OffroadHome + + + UPDATE + 更新 + + + + ALERTS + 警告 + + + + ALERT + 警告 + + + + PairingPopup + + + Pair your device to your comma account + デバイスと comma アカウントを連携する + + + + Go to https://connect.comma.ai on your phone + モバイルデバイスで「connect.comma.ai」にアクセスして + + + + Click "add new device" and scan the QR code on the right + 「新しいデバイスを追加」を押すと、右側のQRコードをスキャンしてください + + + + Bookmark connect.comma.ai to your home screen to use it like an app + 「connect.comma.ai」をホーム画面に追加して、アプリのように使うことができます + + + + PrimeAdWidget + + + Upgrade Now + 今すぐｱｯﾌﾟｸﾞﾚｰﾄ + + + + Become a comma prime member at connect.comma.ai + connect.comma.ai でﾌﾟﾗｲﾑ会員に登録できます + + + + PRIME FEATURES: + 特典： + + + + Remote access + リモートアクセス + + + + 1 year of storage + 一年間の保存期間 + + + + Developer perks + 開発者向け特典 + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ 入会しました + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA POINTS + + + + QObject + + + Reboot + 再起動 + + + + Exit + 閉じる + + + + dashcam + ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n 分前 + + + + + %n hour(s) ago + + %n 時間前 + + + + + %n day(s) ago + + %n 日前 + + + + + Reset + + + Reset failed. Reboot to try again. + 初期化に失敗しました。再起動後に再試行してください。 + + + + Are you sure you want to reset your device? + 初期化してもよろしいですか？ + + + + Resetting device... + デバイスが初期化されます... + + + + System Reset + システムを初期化 + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + システムの初期化をリクエストしました。「確認」ボタンを押すとデバイスが初期化されます。「キャンセル」ボタンを押すと起動を続行します。 + + + + Cancel + キャンセル + + + + Reboot + 再起動 + + + + Confirm + 確認 + + + + Unable to mount data partition. Press confirm to reset your device. + 「data」パーティションをマウントできません。「確認」ボタンを押すとデバイスが初期化されます。 + + + + RichTextDialog + + + Ok + OK + + + + SettingsWindow + + + × + × + + + + Device + デバイス + + + + + Network + ﾈｯﾄﾜｰｸ + + + + Toggles + 切り替え + + + + Software + ｿﾌﾄｳｪｱ + + + + Navigation + ﾅﾋﾞｹﾞｰｼｮﾝ + + + + Setup + + + WARNING: Low Voltage + 警告：低電圧 + + + + Power your device in a car with a harness or proceed at your own risk. + 自己責任でハーネスから電源を供給してください。 + + + + Power off + 電源を切る + + + + + + Continue + 続ける + + + + Getting Started + はじめに + + + + Before we get on the road, let’s finish installation and cover some details. + その前に、インストールを完了し、いくつかの詳細を説明します。 + + + + Connect to Wi-Fi + Wi-Fi に接続 + + + + + Back + 戻る + + + + Continue without Wi-Fi + Wi-Fi に未接続で続行 + + + + Waiting for internet + インターネット接続を待機中 + + + + Choose Software to Install + インストールするソフトウェアを選びます + + + + Dashcam + ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ + + + + Custom Software + カスタムソフトウェア + + + + Enter URL + URL を入力 + + + + for Custom Software + カスタムソフトウェア + + + + Downloading... + ダウンロード中... + + + + Download Failed + ダウンロード失敗 + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + 入力された URL を確認し、デバイスがインターネットに接続されていることを確認してください。 + + + + Reboot device + デバイスを再起動 + + + + Start over + 最初からやり直す + + + + SetupWidget + + + Finish Setup + セットアップ完了 + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + デバイスを comma connect (connect.comma.ai)でペアリングし comma prime 特典を申請してください。 + + + + Pair device + デバイスをペアリング + + + + Sidebar + + + + CONNECT + 接続 + + + + OFFLINE + オフライン + + + + + ONLINE + オンライン + + + + ERROR + エラー + + + + + + TEMP + 温度 + + + + HIGH + 高温 + + + + GOOD + 最適 + + + + OK + OK + + + + VEHICLE + 車両 + + + + NO + NO + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + 検索 + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git ブランチ + + + + Git Commit + Git コミット + + + + OS Version + OS バージョン + + + + Version + バージョン + + + + Last Update Check + 最終更新確認 + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + openpilotが最後にアップデートの確認に成功してからの時間です。アップデート処理は、車の電源が切れているときのみ実行されます。 + + + + Check for Update + 更新プログラムをチェック + + + + CHECKING + 確認中 + + + + Switch Branch + ブランチの切り替え + + + + ENTER + 切替 + + + + + The new branch will be pulled the next time the updater runs. + updater を実行する時にブランチを切り替えます。 + + + + Enter branch name + ブランチ名を入力 + + + + UNINSTALL + アンインストール + + + + Uninstall %1 + %1をアンインストール + + + + Are you sure you want to uninstall? + アンインストールしてもよろしいですか？ + + + + failed to fetch update + 更新のﾀﾞｳﾝﾛｰﾄﾞにｴﾗｰが発生しました + + + + + CHECK + 確認 + + + + SshControl + + + SSH Keys + SSH 鍵 + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + 警告: これは、GitHub の設定にあるすべての公開鍵への SSH アクセスを許可するものです。自分以外の GitHub のユーザー名を入力しないでください。コンマのスタッフが GitHub のユーザー名を追加するようお願いすることはありません。 + + + + + ADD + 追加 + + + + Enter your GitHub username + GitHub のユーザー名を入力してください + + + + LOADING + ローディング + + + + REMOVE + 削除 + + + + Username '%1' has no keys on GitHub + ユーザー名 “%1” は GitHub に鍵がありません + + + + Request timed out + リクエストタイムアウト + + + + Username '%1' doesn't exist on GitHub + ユーザー名 '%1' は GitHub に存在しません + + + + SshToggle + + + Enable SSH + SSH を有効化 + + + + TermsPage + + + Terms & Conditions + 利用規約 + + + + Decline + 拒否 + + + + Scroll to accept + スクロールして同意 + + + + Agree + 同意 + + + + TogglesPanel + + + Enable openpilot + openpilot を有効化 + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + アダプティブクルーズコントロールとレーンキーピングドライバーアシスト（openpilotシステム）。この機能を使用するには、常に注意が必要です。この設定を変更すると、車の電源が切れたときに有効になります。 + + + + Enable Lane Departure Warnings + 車線逸脱警報機能を有効化 + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + 時速31マイル（50km）を超えるスピードで走行中、ウインカーを作動させずに検出された車線ライン上に車両が触れた場合、車線に戻るアラートを受信します。 + + + + Use Metric System + メートル法を有効化 + + + + Display speed in km/h instead of mph. + 速度は mph ではなく km/h で表示されます。 + + + + Record and Upload Driver Camera + 車内カメラの録画とアップロード + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + 車内カメラの映像をアップロードし、ドライバー監視システムのアルゴリズムの向上に役立てます。 + + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + 🌮 エンドツーエンドのアクセル制御 (超アルファ版) 🌮 + + + + Experimental openpilot longitudinal control + 実験段階のopenpilotによるアクセル制御 + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + <b>警告: openpilotによるアクセル制御は実験段階であり、AEBを無効化します。</b> + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + アクセルとブレーキの制御をopenpilotに任せます。openpilotが人間と同じように運転します。最初期の実験段階です。 + + + + Disengage On Accelerator Pedal + ｱｸｾﾙ踏むと openpilot をｷｬﾝｾﾙ + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + 有効な場合は、アクセルを踏むと openpilot をキャンセルします。 + + + + Show ETA in 24h Format + 24時間表示 + + + + Use 24h format instead of am/pm + AM/PM の代わりに24時間形式を使用します + + + + Show Map on Left Side of UI + ﾃﾞｨｽﾌﾟﾚｲの左側にﾏｯﾌﾟを表示 + + + + Show map on left side when in split screen view. + 分割画面表示の場合、ディスプレイの左側にマップを表示します。 + + + + Updater + + + Update Required + 更新が必要です + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + ｵﾍﾟﾚｰﾃｨﾝｸﾞｼｽﾃﾑのアップデートが必要です。Wi-Fi に接続することで、最速のアップデートを体験できます。ダウンロードサイズは約 1GB です。 + + + + Connect to Wi-Fi + Wi-Fi に接続 + + + + Install + インストール + + + + Back + 戻る + + + + Loading... + 読み込み中... + + + + Reboot + 再起動 + + + + Update failed + 更新失敗 + + + + WifiUI + + + + Scanning for networks... + ネットワークをスキャン中... + + + + CONNECTING... + 接続中... + + + + FORGET + 削除 + + + + Forget Wi-Fi Network "%1"? + Wi-Fiﾈｯﾄﾜｰｸ%1を削除してもよろしいですか？ + + + diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts new file mode 100644 index 00000000000000..5d7df2162e5d54 --- /dev/null +++ b/selfdrive/ui/translations/main_ko.ts @@ -0,0 +1,1313 @@ + + + + + AbstractAlert + + + Close + 닫기 + + + + Snooze Update + 업데이트 일시중지 + + + + Reboot and Update + 업데이트 및 재부팅 + + + + AdvancedNetworking + + + Back + 뒤로 + + + + Enable Tethering + 테더링 사용 + + + + Tethering Password + 테더링 비밀번호 + + + + + EDIT + 편집 + + + + Enter new tethering password + 새 테더링 비밀번호를 입력하세요 + + + + IP Address + IP 주소 + + + + Enable Roaming + 로밍 사용 + + + + APN Setting + APN 설정 + + + + Enter APN + APN 입력 + + + + leave blank for automatic configuration + 자동설정하려면 공백으로 두세요 + + + + ConfirmationDialog + + + + Ok + 확인 + + + + Cancel + 취소 + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + openpilot을 사용하려면 이용약관에 동의해야 합니다. + + + + Back + 뒤로 + + + + Decline, uninstall %1 + 거절, %1 제거 + + + + DevicePanel + + + Dongle ID + Dongle ID + + + + N/A + N/A + + + + Serial + Serial + + + + Driver Camera + 운전자 카메라 + + + + PREVIEW + 미리보기 + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + 운전자 모니터링이 좋은 가시성을 갖도록 운전자를 향한 카메라를 미리 봅니다. (차량연결은 해제되어있어야 합니다) + + + + Reset Calibration + 캘리브레이션 + + + + RESET + 재설정 + + + + Are you sure you want to reset calibration? + 캘리브레이션을 재설정하시겠습니까? + + + + Review Training Guide + 트레이닝 가이드 + + + + REVIEW + 다시보기 + + + + Review the rules, features, and limitations of openpilot + openpilot의 규칙, 기능 및 제한 다시보기 + + + + Are you sure you want to review the training guide? + 트레이닝 가이드를 다시보시겠습니까? + + + + Regulatory + 규제 + + + + VIEW + 보기 + + + + Change Language + 언어 변경 + + + + CHANGE + 변경 + + + + Select a language + 언어를 선택하세요 + + + + Reboot + 재부팅 + + + + Power Off + 전원 종료 + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot은 좌우측은 4° 이내, 위쪽은 5° 아래쪽은 8° 이내로 장치를 설치해야 합니다. openpilot은 지속적으로 보정되므로 리셋은 거의 필요하지 않습니다. + + + + Your device is pointed %1° %2 and %3° %4. + 사용자의 장치가 %1° %2 및 %3° %4 위치에 설치되어있습니다. + + + + down + 아래로 + + + + up + 위로 + + + + left + 좌측으로 + + + + right + 우측으로 + + + + Are you sure you want to reboot? + 재부팅 하시겠습니까? + + + + Disengage to Reboot + 재부팅 하려면 해제하세요 + + + + Are you sure you want to power off? + 전원을 종료하시겠습니까? + + + + Disengage to Power Off + 전원을 종료하려면 해제하세요 + + + + DriveStats + + + Drives + 주행 + + + + Hours + 시간 + + + + ALL TIME + 전체 + + + + PAST WEEK + 지난주 + + + + KM + Km + + + + Miles + Miles + + + + DriverViewScene + + + camera starting + 카메라 시작중 + + + + InputDialog + + + Cancel + 취소 + + + + Need at least %n character(s)! + + 최소 %n 자가 필요합니다! + + + + + Installer + + + Installing... + 설치중... + + + + Receiving objects: + 수신중： + + + + Resolving deltas: + 델타병합： + + + + Updating files: + 파일갱신： + + + + MapETA + + + eta + 도착 + + + + min + 분 + + + + hr + 시간 + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + mi + + + + ft + ft + + + + MapPanel + + + Current Destination + 현재 목적지 + + + + CLEAR + 삭제 + + + + Recent Destinations + 최근 목적지 + + + + Try the Navigation Beta + 네비게이션(베타)를 사용해보세요 + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + 자세한 경로안내를 원하시면 comma prime을 구독하세요. +등록：https://connect.comma.ai + + + + No home +location set + 집 +설정되지않음 + + + + No work +location set + 회사 +설정되지않음 + + + + no recent destinations + 최근 목적지 없음 + + + + MapWindow + + + Map Loading + 지도 로딩 + + + + Waiting for GPS + GPS를 기다리는 중 + + + + MultiOptionDialog + + + Select + 선택 + + + + Cancel + 취소 + + + + Networking + + + Advanced + 고급 설정 + + + + Enter password + 비밀번호를 입력하세요 + + + + + for "%1" + "%1"에 접속하려면 인증이 필요합니다 + + + + Wrong password + 비밀번호가 틀렸습니다 + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + MAX + + + + + SPEED + SPEED + + + + + LIMIT + LIMIT + + + + OffroadHome + + + UPDATE + 업데이트 + + + + ALERTS + 알림 + + + + ALERT + 알림 + + + + PairingPopup + + + Pair your device to your comma account + 장치를 콤마 계정과 페어링합니다 + + + + Go to https://connect.comma.ai on your phone + https://connect.comma.ai에 접속하세요 + + + + Click "add new device" and scan the QR code on the right + "새 장치 추가"를 클릭하고 오른쪽 QR 코드를 검색합니다 + + + + Bookmark connect.comma.ai to your home screen to use it like an app + connect.comma.ai을 앱처럼 사용하려면 홈 화면에 바로가기를 만드십시오 + + + + PrimeAdWidget + + + Upgrade Now + 지금 업그레이드 + + + + Become a comma prime member at connect.comma.ai + connect.comma.ai에서 comma prime에 가입합니다 + + + + PRIME FEATURES: + PRIME 기능： + + + + Remote access + 원격 접속 + + + + 1 year of storage + 1년간 저장 + + + + Developer perks + 개발자 혜택 + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ 구독함 + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA POINTS + + + + QObject + + + Reboot + 재부팅 + + + + Exit + 종료 + + + + dashcam + dashcam + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n 분전 + + + + + %n hour(s) ago + + %n 시간전 + + + + + %n day(s) ago + + %n 일전 + + + + + Reset + + + Reset failed. Reboot to try again. + 초기화 실패. 재부팅후 다시 시도하세요. + + + + Are you sure you want to reset your device? + 장치를 초기화 하시겠습니까? + + + + Resetting device... + 장치 초기화중... + + + + System Reset + 장치 초기화 + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + 장치를 초기화 합니다. 확인버튼을 누르면 모든 내용과 설정이 초기화됩니다. 부팅을 재개하려면 취소를 누르세요. + + + + Cancel + 취소 + + + + Reboot + 재부팅 + + + + Confirm + 확인 + + + + Unable to mount data partition. Press confirm to reset your device. + 데이터 파티션을 마운트할 수 없습니다. 확인 버튼을 눌러 장치를 리셋합니다. + + + + RichTextDialog + + + Ok + 확인 + + + + SettingsWindow + + + × + × + + + + Device + 장치 + + + + + Network + 네트워크 + + + + Toggles + 토글 + + + + Software + 소프트웨어 + + + + Navigation + 네비게이션 + + + + Setup + + + WARNING: Low Voltage + 경고: 전압이 낮습니다 + + + + Power your device in a car with a harness or proceed at your own risk. + 하네스 보드에 차량의 전원을 연결하세요. + + + + Power off + 전원 종료 + + + + + + Continue + 계속 + + + + Getting Started + 설정 시작 + + + + Before we get on the road, let’s finish installation and cover some details. + 출발하기 전에 설정을 완료하고 몇 가지 세부 사항을 살펴보겠습니다. + + + + Connect to Wi-Fi + wifi 연결 + + + + + Back + 뒤로 + + + + Continue without Wi-Fi + wifi 연결없이 계속하기 + + + + Waiting for internet + 네트워크 접속을 기다립니다 + + + + Choose Software to Install + 설치할 소프트웨어를 선택하세요 + + + + Dashcam + Dashcam + + + + Custom Software + Custom Software + + + + Enter URL + URL 입력 + + + + for Custom Software + for Custom Software + + + + Downloading... + 다운로드중... + + + + Download Failed + 다운로드 실패 + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + 입력된 URL이 유효하고 장치의 네트워크 연결이 잘 되어 있는지 확인하세요. + + + + Reboot device + 재부팅 + + + + Start over + 다시 시작 + + + + SetupWidget + + + Finish Setup + 설정 완료 + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 장치를 (connect.comma.ai)에서 페어링하고 comma prime 오퍼를 청구합니다. + + + + Pair device + 장치 페어링 + + + + Sidebar + + + + CONNECT + 연결 + + + + OFFLINE + 오프라인 + + + + + ONLINE + 온라인 + + + + ERROR + 오류 + + + + + + TEMP + 온도 + + + + HIGH + 높음 + + + + GOOD + 좋음 + + + + OK + 경고 + + + + VEHICLE + 차량 + + + + NO + NO + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + 검색중 + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + 이더넷 + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git 브렌치 + + + + Git Commit + Git 커밋 + + + + OS Version + OS 버전 + + + + Version + 버전 + + + + Last Update Check + 최신 업데이트 검사 + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + 최근에 openpilot이 업데이트를 성공적으로 확인했습니다. 업데이트 프로그램은 차량 연결이 해제되었을때만 작동합니다. + + + + Check for Update + 업데이트 확인 + + + + CHECKING + 확인중 + + + + Switch Branch + 브랜치 변경 + + + + ENTER + 입력하세요 + + + + + The new branch will be pulled the next time the updater runs. + 다음 업데이트 프로그램이 실행될 때 새 브랜치가 적용됩니다. + + + + Enter branch name + 브랜치명 입력 + + + + UNINSTALL + 제거 + + + + Uninstall %1 + %1 제거 + + + + Are you sure you want to uninstall? + 제거하시겠습니까? + + + + failed to fetch update + 업데이트를 가져올수없습니다 + + + + + CHECK + 확인 + + + + SshControl + + + SSH Keys + SSH 키 + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + 경고: 허용으로 설정하면 GitHub 설정의 모든 공용 키에 대한 SSH 액세스 권한이 부여됩니다. GitHub 사용자 ID 이외에는 입력하지 마십시오. comma에서는 GitHub ID를 추가하라는 요청을 하지 않습니다. + + + + + ADD + 추가 + + + + Enter your GitHub username + GitHub 사용자 ID + + + + LOADING + 로딩 + + + + REMOVE + 제거 + + + + Username '%1' has no keys on GitHub + '%1'의 키가 GitHub에 없습니다 + + + + Request timed out + 요청 시간 초과 + + + + Username '%1' doesn't exist on GitHub + '%1'은 GitHub에 없습니다 + + + + SshToggle + + + Enable SSH + SSH 사용 + + + + TermsPage + + + Terms & Conditions + 약관 + + + + Decline + 거절 + + + + Scroll to accept + 허용하려면 아래로 스크롤하세요 + + + + Agree + 동의 + + + + TogglesPanel + + + Enable openpilot + openpilot 사용 + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + 어댑티브 크루즈 컨트롤 및 차선 유지 운전자 보조를 위해 openpilot 시스템을 사용하십시오. 이 기능을 사용하려면 항상 주의를 기울여야 합니다. 설정변경은 장치 재부팅후 적용됩니다. + + + + Enable Lane Departure Warnings + 차선 이탈 경고 사용 + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향지시등 없이 감지된 차선 위를 주행할 경우 차선이탈 경고를 표시합니다. + + + + Use Metric System + 미터법 사용 + + + + Display speed in km/h instead of mph. + mph 대신 km/h로 속도를 표시합니다. + + + + Record and Upload Driver Camera + 운전자 카메라 녹화 및 업로드 + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + 운전자 카메라에서 데이터를 업로드하고 운전자 모니터링 알고리즘을 개선합니다. + + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + 🌮 e2e 롱컨트롤 사용 (매우 실험적) 🌮 + + + + Experimental openpilot longitudinal control + openpilot 롱컨트롤 (실험적) + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + <b>경고: openpilot 롱컨트롤은 실험적인 기능으로 차량의 AEB를 비활성화합니다.</b> + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + 주행모델이 가속과 감속을 제어하도록 하면 openpilot은 운전자가 생각하는것처럼 운전합니다. (매우 실험적) + + + + Disengage On Accelerator Pedal + 가속페달 조작시 해제 + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + 활성화된 경우 가속 페달을 누르면 openpilot이 해제됩니다. + + + + Show ETA in 24h Format + 24시간 형식으로 도착예정시간 표시 + + + + Use 24h format instead of am/pm + 오전/오후 대신 24시간 형식 사용 + + + + Show Map on Left Side of UI + UI 왼쪽에 지도 표시 + + + + Show map on left side when in split screen view. + 분할 화면 보기에서 지도를 왼쪽에 표시합니다. + + + + Updater + + + Update Required + 업데이트 필요 + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + OS 업데이트가 필요합니다. 장치를 wifi에 연결하면 가장 빠른 업데이트 경험을 제공합니다. 다운로드 크기는 약 1GB입니다. + + + + Connect to Wi-Fi + wifi 연결 + + + + Install + 설치 + + + + Back + 뒤로 + + + + Loading... + 로딩중... + + + + Reboot + 재부팅 + + + + Update failed + 업데이트 실패 + + + + WifiUI + + + + Scanning for networks... + 네트워크 검색 중... + + + + CONNECTING... + 연결중... + + + + FORGET + 저장안함 + + + + Forget Wi-Fi Network "%1"? + wifi 네트워크 저장안함 "%1"? + + + diff --git a/selfdrive/ui/translations/main_nl.ts b/selfdrive/ui/translations/main_nl.ts new file mode 100644 index 00000000000000..99646ed749e9f7 --- /dev/null +++ b/selfdrive/ui/translations/main_nl.ts @@ -0,0 +1,1307 @@ + + + + + AbstractAlert + + + Close + Sluit + + + + Snooze Update + Update uitstellen + + + + Reboot and Update + Opnieuw Opstarten en Updaten + + + + AdvancedNetworking + + + Back + Terug + + + + Enable Tethering + Tethering Inschakelen + + + + Tethering Password + Tethering Wachtwoord + + + + + EDIT + AANPASSEN + + + + Enter new tethering password + Voer nieuw tethering wachtwoord in + + + + IP Address + IP Adres + + + + Enable Roaming + Roaming Inschakelen + + + + APN Setting + APN Instelling + + + + Enter APN + Voer APN in + + + + leave blank for automatic configuration + laat leeg voor automatische configuratie + + + + ConfirmationDialog + + + + Ok + Ok + + + + Cancel + Annuleren + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + U moet de Algemene Voorwaarden accepteren om openpilot te gebruiken. + + + + Back + Terug + + + + Decline, uninstall %1 + Afwijzen, verwijder %1 + + + + DevicePanel + + + Dongle ID + Dongle ID + + + + N/A + Nvt + + + + Serial + Serienummer + + + + Driver Camera + Bestuurders Camera + + + + PREVIEW + BEKIJKEN + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Bekijk de naar de bestuurder gerichte camera om ervoor te zorgen dat het monitoren van de bestuurder goed zicht heeft. (Voertuig moet uitgschakeld zijn) + + + + Reset Calibration + Kalibratie Resetten + + + + RESET + RESET + + + + Are you sure you want to reset calibration? + Weet u zeker dat u de kalibratie wilt resetten? + + + + Review Training Guide + Doorloop de Training Opnieuw + + + + REVIEW + BEKIJKEN + + + + Review the rules, features, and limitations of openpilot + Bekijk de regels, functies en beperkingen van openpilot + + + + Are you sure you want to review the training guide? + Weet u zeker dat u de training opnieuw wilt doorlopen? + + + + Regulatory + Regelgeving + + + + VIEW + BEKIJKEN + + + + Change Language + Taal Wijzigen + + + + CHANGE + WIJZIGEN + + + + Select a language + Selecteer een taal + + + + Reboot + Opnieuw Opstarten + + + + Power Off + Uitschakelen + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot vereist dat het apparaat binnen 4° links of rechts en binnen 5° omhoog of 8° omlaag wordt gemonteerd. openpilot kalibreert continu, resetten is zelden nodig. + + + + Your device is pointed %1° %2 and %3° %4. + Uw apparaat is gericht op %1° %2 en %3° %4. + + + + down + omlaag + + + + up + omhoog + + + + left + links + + + + right + rechts + + + + Are you sure you want to reboot? + Weet u zeker dat u opnieuw wilt opstarten? + + + + Disengage to Reboot + Deactiveer openpilot om opnieuw op te starten + + + + Are you sure you want to power off? + Weet u zeker dat u wilt uitschakelen? + + + + Disengage to Power Off + Deactiveer openpilot om uit te schakelen + + + + DriveStats + + + Drives + Ritten + + + + Hours + Uren + + + + ALL TIME + TOTAAL + + + + PAST WEEK + AFGELOPEN WEEK + + + + KM + Km + + + + Miles + Mijl + + + + DriverViewScene + + + camera starting + Camera wordt gestart + + + + InputDialog + + + Cancel + Annuleren + + + + Need at least %n character(s)! + + Heeft minstens %n karakter nodig! + Heeft minstens %n karakters nodig! + + + + + Installer + + + Installing... + Installeren... + + + + Receiving objects: + Objecten ontvangen: + + + + Resolving deltas: + Deltas verwerken: + + + + Updating files: + Bestanden bijwerken: + + + + MapETA + + + eta + eta + + + + min + min + + + + hr + uur + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + mi + + + + ft + ft + + + + MapPanel + + + Current Destination + Huidige Bestemming + + + + CLEAR + LEEGMAKEN + + + + Recent Destinations + Recente Bestemmingen + + + + Try the Navigation Beta + Probeer de Navigatie Bèta + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Krijg stapsgewijze routebeschrijving en meer met een comma +prime abonnement. Meld u nu aan: https://connect.comma.ai + + + + No home +location set + Geen thuislocatie +ingesteld + + + + No work +location set + Geen werklocatie +ingesteld + + + + no recent destinations + geen recente bestemmingen + + + + MapWindow + + + Map Loading + Kaart wordt geladen + + + + Waiting for GPS + Wachten op GPS + + + + MultiOptionDialog + + + Select + Selecteer + + + + Cancel + Annuleren + + + + Networking + + + Advanced + Geavanceerd + + + + Enter password + Voer wachtwoord in + + + + + for "%1" + voor "%1" + + + + Wrong password + Verkeerd wachtwoord + + + + NvgWindow + + + km/h + km/u + + + + mph + mph + + + + + MAX + MAX + + + + + SPEED + SPEED + + + + + LIMIT + LIMIT + + + + OffroadHome + + + UPDATE + UPDATE + + + + ALERTS + WAARSCHUWINGEN + + + + ALERT + WAARSCHUWING + + + + PairingPopup + + + Pair your device to your comma account + Koppel uw apparaat aan uw comma-account + + + + Go to https://connect.comma.ai on your phone + Ga naar https://connect.comma.ai op uw telefoon + + + + Click "add new device" and scan the QR code on the right + Klik op "add new device" en scan de QR-code aan de rechterkant + + + + Bookmark connect.comma.ai to your home screen to use it like an app + Voeg connect.comma.ai toe op uw startscherm om het als een app te gebruiken + + + + PrimeAdWidget + + + Upgrade Now + Upgrade nu + + + + Become a comma prime member at connect.comma.ai + Word een comma prime lid op connect.comma.ai + + + + PRIME FEATURES: + PRIME BEVAT: + + + + Remote access + Toegang op afstand + + + + 1 year of storage + 1 jaar lang opslag + + + + Developer perks + Voordelen voor ontwikkelaars + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ GEABONNEERD + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA PUNTEN + + + + QObject + + + Reboot + Opnieuw Opstarten + + + + Exit + Afsluiten + + + + dashcam + dashcam + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n minuut geleden + %n minuten geleden + + + + + %n hour(s) ago + + %n uur geleden + %n uur geleden + + + + + %n day(s) ago + + %n dag geleden + %n dagen geleden + + + + + Reset + + + Reset failed. Reboot to try again. + Opnieuw instellen mislukt. Start opnieuw op om opnieuw te proberen. + + + + Are you sure you want to reset your device? + Weet u zeker dat u uw apparaat opnieuw wilt instellen? + + + + Resetting device... + Apparaat opnieuw instellen... + + + + System Reset + Systeemreset + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Systeemreset geactiveerd. Druk op bevestigen om alle inhoud en instellingen te wissen. Druk op Annuleren om het opstarten te hervatten. + + + + Cancel + Annuleren + + + + Reboot + Opnieuw Opstarten + + + + Confirm + Bevestigen + + + + Unable to mount data partition. Press confirm to reset your device. + Kan gegevenspartitie niet koppelen. Druk op bevestigen om uw apparaat te resetten. + + + + RichTextDialog + + + Ok + Ok + + + + SettingsWindow + + + × + × + + + + Device + Apparaat + + + + + Network + Netwerk + + + + Toggles + Opties + + + + Software + Software + + + + Navigation + Navigatie + + + + Setup + + + WARNING: Low Voltage + WAARCHUWING: Lage Spanning + + + + Power your device in a car with a harness or proceed at your own risk. + Voorzie uw apparaat van stroom in een auto met een harnas (car harness) of ga op eigen risico verder. + + + + Power off + Uitschakelen + + + + + + Continue + Doorgaan + + + + Getting Started + Aan de slag + + + + Before we get on the road, let’s finish installation and cover some details. + Laten we, voordat we op pad gaan, de installatie afronden en enkele details bespreken. + + + + Connect to Wi-Fi + Maak verbinding met Wi-Fi + + + + + Back + Terug + + + + Continue without Wi-Fi + Doorgaan zonder Wi-Fi + + + + Waiting for internet + Wachten op internet + + + + Choose Software to Install + Kies Software om te Installeren + + + + Dashcam + Dashcam + + + + Custom Software + Andere Software + + + + Enter URL + Voer URL in + + + + for Custom Software + voor Andere Software + + + + Downloading... + Downloaden... + + + + Download Failed + Downloaden Mislukt + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Zorg ervoor dat de ingevoerde URL geldig is en dat de internetverbinding van het apparaat goed is. + + + + Reboot device + Apparaat opnieuw opstarten + + + + Start over + Begin opnieuw + + + + SetupWidget + + + Finish Setup + Installatie voltooien + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Koppel uw apparaat met comma connect (connect.comma.ai) en claim uw comma prime-aanbieding. + + + + Pair device + Apparaat koppelen + + + + Sidebar + + + + CONNECT + VERBINDING + + + + OFFLINE + OFFLINE + + + + + ONLINE + ONLINE + + + + ERROR + FOUT + + + + + + TEMP + TEMP + + + + HIGH + HOOG + + + + GOOD + GOED + + + + OK + OK + + + + VEHICLE + VOERTUIG + + + + NO + GEEN + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + ZOEKEN + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + 4G + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git Branch + + + + Git Commit + Git Commit + + + + OS Version + OS Versie + + + + Version + Versie + + + + Last Update Check + Laatste Updatecontrole + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + De laatste keer dat openpilot met succes heeft gecontroleerd op een update. De updater werkt alleen als de auto is uitgeschakeld. + + + + Check for Update + Controleer op Updates + + + + CHECKING + CONTROLEER + + + + Switch Branch + Branch Verwisselen + + + + ENTER + INVOEREN + + + + + The new branch will be pulled the next time the updater runs. + Tijdens de volgende update wordt de nieuwe branch opgehaald. + + + + Enter branch name + Voer branch naam in + + + + Uninstall %1 + Verwijder %1 + + + + UNINSTALL + VERWIJDER + + + + Are you sure you want to uninstall? + Weet u zeker dat u de installatie ongedaan wilt maken? + + + + failed to fetch update + ophalen van update mislukt + + + + + CHECK + CONTROLEER + + + + SshControl + + + SSH Keys + SSH Sleutels + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Waarschuwing: dit geeft SSH toegang tot alle openbare sleutels in uw GitHub-instellingen. Voer nooit een andere GitHub-gebruikersnaam in dan die van uzelf. Een medewerker van comma zal u NOOIT vragen om zijn GitHub-gebruikersnaam toe te voegen. + + + + + ADD + TOEVOEGEN + + + + Enter your GitHub username + Voer uw GitHub gebruikersnaam in + + + + LOADING + LADEN + + + + REMOVE + VERWIJDEREN + + + + Username '%1' has no keys on GitHub + Gebruikersnaam '%1' heeft geen SSH sleutels op GitHub + + + + Request timed out + Time-out van aanvraag + + + + Username '%1' doesn't exist on GitHub + Gebruikersnaam '%1' bestaat niet op GitHub + + + + SshToggle + + + Enable SSH + SSH Inschakelen + + + + TermsPage + + + Terms & Conditions + Algemene Voorwaarden + + + + Decline + Afwijzen + + + + Scroll to accept + Scroll om te accepteren + + + + Agree + Akkoord + + + + TogglesPanel + + + Enable openpilot + openpilot Inschakelen + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Gebruik het openpilot-systeem voor adaptieve cruisecontrol en rijstrookassistentie. Uw aandacht is te allen tijde vereist om deze functie te gebruiken. Het wijzigen van deze instelling wordt van kracht wanneer de auto wordt uitgeschakeld. + + + + Enable Lane Departure Warnings + Waarschuwingen bij Verlaten Rijstrook Inschakelen + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Ontvang waarschuwingen om terug naar de rijstrook te sturen wanneer uw voertuig over een gedetecteerde rijstrookstreep drijft zonder dat de richtingaanwijzer wordt geactiveerd terwijl u harder rijdt dan 50 km/u (31 mph). + + + + Use Metric System + Gebruik Metrisch Systeem + + + + Display speed in km/h instead of mph. + Geef snelheid weer in km/u in plaats van mph. + + + + Record and Upload Driver Camera + Opnemen en Uploaden van de Bestuurders Camera + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Upload gegevens van de bestuurders camera en help het algoritme voor het monitoren van de bestuurder te verbeteren. + + + + Disengage On Accelerator Pedal + Deactiveren Met Gaspedaal + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Indien ingeschakeld, zal het indrukken van het gaspedaal openpilot deactiveren. + + + + Show ETA in 24h Format + Toon verwachte aankomsttijd in 24-uurs formaat + + + + Use 24h format instead of am/pm + Gebruik 24-uurs formaat in plaats van AM en PM + + + + Show Map on Left Side of UI + Toon kaart aan linkerkant van het scherm + + + + Show map on left side when in split screen view. + Toon kaart links in gesplitste schermweergave. + + + + openpilot Longitudinal Control + openpilot Longitudinale Controle + + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + openpilot zal de radar van de auto uitschakelen en de controle over gas en remmen overnemen. Waarschuwing: hierdoor wordt AEB (automatische noodrem) uitgeschakeld! + + + + Updater + + + Update Required + Update Vereist + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Een update van het besturingssysteem is vereist. Verbind je apparaat met Wi-Fi voor de snelste update-ervaring. De downloadgrootte is ongeveer 1 GB. + + + + Connect to Wi-Fi + Maak verbinding met Wi-Fi + + + + Install + Installeer + + + + Back + Terug + + + + Loading... + Aan het laden... + + + + Reboot + Opnieuw Opstarten + + + + Update failed + Update mislukt + + + + WifiUI + + + + Scanning for networks... + Scannen naar netwerken... + + + + CONNECTING... + VERBINDEN... + + + + FORGET + VERGETEN + + + + Forget Wi-Fi Network "%1"? + Vergeet Wi-Fi Netwerk "%1"? + + + diff --git a/selfdrive/ui/translations/main_pl.ts b/selfdrive/ui/translations/main_pl.ts new file mode 100644 index 00000000000000..8593d68261b5c8 --- /dev/null +++ b/selfdrive/ui/translations/main_pl.ts @@ -0,0 +1,1311 @@ + + + + + AbstractAlert + + + Close + Zamknij + + + + Snooze Update + Zaktualizuj później + + + + Reboot and Update + Uruchom ponownie i zaktualizuj + + + + AdvancedNetworking + + + Back + Wróć + + + + Enable Tethering + Włącz hotspot osobisty + + + + Tethering Password + Hasło do hotspotu + + + + + EDIT + EDYTUJ + + + + Enter new tethering password + Wprowadź nowe hasło do hotspotu + + + + IP Address + Adres IP + + + + Enable Roaming + Włącz roaming danych + + + + APN Setting + Ustawienia APN + + + + Enter APN + Wprowadź APN + + + + leave blank for automatic configuration + Pozostaw puste, aby użyć domyślnej konfiguracji + + + + ConfirmationDialog + + + + Ok + Ok + + + + Cancel + Anuluj + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + Aby korzystać z openpilota musisz zaakceptować regulamin. + + + + Back + Wróć + + + + Decline, uninstall %1 + Odrzuć, odinstaluj %1 + + + + DevicePanel + + + Dongle ID + ID adaptera + + + + N/A + N/A + + + + Serial + Numer seryjny + + + + Driver Camera + Kamera kierowcy + + + + PREVIEW + PODGLĄD + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Wyświetl podgląd z kamery skierowanej na kierowcę, aby upewnić się, że monitoring kierowcy ma dobry zakres widzenia. (pojazd musi być wyłączony) + + + + Reset Calibration + Zresetuj kalibrację + + + + RESET + ZRESETUJ + + + + Are you sure you want to reset calibration? + Czy na pewno chcesz zresetować kalibrację? + + + + Review Training Guide + Zapoznaj się z samouczkiem + + + + REVIEW + ZAPOZNAJ SIĘ + + + + Review the rules, features, and limitations of openpilot + Zapoznaj się z zasadami, funkcjami i ograniczeniami openpilota + + + + Are you sure you want to review the training guide? + Czy na pewno chcesz się zapoznać z samouczkiem? + + + + Regulatory + Regulacja + + + + VIEW + WIDOK + + + + Change Language + Zmień język + + + + CHANGE + ZMIEŃ + + + + Select a language + Wybierz język + + + + Reboot + Uruchom ponownie + + + + Power Off + Wyłącz + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot wymaga, aby urządzenie było zamontowane z maksymalnym odchyłem 4° poziomo, 5° w górę oraz 8° w dół. openpilot jest ciągle kalibrowany, rzadko konieczne jest resetowania urządzenia. + + + + Your device is pointed %1° %2 and %3° %4. + Twoje urządzenie jest skierowane %1° %2 oraz %3° %4. + + + + down + w dół + + + + up + w górę + + + + left + w lewo + + + + right + w prawo + + + + Are you sure you want to reboot? + Czy na pewno chcesz uruchomić ponownie urządzenie? + + + + Disengage to Reboot + Aby uruchomić ponownie, odłącz sterowanie + + + + Are you sure you want to power off? + Czy na pewno chcesz wyłączyć urządzenie? + + + + Disengage to Power Off + Aby wyłączyć urządzenie, odłącz sterowanie + + + + DriveStats + + + Drives + Przejazdy + + + + Hours + Godziny + + + + ALL TIME + CAŁKOWICIE + + + + PAST WEEK + OSTATNI TYDZIEŃ + + + + KM + KM + + + + Miles + Mile + + + + DriverViewScene + + + camera starting + uruchamianie kamery + + + + InputDialog + + + Cancel + Anuluj + + + + Need at least %n character(s)! + + Wpisana wartość powinna składać się przynajmniej z %n znaku! + Wpisana wartość powinna skłądać się przynajmniej z %n znaków! + Wpisana wartość powinna skłądać się przynajmniej z %n znaków! + + + + + Installer + + + Installing... + Instalowanie... + + + + Receiving objects: + Odbieranie obiektów: + + + + Resolving deltas: + Rozwiązywanie różnic: + + + + Updating files: + Aktualizacja plików: + + + + MapETA + + + eta + przewidywany czas + + + + min + min + + + + hr + godz + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + mi + + + + ft + ft + + + + MapPanel + + + Current Destination + Miejsce docelowe + + + + CLEAR + WYCZYŚĆ + + + + Recent Destinations + Ostatnie miejsca docelowe + + + + Try the Navigation Beta + Wypróbuj nawigację w wersji beta + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Odblokuj nawigację zakręt po zakęcie i wiele więcej subskrybując +comma prime. Zarejestruj się teraz: https://connect.comma.ai + + + + No home +location set + Lokalizacja domu +nie została ustawiona + + + + No work +location set + Miejsce pracy +nie zostało ustawione + + + + no recent destinations + brak ostatnich miejsc docelowych + + + + MapWindow + + + Map Loading + Ładowanie Mapy + + + + Waiting for GPS + Oczekiwanie na sygnał GPS + + + + MultiOptionDialog + + + Select + Wybierz + + + + Cancel + Anuluj + + + + Networking + + + Advanced + Zaawansowane + + + + Enter password + Wprowadź hasło + + + + + for "%1" + do "%1" + + + + Wrong password + Niepoprawne hasło + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + MAX + + + + + SPEED + PRĘDKOŚĆ + + + + + LIMIT + OGRANICZENIE + + + + OffroadHome + + + UPDATE + UAKTUALNIJ + + + + ALERTS + ALERTY + + + + ALERT + ALERT + + + + PairingPopup + + + Pair your device to your comma account + Sparuj swoje urzadzenie ze swoim kontem comma + + + + Go to https://connect.comma.ai on your phone + Wejdź na stronę https://connect.comma.ai na swoim telefonie + + + + Click "add new device" and scan the QR code on the right + Kliknij "add new device" i zeskanuj kod QR znajdujący się po prawej stronie + + + + Bookmark connect.comma.ai to your home screen to use it like an app + Dodaj connect.comma.ai do zakładek na swoim ekranie początkowym, aby korzystać z niej jak z aplikacji + + + + PrimeAdWidget + + + Upgrade Now + Uaktualnij teraz + + + + Become a comma prime member at connect.comma.ai + Zostań członkiem comma prime na connect.comma.ai + + + + PRIME FEATURES: + FUNKCJE PRIME: + + + + Remote access + Zdalny dostęp + + + + 1 year of storage + 1 rok przechowywania danych + + + + Developer perks + Udogodnienia dla programistów + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ ZASUBSKRYBOWANO + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA POINTS + + + + QObject + + + Reboot + Uruchom Ponownie + + + + Exit + Wyjdź + + + + dashcam + wideorejestrator + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n minutę temu + %n minuty temu + %n minut temu + + + + + %n hour(s) ago + + % godzinę temu + %n godziny temu + %n godzin temu + + + + + %n day(s) ago + + %n dzień temu + %n dni temu + %n dni temu + + + + + Reset + + + Reset failed. Reboot to try again. + Wymazywanie zakończone niepowodzeniem. Aby spróbować ponownie, uruchom ponownie urządzenie. + + + + Are you sure you want to reset your device? + Czy na pewno chcesz wymazać urządzenie? + + + + Resetting device... + Wymazywanie urządzenia... + + + + System Reset + Przywróć do ustawień fabrycznych + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Przywracanie do ustawień fabrycznych. Wciśnij potwierdź, aby usunąć wszystkie dane oraz ustawienia. Wciśnij anuluj, aby wznowić uruchamianie. + + + + Cancel + Anuluj + + + + Reboot + Uruchom ponownie + + + + Confirm + Potwiedź + + + + Unable to mount data partition. Press confirm to reset your device. + Partycja nie została zamontowana poprawnie. Wciśnij potwierdź, aby uruchomić ponownie urządzenie. + + + + RichTextDialog + + + Ok + Ok + + + + SettingsWindow + + + × + x + + + + Device + Urządzenie + + + + + Network + Sieć + + + + Toggles + Przełączniki + + + + Software + Oprogramowanie + + + + Navigation + Nawigacja + + + + Setup + + + WARNING: Low Voltage + OSTRZEŻENIE: Niskie Napięcie + + + + Power your device in a car with a harness or proceed at your own risk. + Podłącz swoje urządzenie do zasilania poprzez podłączenienie go do pojazdu lub kontynuuj na własną odpowiedzialność. + + + + Power off + Wyłącz + + + + + + Continue + Kontynuuj + + + + Getting Started + Zacznij + + + + Before we get on the road, let’s finish installation and cover some details. + Zanim ruszysz w drogę, dokończ instalację i podaj kilka szczegółów. + + + + Connect to Wi-Fi + Połącz z Wi-Fi + + + + + Back + Wróć + + + + Continue without Wi-Fi + Kontynuuj bez połączenia z Wif-Fi + + + + Waiting for internet + Oczekiwanie na połączenie sieciowe + + + + Choose Software to Install + Wybierz oprogramowanie do instalacji + + + + Dashcam + Wideorejestrator + + + + Custom Software + Własne oprogramowanie + + + + Enter URL + Wprowadź adres URL + + + + for Custom Software + do własnego oprogramowania + + + + Downloading... + Pobieranie... + + + + Download Failed + Pobieranie nie powiodło się + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Upewnij się, że wpisany adres URL jest poprawny, a połączenie internetowe działa poprawnie. + + + + Reboot device + Uruchom ponownie + + + + Start over + Zacznij od początku + + + + SetupWidget + + + Finish Setup + Zakończ konfigurację + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Sparuj swoje urządzenie z comma connect (connect.comma.ai) i wybierz swoją ofertę comma prime. + + + + Pair device + Sparuj urządzenie + + + + Sidebar + + + + CONNECT + POŁĄCZENIE + + + + OFFLINE + OFFLINE + + + + + ONLINE + ONLINE + + + + ERROR + BŁĄD + + + + + + TEMP + TEMP + + + + HIGH + WYSOKA + + + + GOOD + DOBRA + + + + OK + OK + + + + VEHICLE + POJAZD + + + + NO + BRAK + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + SZUKAJ + + + + -- + -- + + + + Wi-Fi + Wi-FI + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Gałąź Git + + + + Git Commit + Git commit + + + + OS Version + Wersja systemu + + + + Version + Wersja + + + + Last Update Check + Ostatnie sprawdzenie aktualizacji + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + Ostatni raz kiedy openpilot znalazł aktualizację. Aktualizator może być uruchomiony wyłącznie wtedy, kiedy pojazd jest wyłączony. + + + + Check for Update + Sprawdź uaktualnienia + + + + CHECKING + SPRAWDZANIE + + + + Switch Branch + Zmień gąłąź + + + + ENTER + WPROWADŹ + + + + + The new branch will be pulled the next time the updater runs. + Nowa gałąź będzie pobrana przy następnym uruchomieniu aktualizatora. + + + + Enter branch name + Wprowadź nazwę gałęzi + + + + Uninstall %1 + Odinstaluj %1 + + + + UNINSTALL + ODINSTALUJ + + + + Are you sure you want to uninstall? + Czy na pewno chcesz odinstalować? + + + + failed to fetch update + pobieranie aktualizacji zakończone niepowodzeniem + + + + + CHECK + SPRAWDŹ + + + + SshControl + + + SSH Keys + Klucze SSH + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Ostrzeżenie: To spowoduje przekazanie dostępu do wszystkich Twoich publicznych kuczy z ustawień GitHuba. Nigdy nie wprowadzaj nazwy użytkownika innej niż swoja. Pracownik comma NIGDY nie poprosi o dodanie swojej nazwy uzytkownika. + + + + + ADD + DODAJ + + + + Enter your GitHub username + Wpisz swoją nazwę użytkownika GitHub + + + + LOADING + ŁADOWANIE + + + + REMOVE + USUŃ + + + + Username '%1' has no keys on GitHub + Użytkownik '%1' nie posiada żadnych kluczy na GitHubie + + + + Request timed out + Limit czasu rządania + + + + Username '%1' doesn't exist on GitHub + Użytkownik '%1' nie istnieje na GitHubie + + + + SshToggle + + + Enable SSH + Włącz SSH + + + + TermsPage + + + Terms & Conditions + Regulamin + + + + Decline + Odrzuć + + + + Scroll to accept + Przewiń w dół, aby zaakceptować + + + + Agree + Zaakceptuj + + + + TogglesPanel + + + Enable openpilot + Włącz openpilota + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Użyj openpilota do zachowania bezpiecznego odstępu między pojazdami i do asystowania w utrzymywaniu pasa ruchu. Twoja pełna uwaga jest wymagana przez cały czas korzystania z tej funkcji. Ustawienie to może być wdrożone wyłącznie wtedy, gdy pojazd jest wyłączony. + + + + Enable Lane Departure Warnings + Włącz ostrzeganie przed zmianą pasa ruchu + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Otrzymuj alerty o powrocie na właściwy pas, kiedy Twój pojazd przekroczy linię bez włączonego kierunkowskazu jadąc powyżej 50 km/h (31 mph). + + + + Use Metric System + Korzystaj z systemu metrycznego + + + + Display speed in km/h instead of mph. + Wyświetl prędkość w km/h zamiast mph. + + + + Record and Upload Driver Camera + Nagraj i prześlij nagranie z kamery kierowcy + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Prześlij dane z kamery skierowanej na kierowcę i pomóż poprawiać algorytm monitorowania kierowcy. + + + + Disengage On Accelerator Pedal + Odłącz poprzez naciśnięcie gazu + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Po włączeniu, naciśnięcie na pedał gazu odłączy openpilota. + + + + Show ETA in 24h Format + Pokaż oczekiwany czas dojazdu w formacie 24-godzinnym + + + + Use 24h format instead of am/pm + Korzystaj z formatu 24-godzinnego zamiast 12-godzinnego + + + + Show Map on Left Side of UI + Pokaż mapę po lewej stronie ekranu + + + + Show map on left side when in split screen view. + Pokaż mapę po lewej stronie kiedy ekran jest podzielony. + + + + openpilot Longitudinal Control + Kontrola wzdłużna openpilota + + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + openpilot wyłączy radar samochodu i przejmie kontrolę nad gazem i hamulcem. Ostrzeżenie: wyłączony zostanie system AEB! + + + + Updater + + + Update Required + Wymagana Aktualizacja + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Wymagana aktualizacja systemu operacyjnego. Aby przyspieszyć proces aktualizacji połącz swoje urzeądzenie do Wi-Fi. Rozmiar pobieranej paczki wynosi około 1GB. + + + + Connect to Wi-Fi + Połącz się z Wi-Fi + + + + Install + Zainstaluj + + + + Back + Wróć + + + + Loading... + Ładowanie... + + + + Reboot + Uruchom ponownie + + + + Update failed + Aktualizacja nie powiodła się + + + + WifiUI + + + + Scanning for networks... + Wyszukiwanie sieci... + + + + CONNECTING... + ŁĄCZENIE... + + + + FORGET + ZAPOMNIJ + + + + Forget Wi-Fi Network "%1"? + Czy chcesz zapomnieć sieć "%1"? + + + diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts new file mode 100644 index 00000000000000..2b3acb369ffeb2 --- /dev/null +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -0,0 +1,1317 @@ + + + + + AbstractAlert + + + Close + Fechar + + + + Snooze Update + Adiar Atualização + + + + Reboot and Update + Reiniciar e Atualizar + + + + AdvancedNetworking + + + Back + Voltar + + + + Enable Tethering + Ativar Tether + + + + Tethering Password + Senha Tethering + + + + + EDIT + EDITAR + + + + Enter new tethering password + Insira nova senha tethering + + + + IP Address + Endereço IP + + + + Enable Roaming + Ativar Roaming + + + + APN Setting + APN Config + + + + Enter APN + Insira APN + + + + leave blank for automatic configuration + deixe em branco para configuração automática + + + + ConfirmationDialog + + + + Ok + OK + + + + Cancel + Cancelar + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + Você precisa aceitar os Termos e Condições para utilizar openpilot. + + + + Back + Voltar + + + + Decline, uninstall %1 + Rejeitar, desintalar %1 + + + + DevicePanel + + + Dongle ID + Dongle ID + + + + N/A + N/A + + + + Serial + Serial + + + + Driver Camera + Câmera voltada para o Motorista + + + + PREVIEW + PREVISUAL + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Pré-visualizar a câmera voltada para o motorista para garantir que monitor tem uma boa visibilidade (veículo precisa estar desligado) + + + + Reset Calibration + Resetar Calibragem + + + + RESET + RESET + + + + Are you sure you want to reset calibration? + Tem certeza que quer resetar a calibragem? + + + + Review Training Guide + Revisar Guia de Treinamento + + + + REVIEW + REVISAR + + + + Review the rules, features, and limitations of openpilot + Revisar regras, aprimoramentos e limitações do openpilot + + + + Are you sure you want to review the training guide? + Tem certeza que quer rever o treinamento? + + + + Regulatory + Regulatório + + + + VIEW + VER + + + + Change Language + Alterar Idioma + + + + CHANGE + ALTERAR + + + + Select a language + Selecione o Idioma + + + + Reboot + Reiniciar + + + + Power Off + Desligar + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + o openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. o openpilot está continuamente calibrando, resetar raramente é necessário. + + + + Your device is pointed %1° %2 and %3° %4. + Seu dispositivo está montado %1° %2 e %3° %4. + + + + down + baixo + + + + up + cima + + + + left + esquerda + + + + right + direita + + + + Are you sure you want to reboot? + Tem certeza que quer reiniciar? + + + + Disengage to Reboot + Desacione para Reiniciar + + + + Are you sure you want to power off? + Tem certeza que quer desligar? + + + + Disengage to Power Off + Desacione para Desligar + + + + DriveStats + + + Drives + Dirigidas + + + + Hours + Horas + + + + ALL TIME + TOTAL + + + + PAST WEEK + SEMANA PASSADA + + + + KM + KM + + + + Miles + Milhas + + + + DriverViewScene + + + camera starting + câmera iniciando + + + + InputDialog + + + Cancel + Cancelar + + + + Need at least %n character(s)! + + Necessita no mínimo %n caractere! + Necessita no mínimo %n caracteres! + + + + + Installer + + + Installing... + Instalando... + + + + Receiving objects: + Recebendo objetos: + + + + Resolving deltas: + Resolvendo deltas: + + + + Updating files: + Atualizando arquivos: + + + + MapETA + + + eta + eta + + + + min + min + + + + hr + hr + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + milha + + + + ft + pés + + + + MapPanel + + + Current Destination + Destino Atual + + + + CLEAR + LIMPAR + + + + Recent Destinations + Destinos Recentes + + + + Try the Navigation Beta + Experimente a Navegação Beta + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Obtenha instruções passo a passo exibidas e muito mais com +uma assinatura prime Inscreva-se agora: https://connect.comma.ai + + + + No home +location set + Sem local +residência definido + + + + No work +location set + Sem local de +trabalho definido + + + + no recent destinations + sem destinos recentes + + + + MapWindow + + + Map Loading + Carregando Mapa + + + + Waiting for GPS + Esperando por GPS + + + + MultiOptionDialog + + + Select + Selecione + + + + Cancel + Cancelar + + + + Networking + + + Advanced + Avançado + + + + Enter password + Insira a senha + + + + + for "%1" + para "%1" + + + + Wrong password + Senha incorreta + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + LIMITE + + + + + SPEED + MAX + + + + + LIMIT + VELO + + + + OffroadHome + + + UPDATE + ATUALIZAÇÃO + + + + ALERTS + ALERTAS + + + + ALERT + ALERTA + + + + PairingPopup + + + Pair your device to your comma account + Pareie seu dispositivo à sua conta comma + + + + Go to https://connect.comma.ai on your phone + navegue até https://connect.comma.ai no seu telefone + + + + Click "add new device" and scan the QR code on the right + Clique "add new device" e escaneie o QR code a seguir + + + + Bookmark connect.comma.ai to your home screen to use it like an app + Salve connect.comma.ai como sua página inicial para utilizar como um app + + + + PrimeAdWidget + + + Upgrade Now + Atualizar Agora + + + + Become a comma prime member at connect.comma.ai + Torne-se um membro comma prime em connect.comma.ai + + + + PRIME FEATURES: + APRIMORAMENTOS PRIME: + + + + Remote access + Acesso remoto + + + + 1 year of storage + 1 ano de armazenamento + + + + Developer perks + Benefícios para desenvolvedor + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ INSCRITO + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + PONTOS COMMA + + + + QObject + + + Reboot + Reiniciar + + + + Exit + Sair + + + + dashcam + dashcam + + + + openpilot + openpilot + + + + %n minute(s) ago + + há %n minuto + há %n minutos + + + + + %n hour(s) ago + + há %n hora + há %n horas + + + + + %n day(s) ago + + há %n dia + há %n dias + + + + + Reset + + + Reset failed. Reboot to try again. + Reset falhou. Reinicie para tentar novamente. + + + + Are you sure you want to reset your device? + Tem certeza que quer resetar seu dispositivo? + + + + Resetting device... + Resetando dispositivo... + + + + System Reset + Resetar Sistema + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Solicitado reset do sistema. Confirme para apagar todo conteúdo e configurações. Aperte cancelar para continuar boot. + + + + Cancel + Cancelar + + + + Reboot + Reiniciar + + + + Confirm + Confirmar + + + + Unable to mount data partition. Press confirm to reset your device. + Não foi possível montar a partição de dados. Pressione confirmar para resetar seu dispositivo. + + + + RichTextDialog + + + Ok + Ok + + + + SettingsWindow + + + × + × + + + + Device + Dispositivo + + + + + Network + Rede + + + + Toggles + Ajustes + + + + Software + Software + + + + Navigation + Navegação + + + + Setup + + + WARNING: Low Voltage + ALERTA: Baixa Voltagem + + + + Power your device in a car with a harness or proceed at your own risk. + Ligue seu dispositivo em um carro com um chicote ou prossiga por sua conta e risco. + + + + Power off + Desligar + + + + + + Continue + Continuar + + + + Getting Started + Começando + + + + Before we get on the road, let’s finish installation and cover some details. + Antes de pegarmos a estrada, vamos terminar a instalação e cobrir alguns detalhes. + + + + Connect to Wi-Fi + Conectar ao Wi-Fi + + + + + Back + Voltar + + + + Continue without Wi-Fi + Continuar sem Wi-Fi + + + + Waiting for internet + Esperando pela internet + + + + Choose Software to Install + Escolher Software para Instalar + + + + Dashcam + Dashcam + + + + Custom Software + Sofware Customizado + + + + Enter URL + Preencher URL + + + + for Custom Software + para o Software Customizado + + + + Downloading... + Baixando... + + + + Download Failed + Download Falhou + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Garanta que a URL inserida é valida, e uma boa conexão à internet. + + + + Reboot device + Reiniciar Dispositivo + + + + Start over + Inicializar + + + + SetupWidget + + + Finish Setup + Concluir + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Pareie seu dispositivo com comma connect (connect.comma.ai) e reivindique sua oferta de comma prime. + + + + Pair device + Parear dispositivo + + + + Sidebar + + + + CONNECT + CONEXÃO + + + + OFFLINE + DESCONEC + + + + + ONLINE + CONECTADO + + + + ERROR + ERRO + + + + + + TEMP + TEMP + + + + HIGH + ALTA + + + + GOOD + BOA + + + + OK + OK + + + + VEHICLE + VEÍCULO + + + + NO + SEM + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + PROCURA + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git Branch + + + + Git Commit + Último Commit + + + + OS Version + Versão do Sistema + + + + Version + Versão + + + + Last Update Check + Verificação da última atualização + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + A última vez que o openpilot verificou com sucesso uma atualização. O atualizador só funciona com o carro desligado. + + + + Check for Update + Verifique atualizações + + + + CHECKING + VERIFICANDO + + + + Switch Branch + Alterar Branch + + + + ENTER + INSERIR + + + + + The new branch will be pulled the next time the updater runs. + A nova branch será aplicada ao verificar atualizações. + + + + Enter branch name + Inserir o nome da branch + + + + UNINSTALL + DESINSTALAR + + + + Uninstall %1 + Desintalar o %1 + + + + Are you sure you want to uninstall? + Tem certeza que quer desinstalar? + + + + failed to fetch update + falha ao buscar atualização + + + + + CHECK + VERIFICAR + + + + SshControl + + + SSH Keys + Chave SSH + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Aviso: isso concede acesso SSH a todas as chaves públicas nas configurações do GitHub. Nunca insira um nome de usuário do GitHub que não seja o seu. Um funcionário da comma NUNCA pedirá que você adicione seu nome de usuário do GitHub. + + + + + ADD + ADICIONAR + + + + Enter your GitHub username + Insira seu nome de usuário do GitHub + + + + LOADING + CARREGANDO + + + + REMOVE + REMOVER + + + + Username '%1' has no keys on GitHub + Usuário "%1” não possui chaves no GitHub + + + + Request timed out + A solicitação expirou + + + + Username '%1' doesn't exist on GitHub + Usuário '%1' não existe no GitHub + + + + SshToggle + + + Enable SSH + Habilitar SSH + + + + TermsPage + + + Terms & Conditions + Termos & Condições + + + + Decline + Declinar + + + + Scroll to accept + Role a tela para aceitar + + + + Agree + Concordo + + + + TogglesPanel + + + Enable openpilot + Ativar openpilot + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Use o sistema openpilot para controle de cruzeiro adaptativo e assistência ao motorista de manutenção de faixa. Sua atenção é necessária o tempo todo para usar esse recurso. A alteração desta configuração tem efeito quando o carro é desligado. + + + + Enable Lane Departure Warnings + Ativar Avisos de Saída de Faixa + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Receba alertas para voltar para a pista se o seu veículo sair da faixa e a seta não tiver sido acionada previamente quando em velocidades superiores a 50 km/h. + + + + Use Metric System + Usar Sistema Métrico + + + + Display speed in km/h instead of mph. + Exibir velocidade em km/h invés de mph. + + + + Record and Upload Driver Camera + Gravar e Upload Câmera Motorista + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Upload dados da câmera voltada para o motorista e ajude a melhorar o algoritmo de monitoramentor. + + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + 🌮 End-to-end longitudinal (experimental) 🌮 + + + + Experimental openpilot longitudinal control + + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + + Disengage On Accelerator Pedal + Desacionar Com Pedal Do Acelerador + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Quando ativado, pressionar o pedal do acelerador desacionará o openpilot. + + + + Show ETA in 24h Format + Mostrar ETA em formato 24h + + + + Use 24h format instead of am/pm + Use o formato 24h em vez de am/pm + + + + Show Map on Left Side of UI + Exibir Mapa no Lado Esquerdo + + + + Show map on left side when in split screen view. + Exibir mapa do lado esquerdo quando a tela for dividida. + + + + Updater + + + Update Required + Atualização Necessária + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Uma atualização do sistema operacional é necessária. Conecte seu dispositivo ao Wi-Fi para a experiência de atualização mais rápida. O tamanho do download é de aproximadamente 1GB. + + + + Connect to Wi-Fi + Conecte-se ao Wi-Fi + + + + Install + Instalar + + + + Back + Voltar + + + + Loading... + Carregando... + + + + Reboot + Reiniciar + + + + Update failed + Falha na atualização + + + + WifiUI + + + + Scanning for networks... + Procurando redes... + + + + CONNECTING... + CONECTANDO... + + + + FORGET + ESQUECER + + + + Forget Wi-Fi Network "%1"? + Esquecer Rede Wi-Fi "%1"? + + + diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts new file mode 100644 index 00000000000000..7e7fcf2788b954 --- /dev/null +++ b/selfdrive/ui/translations/main_th.ts @@ -0,0 +1,1303 @@ + + + + + AbstractAlert + + + Close + ปิด + + + + Snooze Update + เลื่อนการอัปเดต + + + + Reboot and Update + รีบูตและอัปเดต + + + + AdvancedNetworking + + + Back + ย้อนกลับ + + + + Enable Tethering + ปล่อยฮอตสปอต + + + + Tethering Password + รหัสผ่านฮอตสปอต + + + + + EDIT + แก้ไข + + + + Enter new tethering password + ป้อนรหัสผ่านฮอตสปอตใหม่ + + + + IP Address + หมายเลขไอพี + + + + Enable Roaming + เปิดใช้งานโรมมิ่ง + + + + APN Setting + ตั้งค่า APN + + + + Enter APN + ป้อนค่า APN + + + + leave blank for automatic configuration + เว้นว่างเพื่อตั้งค่าอัตโนมัติ + + + + ConfirmationDialog + + + + Ok + ตกลง + + + + Cancel + ยกเลิก + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + คุณต้องยอมรับเงื่อนไขและข้อตกลง เพื่อใช้งาน openpilot + + + + Back + ย้อนกลับ + + + + Decline, uninstall %1 + ปฏิเสธ และถอนการติดตั้ง %1 + + + + DevicePanel + + + Dongle ID + Dongle ID + + + + N/A + ไม่มี + + + + Serial + ซีเรียล + + + + Driver Camera + กล้องฝั่งคนขับ + + + + PREVIEW + แสดงภาพ + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + ดูภาพตัวอย่างกล้องที่หันเข้าหาคนขับเพื่อให้แน่ใจว่าการตรวจสอบคนขับมีทัศนวิสัยที่ดี (รถต้องดับเครื่องยนต์) + + + + Reset Calibration + รีเซ็ตการคาลิเบรท + + + + RESET + รีเซ็ต + + + + Are you sure you want to reset calibration? + คุณแน่ใจหรือไม่ว่าต้องการรีเซ็ตการคาลิเบรท? + + + + Review Training Guide + ทบทวนคู่มือการใช้งาน + + + + REVIEW + ทบทวน + + + + Review the rules, features, and limitations of openpilot + ตรวจสอบกฎ คุณสมบัติ และข้อจำกัดของ openpilot + + + + Are you sure you want to review the training guide? + คุณแน่ใจหรือไม่ว่าต้องการทบทวนคู่มือการใช้งาน? + + + + Regulatory + ระเบียบข้อบังคับ + + + + VIEW + ดู + + + + Change Language + เปลี่ยนภาษา + + + + CHANGE + เปลี่ยน + + + + Select a language + เลือกภาษา + + + + Reboot + รีบูต + + + + Power Off + ปิดเครื่อง + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot กำหนดให้ติดตั้งอุปกรณ์ โดยสามารถเอียงด้านซ้ายหรือขวาไม่เกิน 4° และเอียงขึ้นด้านบนไม่เกิน 5° หรือเอียงลงด้านล่างไม่เกิน 8° openpilot ทำการคาลิเบรทอย่างต่อเนื่อง แทบจะไม่จำเป็นต้องทำการรีเซ็ตการคาลิเบรท + + + + Your device is pointed %1° %2 and %3° %4. + อุปกรณ์ของคุณเอียงไปทาง %2 %1° และ %4 %3° + + + + down + ด้านล่าง + + + + up + ด้านบน + + + + left + ด้านซ้าย + + + + right + ด้านขวา + + + + Are you sure you want to reboot? + คุณแน่ใจหรือไม่ว่าต้องการรีบูต? + + + + Disengage to Reboot + ยกเลิกระบบช่วยขับเพื่อรีบูต + + + + Are you sure you want to power off? + คุณแน่ใจหรือไม่ว่าต้องการปิดเครื่อง? + + + + Disengage to Power Off + ยกเลิกระบบช่วยขับเพื่อปิดเครื่อง + + + + DriveStats + + + Drives + การขับขี่ + + + + Hours + ชั่วโมง + + + + ALL TIME + ทั้งหมด + + + + PAST WEEK + สัปดาห์ที่ผ่านมา + + + + KM + กิโลเมตร + + + + Miles + ไมล์ + + + + DriverViewScene + + + camera starting + กำลังเปิดกล้อง + + + + InputDialog + + + Cancel + ยกเลิก + + + + Need at least %n character(s)! + + ต้องการอย่างน้อย %n ตัวอักษร! + + + + + Installer + + + Installing... + กำลังติดตั้ง... + + + + Receiving objects: + กำลังรับข้อมูล: + + + + Resolving deltas: + การแก้ไขเดลต้า: + + + + Updating files: + กำลังอัปเดตไฟล์: + + + + MapETA + + + eta + eta + + + + min + นาที + + + + hr + ชม. + + + + km + กม. + + + + mi + ไมล์ + + + + MapInstructions + + + km + กม. + + + + m + ม. + + + + mi + ไมล์ + + + + ft + ฟุต + + + + MapPanel + + + Current Destination + ปลายทางปัจจุบัน + + + + CLEAR + ล้างข้อมูล + + + + Recent Destinations + ปลายทางล่าสุด + + + + Try the Navigation Beta + ลองใช้ระบบนำทาง (เบต้า) + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + รับการแสดงเส้นทางแบบเลี้ยวต่อเลี้ยว และอื่นๆ ด้วยการสมัครบริการ +comma prime สมัครเลย: https://connect.comma.ai + + + + No home +location set + ยังไม่ได้กำหนด +ตำแหน่งของบ้าน + + + + No work +location set + ยังไม่ได้กำหนด +ตำแหน่งของที่ทำงาน + + + + no recent destinations + ไม่พบปลายทางล่าสุด + + + + MapWindow + + + Map Loading + กำลังโหลดแผนที่ + + + + Waiting for GPS + กำลังรอสัญญาณ GPS + + + + MultiOptionDialog + + + Select + เลือก + + + + Cancel + ยกเลิก + + + + Networking + + + Advanced + ขั้นสูง + + + + Enter password + ใส่รหัสผ่าน + + + + + for "%1" + สำหรับ "%1" + + + + Wrong password + รหัสผ่านผิด + + + + NvgWindow + + + km/h + กม./ชม. + + + + mph + ไมล์/ชม. + + + + + MAX + สูงสุด + + + + + SPEED + ความเร็ว + + + + + LIMIT + จำกัด + + + + OffroadHome + + + UPDATE + อัปเดต + + + + ALERTS + การแจ้งเตือน + + + + ALERT + การแจ้งเตือน + + + + PairingPopup + + + Pair your device to your comma account + จับคู่อุปกรณ์ของคุณกับบัญชี comma ของคุณ + + + + Go to https://connect.comma.ai on your phone + ไปที่ https://connect.comma.ai ด้วยโทรศัพท์ของคุณ + + + + Click "add new device" and scan the QR code on the right + กดที่ "add new device" และสแกนคิวอาร์โค้ดทางด้านขวา + + + + Bookmark connect.comma.ai to your home screen to use it like an app + จดจำ connect.comma.ai โดยการเพิ่มไปยังหน้าจอโฮม เพื่อใช้งานเหมือนเป็นแอปพลิเคชัน + + + + PrimeAdWidget + + + Upgrade Now + อัพเกรดเดี๋ยวนี้ + + + + Become a comma prime member at connect.comma.ai + สมัครสมาชิก comma prime ได้ที่ connect.comma.ai + + + + PRIME FEATURES: + คุณสมบัติของ PRIME: + + + + Remote access + การเข้าถึงระยะไกล + + + + 1 year of storage + จัดเก็บข้อมูลนาน 1 ปี + + + + Developer perks + สิทธิพิเศษสำหรับนักพัฒนา + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ สมัครสำเร็จ + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + คะแนน COMMA + + + + QObject + + + Reboot + รีบูต + + + + Exit + ปิด + + + + dashcam + กล้องติดรถยนต์ + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n นาทีที่แล้ว + + + + + %n hour(s) ago + + %n ชั่วโมงที่แล้ว + + + + + %n day(s) ago + + %n วันที่แล้ว + + + + + Reset + + + Reset failed. Reboot to try again. + การรีเซ็ตล้มเหลว รีบูตเพื่อลองอีกครั้ง + + + + Are you sure you want to reset your device? + คุณแน่ใจหรือไม่ว่าต้องการรีเซ็ตอุปกรณ์? + + + + Resetting device... + กำลังรีเซ็ตอุปกรณ์... + + + + System Reset + รีเซ็ตระบบ + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + มีการสั่งรีเซ็ตระบบ กดยืนยันเพื่อลบข้อมูลและการตั้งค่าทั้งหมด กดยกเลิกเพื่อบูตเข้าระบบตามปกติ + + + + Cancel + ยกเลิก + + + + Reboot + รีบูต + + + + Confirm + ยืนยัน + + + + Unable to mount data partition. Press confirm to reset your device. + ไม่สามารถเมานต์พาร์ติชั่นข้อมูล กดยืนยันเพื่อรีเซ็ตอุปกรณ์ของคุณ + + + + RichTextDialog + + + Ok + ตกลง + + + + SettingsWindow + + + × + × + + + + Device + อุปกรณ์ + + + + + Network + เครือข่าย + + + + Toggles + ตัวเลือก + + + + Software + ซอฟต์แวร์ + + + + Navigation + การนำทาง + + + + Setup + + + WARNING: Low Voltage + คำเตือน: แรงดันแบตเตอรี่ต่ำ + + + + Power your device in a car with a harness or proceed at your own risk. + โปรดต่ออุปกรณ์ของคุณเข้ากับสายควบคุมในรถยนต์ หรือดำเนินการด้วยความเสี่ยงของคุณเอง + + + + Power off + ปิดเครื่อง + + + + + + Continue + ดำเนินการต่อ + + + + Getting Started + เริ่มกันเลย + + + + Before we get on the road, let’s finish installation and cover some details. + ก่อนออกเดินทาง เรามาทำการติดตั้งซอฟต์แวร์ และตรวจสอบการตั้งค่า + + + + Connect to Wi-Fi + เชื่อมต่อ Wi-Fi + + + + + Back + ย้อนกลับ + + + + Continue without Wi-Fi + ดำเนินการต่อโดยไม่ใช้ Wi-Fi + + + + Waiting for internet + กำลังรอสัญญาณอินเตอร์เน็ต + + + + Choose Software to Install + เลือกซอฟต์แวร์ที่จะติดตั้ง + + + + Dashcam + กล้องติดรถยนต์ + + + + Custom Software + ซอฟต์แวร์ที่กำหนดเอง + + + + Enter URL + ป้อน URL + + + + for Custom Software + สำหรับซอฟต์แวร์ที่กำหนดเอง + + + + Downloading... + กำลังดาวน์โหลด... + + + + Download Failed + ดาวน์โหลดล้มเหลว + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + ตรวจสอบให้แน่ใจว่า URL ที่ป้อนนั้นถูกต้อง และอุปกรณ์เชื่อมต่ออินเทอร์เน็ตอยู่ + + + + Reboot device + รีบูตอุปกรณ์ + + + + Start over + เริ่มต้นใหม่ + + + + SetupWidget + + + Finish Setup + ตั้งค่าเสร็จสิ้น + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + จับคู่อุปกรณ์ของคุณกับ comma connect (connect.comma.ai) และรับข้อเสนอ comma prime ของคุณ + + + + Pair device + จับคู่อุปกรณ์ + + + + Sidebar + + + + CONNECT + เชื่อมต่อ + + + + OFFLINE + ออฟไลน์ + + + + + ONLINE + ออนไลน์ + + + + ERROR + เกิดข้อผิดพลาด + + + + + + TEMP + อุณหภูมิ + + + + HIGH + สูง + + + + GOOD + ดี + + + + OK + พอใช้ + + + + VEHICLE + รถยนต์ + + + + NO + ไม่พบ + + + + PANDA + PANDA + + + + GPS + จีพีเอส + + + + SEARCH + ค้นหา + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + ETH + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git Branch + + + + Git Commit + Git Commit + + + + OS Version + เวอร์ชันระบบปฏิบัติการ + + + + Version + เวอร์ชั่น + + + + Last Update Check + ตรวจสอบการอัปเดตล่าสุด + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + ครั้งสุดท้ายที่ openpilot ตรวจสอบการอัปเดตสำเร็จ ตัวอัปเดตจะทำงานในขณะที่รถดับเครื่องอยู่เท่านั้น + + + + Check for Update + ตรวจสอบการอัปเดต + + + + CHECKING + กำลังตรวจสอบ + + + + Switch Branch + เปลี่ยน Branch + + + + ENTER + เปลี่ยน + + + + + The new branch will be pulled the next time the updater runs. + Branch ใหม่จะถูกติดตั้งในครั้งต่อไปที่ตัวอัปเดตทำงาน + + + + Enter branch name + ใส่ชื่อ Branch + + + + Uninstall %1 + ถอนการติดตั้ง %1 + + + + UNINSTALL + ถอนการติดตั้ง + + + + Are you sure you want to uninstall? + คุณแน่ใจหรือไม่ว่าต้องการถอนการติดตั้ง? + + + + failed to fetch update + โหลดข้อมูลอัปเดตไม่สำเร็จ + + + + + CHECK + ตรวจสอบ + + + + SshControl + + + SSH Keys + คีย์ SSH + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + คำเตือน: สิ่งนี้ให้สิทธิ์ SSH เข้าถึงคีย์สาธารณะทั้งหมดใน GitHub ของคุณ อย่าป้อนชื่อผู้ใช้ GitHub อื่นนอกเหนือจากของคุณเอง พนักงาน comma จะไม่ขอให้คุณเพิ่มชื่อผู้ใช้ GitHub ของพวกเขา + + + + + ADD + เพิ่ม + + + + Enter your GitHub username + ป้อนชื่อผู้ใช้ GitHub ของคุณ + + + + LOADING + กำลังโหลด + + + + REMOVE + ลบ + + + + Username '%1' has no keys on GitHub + ชื่อผู้ใช้ '%1' ไม่มีคีย์บน GitHub + + + + Request timed out + ตรวจสอบไม่สำเร็จ เนื่องจากใช้เวลามากเกินไป + + + + Username '%1' doesn't exist on GitHub + ไม่พบชื่อผู้ใช้ '%1' บน GitHub + + + + SshToggle + + + Enable SSH + เปิดใช้งาน SSH + + + + TermsPage + + + Terms & Conditions + ข้อตกลงและเงื่อนไข + + + + Decline + ปฏิเสธ + + + + Scroll to accept + เลื่อนเพื่อตอบรับข้อตกลง + + + + Agree + ยอมรับ + + + + TogglesPanel + + + Enable openpilot + เปิดใช้งาน openpilot + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + ใช้ระบบ openpilot สำหรับระบบควบคุมความเร็วอัตโนมัติ และระบบช่วยควบคุมรถให้อยู่ในเลน คุณจำเป็นต้องให้ความสนใจตลอดเวลาที่ใช้คุณสมบัตินี้ การเปลี่ยนการตั้งค่านี้จะมีผลเมื่อคุณดับเครื่องยนต์ + + + + Enable Lane Departure Warnings + เปิดใช้งานการเตือนการออกนอกเลน + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + รับการแจ้งเตือนให้เลี้ยวกลับเข้าเลนเมื่อรถของคุณตรวจพบการข้ามช่องจราจรโดยไม่เปิดสัญญาณไฟเลี้ยวในขณะขับขี่ที่ความเร็วเกิน 31 ไมล์ต่อชั่วโมง (50 กม./ชม) + + + + Use Metric System + ใช้ระบบเมตริก + + + + Display speed in km/h instead of mph. + แสดงความเร็วเป็น กม./ชม. แทน ไมล์/ชั่วโมง + + + + Record and Upload Driver Camera + บันทึกและอัปโหลดภาพจากกล้องคนขับ + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + อัปโหลดข้อมูลจากกล้องที่หันหน้าไปทางคนขับ และช่วยปรับปรุงอัลกอริธึมการตรวจสอบผู้ขับขี่ + + + + Disengage On Accelerator Pedal + ยกเลิกระบบช่วยขับเมื่อเหยียบคันเร่ง + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + เมื่อเปิดใช้งาน การกดแป้นคันเร่งจะเป็นการยกเลิกระบบช่วยขับโดย openpilot + + + + Show ETA in 24h Format + แสดงเวลา ETA ในรูปแบบ 24 ชั่วโมง + + + + Use 24h format instead of am/pm + ใช้รูปแบบเวลา 24 ชั่วโมง แทน am/pm + + + + Show Map on Left Side of UI + แสดงแผนที่ที่ด้านซ้ายของหน้าจอ + + + + Show map on left side when in split screen view. + แสดงแผนที่ด้านซ้ายของหน้าจอเมื่ออยู่ในโหมดแบ่งหน้าจอ + + + + openpilot Longitudinal Control + openpilot การควบคุมการเร่งและลดความเร็ว + + + + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! + openpilot จะปิดการใช้งานเรดาร์ของรถ และจะเข้าควบคุมการเร่งและเบรก คำเตือน: สิ่งนี้จะปิดระบบ AEB! + + + + Updater + + + Update Required + จำเป็นต้องอัปเดต + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + จำเป็นต้องมีการอัปเดตระบบปฏิบัติการ เชื่อมต่ออุปกรณ์ของคุณกับ Wi-Fi เพื่อประสบการณ์การอัปเดตที่เร็วที่สุด ขนาดดาวน์โหลดประมาณ 1GB + + + + Connect to Wi-Fi + เชื่อมต่อกับ Wi-Fi + + + + Install + ติดตั้ง + + + + Back + ย้อนกลับ + + + + Loading... + กำลังโหลด... + + + + Reboot + รีบูต + + + + Update failed + การอัปเดตล้มเหลว + + + + WifiUI + + + + Scanning for networks... + กำลังสแกนหาเครือข่าย... + + + + CONNECTING... + กำลังเชื่อมต่อ... + + + + FORGET + เลิกใช้ + + + + Forget Wi-Fi Network "%1"? + เลิกใช้เครือข่าย Wi-Fi "%1"? + + + diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts new file mode 100644 index 00000000000000..a00bf28303e5f5 --- /dev/null +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -0,0 +1,1311 @@ + + + + + AbstractAlert + + + Close + 关闭 + + + + Snooze Update + 暂停更新 + + + + Reboot and Update + 重启并更新 + + + + AdvancedNetworking + + + Back + 返回 + + + + Enable Tethering + 启用WiFi热点 + + + + Tethering Password + WiFi热点密码 + + + + + EDIT + 编辑 + + + + Enter new tethering password + 输入新的WiFi热点密码 + + + + IP Address + IP地址 + + + + Enable Roaming + 启用数据漫游 + + + + APN Setting + APN设置 + + + + Enter APN + 输入APN + + + + leave blank for automatic configuration + 留空以自动配置 + + + + ConfirmationDialog + + + + Ok + 好的 + + + + Cancel + 取消 + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + 您必须接受条款和条件以使用openpilot。 + + + + Back + 返回 + + + + Decline, uninstall %1 + 拒绝并卸载%1 + + + + DevicePanel + + + Dongle ID + 设备ID（Dongle ID） + + + + N/A + N/A + + + + Serial + 序列号 + + + + Driver Camera + 驾驶员摄像头 + + + + PREVIEW + 预览 + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + 打开并预览驾驶员摄像头，以确保驾驶员监控具有良好视野。仅熄火时可用。 + + + + Reset Calibration + 重置设备校准 + + + + RESET + 重置 + + + + Are you sure you want to reset calibration? + 您确定要重置设备校准吗？ + + + + Review Training Guide + 新手指南 + + + + REVIEW + 查看 + + + + Review the rules, features, and limitations of openpilot + 查看openpilot的使用规则，以及其功能和限制。 + + + + Are you sure you want to review the training guide? + 您确定要查看新手指南吗？ + + + + Regulatory + 监管信息 + + + + VIEW + 查看 + + + + Change Language + 切换语言 + + + + CHANGE + 切换 + + + + Select a language + 选择语言 + + + + Reboot + 重启 + + + + Power Off + 关机 + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot要求设备安装的偏航角在左4°和右4°之间，俯仰角在上5°和下8°之间。一般来说，openpilot会持续更新校准，很少需要重置。 + + + + Your device is pointed %1° %2 and %3° %4. + 您的设备校准为%1° %2、%3° %4。 + + + + down + 朝下 + + + + up + 朝上 + + + + left + 朝左 + + + + right + 朝右 + + + + Are you sure you want to reboot? + 您确定要重新启动吗？ + + + + Disengage to Reboot + 取消openpilot以重新启动 + + + + Are you sure you want to power off? + 您确定要关机吗？ + + + + Disengage to Power Off + 取消openpilot以关机 + + + + DriveStats + + + Drives + 旅程数 + + + + Hours + 小时 + + + + ALL TIME + 全部 + + + + PAST WEEK + 过去一周 + + + + KM + 公里 + + + + Miles + 英里 + + + + DriverViewScene + + + camera starting + 正在启动相机 + + + + InputDialog + + + Cancel + 取消 + + + + Need at least %n character(s)! + + 至少需要 %n 个字符！ + + + + + Installer + + + Installing... + 正在安装…… + + + + Receiving objects: + 正在接收： + + + + Resolving deltas: + 正在处理： + + + + Updating files: + 正在更新文件： + + + + MapETA + + + eta + 埃塔 + + + + min + 分钟 + + + + hr + 小时 + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + mi + + + + ft + ft + + + + MapPanel + + + Current Destination + 当前目的地 + + + + CLEAR + 清空 + + + + Recent Destinations + 最近目的地 + + + + Try the Navigation Beta + 试用导航测试版 + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + 订阅comma prime以获取导航。 +立即注册：https://connect.comma.ai + + + + No home +location set + 家：未设定 + + + + No work +location set + 工作：未设定 + + + + no recent destinations + 无最近目的地 + + + + MapWindow + + + Map Loading + 地图加载中 + + + + Waiting for GPS + 等待 GPS + + + + MultiOptionDialog + + + Select + 选择 + + + + Cancel + 取消 + + + + Networking + + + Advanced + 高级 + + + + Enter password + 输入密码 + + + + + for "%1" + 网络名称："%1" + + + + Wrong password + 密码错误 + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + 最高定速 + + + + + SPEED + SPEED + + + + + LIMIT + LIMIT + + + + OffroadHome + + + UPDATE + 更新 + + + + ALERTS + 警报 + + + + ALERT + 警报 + + + + PairingPopup + + + Pair your device to your comma account + 将您的设备与comma账号配对 + + + + Go to https://connect.comma.ai on your phone + 在手机上访问 https://connect.comma.ai + + + + Click "add new device" and scan the QR code on the right + 点击“添加新设备”，扫描右侧二维码 + + + + Bookmark connect.comma.ai to your home screen to use it like an app + 将 connect.comma.ai 收藏到您的主屏幕，以便像应用程序一样使用它 + + + + PrimeAdWidget + + + Upgrade Now + 现在升级 + + + + Become a comma prime member at connect.comma.ai + 打开connect.comma.ai以注册comma prime会员 + + + + PRIME FEATURES: + comma prime特权： + + + + Remote access + 远程访问 + + + + 1 year of storage + 1年数据存储 + + + + Developer perks + 开发者福利 + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ 已订阅 + + + + comma prime + comma prime + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA POINTS点数 + + + + QObject + + + Reboot + 重启 + + + + Exit + 退出 + + + + dashcam + 行车记录仪 + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n 分钟前 + + + + + %n hour(s) ago + + %n 小时前 + + + + + %n day(s) ago + + %n 天前 + + + + + Reset + + + Reset failed. Reboot to try again. + 重置失败。 重新启动以重试。 + + + + Are you sure you want to reset your device? + 您确定要重置您的设备吗？ + + + + Resetting device... + 正在重置设备…… + + + + System Reset + 恢复出厂设置 + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + 已触发系统重置：确认以删除所有内容和设置。取消以正常启动设备。 + + + + Cancel + 取消 + + + + Reboot + 重启 + + + + Confirm + 确认 + + + + Unable to mount data partition. Press confirm to reset your device. + 无法挂载数据分区。 确认以重置您的设备。 + + + + RichTextDialog + + + Ok + 好的 + + + + SettingsWindow + + + × + × + + + + Device + 设备 + + + + + Network + 网络 + + + + Toggles + 设定 + + + + Software + 软件 + + + + Navigation + 导航 + + + + Setup + + + WARNING: Low Voltage + 警告：低电压 + + + + Power your device in a car with a harness or proceed at your own risk. + 请使用car harness线束为您的设备供电，或自行承担风险。 + + + + Power off + 关机 + + + + + + Continue + 继续 + + + + Getting Started + 开始设置 + + + + Before we get on the road, let’s finish installation and cover some details. + 开始旅程之前，让我们完成安装并介绍一些细节。 + + + + Connect to Wi-Fi + 连接到WiFi + + + + + Back + 返回 + + + + Continue without Wi-Fi + 不连接WiFi并继续 + + + + Waiting for internet + 等待网络连接 + + + + Choose Software to Install + 选择要安装的软件 + + + + Dashcam + Dashcam（行车记录仪） + + + + Custom Software + 自定义软件 + + + + Enter URL + 输入网址 + + + + for Custom Software + 以下载自定义软件 + + + + Downloading... + 正在下载…… + + + + Download Failed + 下载失败 + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + 请确保互联网连接良好且输入的URL有效。 + + + + Reboot device + 重启设备 + + + + Start over + 重来 + + + + SetupWidget + + + Finish Setup + 完成设置 + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 将您的设备与comma connect （connect.comma.ai）配对并领取您的comma prime优惠。 + + + + Pair device + 配对设备 + + + + Sidebar + + + + CONNECT + CONNECT + + + + OFFLINE + 离线 + + + + + ONLINE + 在线 + + + + ERROR + 连接出错 + + + + + + TEMP + 设备温度 + + + + HIGH + 过热 + + + + GOOD + 良好 + + + + OK + 一般 + + + + VEHICLE + 车辆连接 + + + + NO + 无 + + + + PANDA + PANDA + + + + GPS + GPS + + + + SEARCH + 搜索中 + + + + -- + -- + + + + Wi-Fi + Wi-Fi + + + + ETH + 以太网 + + + + 2G + 2G + + + + 3G + 3G + + + + LTE + LTE + + + + 5G + 5G + + + + SoftwarePanel + + + Git Branch + Git Branch + + + + Git Commit + Git Commit + + + + OS Version + 系统版本 + + + + Version + 软件版本 + + + + Last Update Check + 上次检查更新 + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + 上一次成功检查更新的时间。更新程序仅在汽车熄火时运行。 + + + + Check for Update + 检查更新 + + + + CHECKING + 正在检查更新 + + + + Switch Branch + 切换分支 + + + + ENTER + 输入 + + + + + The new branch will be pulled the next time the updater runs. + 分支将在更新服务下次启动时自动切换。 + + + + Enter branch name + 输入分支名称 + + + + UNINSTALL + 卸载 + + + + Uninstall %1 + 卸载 %1 + + + + Are you sure you want to uninstall? + 您确定要卸载吗？ + + + + failed to fetch update + 获取更新失败 + + + + + CHECK + 查看 + + + + SshControl + + + SSH Keys + SSH密钥 + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + 警告：这将授予SSH访问权限给您GitHub设置中的所有公钥。切勿输入您自己以外的GitHub用户名。comma员工永远不会要求您添加他们的GitHub用户名。 + + + + + ADD + 添加 + + + + Enter your GitHub username + 输入您的GitHub用户名 + + + + LOADING + 正在加载 + + + + REMOVE + 删除 + + + + Username '%1' has no keys on GitHub + 用户名“%1”在GitHub上没有密钥 + + + + Request timed out + 请求超时 + + + + Username '%1' doesn't exist on GitHub + GitHub上不存在用户名“%1” + + + + SshToggle + + + Enable SSH + 启用SSH + + + + TermsPage + + + Terms & Conditions + 条款和条件 + + + + Decline + 拒绝 + + + + Scroll to accept + 滑动以接受 + + + + Agree + 同意 + + + + TogglesPanel + + + Enable openpilot + 启用openpilot + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + 使用openpilot进行自适应巡航和车道保持辅助。使用此功能时您必须时刻保持注意力。该设置的更改在熄火时生效。 + + + + Enable Lane Departure Warnings + 启用车道偏离警告 + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + 车速超过31mph（50km/h）时，若检测到车辆越过车道线且未打转向灯，系统将发出警告以提醒您返回车道。 + + + + Use Metric System + 使用公制单位 + + + + Display speed in km/h instead of mph. + 显示车速时，以km/h代替mph。 + + + + Record and Upload Driver Camera + 录制并上传驾驶员摄像头 + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + 上传驾驶员摄像头的数据，帮助改进驾驶员监控算法。 + + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + + + + + Experimental openpilot longitudinal control + + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + + Disengage On Accelerator Pedal + 踩油门时取消控制 + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + 启用后，踩下油门踏板将取消openpilot。 + + + + Show ETA in 24h Format + 以24小时格式显示预计到达时间 + + + + Use 24h format instead of am/pm + 使用24小时制代替am/pm + + + + Show Map on Left Side of UI + 在介面左侧显示地图 + + + + Show map on left side when in split screen view. + 在分屏模式中，将地图置于屏幕左侧。 + + + + Updater + + + Update Required + 需要更新 + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + 操作系统需要更新。请将您的设备连接到WiFi以获取更快的更新体验。下载大小约为1GB。 + + + + Connect to Wi-Fi + 连接到WiFi + + + + Install + 安装 + + + + Back + 返回 + + + + Loading... + 正在加载…… + + + + Reboot + 重启 + + + + Update failed + 更新失败 + + + + WifiUI + + + + Scanning for networks... + 正在扫描网络…… + + + + CONNECTING... + 正在连接…… + + + + FORGET + 忘记 + + + + Forget Wi-Fi Network "%1"? + 忘记WiFi网络 "%1"? + + + diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts new file mode 100644 index 00000000000000..52bd3013647127 --- /dev/null +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -0,0 +1,1313 @@ + + + + + AbstractAlert + + + Close + 關閉 + + + + Snooze Update + 暫停更新 + + + + Reboot and Update + 重啟並更新 + + + + AdvancedNetworking + + + Back + 回上頁 + + + + Enable Tethering + 啟用網路分享 + + + + Tethering Password + 網路分享密碼 + + + + + EDIT + 編輯 + + + + Enter new tethering password + 輸入新的網路分享密碼 + + + + IP Address + IP 地址 + + + + Enable Roaming + 啟用漫遊 + + + + APN Setting + APN 設置 + + + + Enter APN + 輸入 APN + + + + leave blank for automatic configuration + 留空白將自動配置 + + + + ConfirmationDialog + + + + Ok + 確定 + + + + Cancel + 取消 + + + + DeclinePage + + + You must accept the Terms and Conditions in order to use openpilot. + 您必須先接受條款和條件才能使用 openpilot。 + + + + Back + 回上頁 + + + + Decline, uninstall %1 + 拒絕並卸載 %1 + + + + DevicePanel + + + Dongle ID + Dongle ID + + + + N/A + 無法使用 + + + + Serial + 序號 + + + + Driver Camera + 駕駛員攝像頭 + + + + PREVIEW + 預覽 + + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + 預覽駕駛員監控鏡頭畫面，以確保其具有良好視野。（僅在熄火時可用） + + + + Reset Calibration + 重置校準 + + + + RESET + 重置 + + + + Are you sure you want to reset calibration? + 您確定要重置校準嗎？ + + + + Review Training Guide + 觀看使用教學 + + + + REVIEW + 觀看 + + + + Review the rules, features, and limitations of openpilot + 觀看 openpilot 的使用規則、功能和限制 + + + + Are you sure you want to review the training guide? + 您確定要觀看使用教學嗎？ + + + + Regulatory + 法規/監管 + + + + VIEW + 觀看 + + + + Change Language + 更改語言 + + + + CHANGE + 更改 + + + + Select a language + 選擇語言 + + + + Reboot + 重新啟動 + + + + Power Off + 關機 + + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + openpilot 需要將裝置固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。 + + + + Your device is pointed %1° %2 and %3° %4. + 你的設備目前朝%2 %1° 以及朝%4 %3° 。 + + + + down + 下 + + + + up + 上 + + + + left + 左 + + + + right + 右 + + + + Are you sure you want to reboot? + 您確定要重新啟動嗎？ + + + + Disengage to Reboot + 請先取消控車才能重新啟動 + + + + Are you sure you want to power off? + 您確定您要關機嗎？ + + + + Disengage to Power Off + 請先取消控車才能關機 + + + + DriveStats + + + Drives + 旅程 + + + + Hours + 小時 + + + + ALL TIME + 總共 + + + + PAST WEEK + 上周 + + + + KM + 公里 + + + + Miles + 英里 + + + + DriverViewScene + + + camera starting + 開啟相機中 + + + + InputDialog + + + Cancel + 取消 + + + + Need at least %n character(s)! + + 需要至少 %n 個字元！ + + + + + Installer + + + Installing... + 安裝中… + + + + Receiving objects: + 接收對象： + + + + Resolving deltas: + 分析差異： + + + + Updating files: + 更新檔案： + + + + MapETA + + + eta + 抵達 + + + + min + 分鐘 + + + + hr + 小時 + + + + km + km + + + + mi + mi + + + + MapInstructions + + + km + km + + + + m + m + + + + mi + mi + + + + ft + ft + + + + MapPanel + + + Current Destination + 當前目的地 + + + + CLEAR + 清除 + + + + Recent Destinations + 最近目的地 + + + + Try the Navigation Beta + 試用導航功能 + + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + 成為 comma 高級會員來使用導航功能 +立即註冊：https://connect.comma.ai + + + + No home +location set + 未設定 +住家位置 + + + + No work +location set + 未設定 +工作位置 + + + + no recent destinations + 沒有最近的導航記錄 + + + + MapWindow + + + Map Loading + 地圖加載中 + + + + Waiting for GPS + 等待 GPS + + + + MultiOptionDialog + + + Select + 選擇 + + + + Cancel + 取消 + + + + Networking + + + Advanced + 進階 + + + + Enter password + 輸入密碼 + + + + + for "%1" + 給 "%1" + + + + Wrong password + 密碼錯誤 + + + + NvgWindow + + + km/h + km/h + + + + mph + mph + + + + + MAX + 最高 + + + + + SPEED + 速度 + + + + + LIMIT + 速限 + + + + OffroadHome + + + UPDATE + 更新 + + + + ALERTS + 提醒 + + + + ALERT + 提醒 + + + + PairingPopup + + + Pair your device to your comma account + 將設備與您的 comma 帳號配對 + + + + Go to https://connect.comma.ai on your phone + 用手機連至 https://connect.comma.ai + + + + Click "add new device" and scan the QR code on the right + 點選 "add new device" 後掃描右邊的二維碼 + + + + Bookmark connect.comma.ai to your home screen to use it like an app + 將 connect.comma.ai 加入您的主屏幕，以便像手機 App 一樣使用它 + + + + PrimeAdWidget + + + Upgrade Now + 馬上升級 + + + + Become a comma prime member at connect.comma.ai + 成為 connect.comma.ai 的高級會員 + + + + PRIME FEATURES: + 高級會員特點： + + + + Remote access + 遠程訪問 + + + + 1 year of storage + 一年的雲端行車記錄 + + + + Developer perks + 開發者福利 + + + + PrimeUserWidget + + + ✓ SUBSCRIBED + ✓ 已訂閱 + + + + comma prime + comma 高級會員 + + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + + COMMA POINTS + COMMA 積分 + + + + QObject + + + Reboot + 重新啟動 + + + + Exit + 離開 + + + + dashcam + 行車記錄器 + + + + openpilot + openpilot + + + + %n minute(s) ago + + %n 分鐘前 + + + + + %n hour(s) ago + + %n 小時前 + + + + + %n day(s) ago + + %n 天前 + + + + + Reset + + + Reset failed. Reboot to try again. + 重置失敗。請重新啟動後再試。 + + + + Are you sure you want to reset your device? + 您確定要重置你的設備嗎？ + + + + Resetting device... + 重置設備中… + + + + System Reset + 系統重置 + + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + 系統重置已觸發。請按確認刪除所有內容和設置。按取消恢復啟動。 + + + + Cancel + 取消 + + + + Reboot + 重新啟動 + + + + Confirm + 確認 + + + + Unable to mount data partition. Press confirm to reset your device. + 無法掛載數據分區。請按確認重置您的設備。 + + + + RichTextDialog + + + Ok + 確定 + + + + SettingsWindow + + + × + × + + + + Device + 設備 + + + + + Network + 網路 + + + + Toggles + 設定 + + + + Software + 軟體 + + + + Navigation + 導航 + + + + Setup + + + WARNING: Low Voltage + 警告：電壓過低 + + + + Power your device in a car with a harness or proceed at your own risk. + 請使用車上 harness 提供的電源，若繼續的話您需要自擔風險。 + + + + Power off + 關機 + + + + + + Continue + 繼續 + + + + Getting Started + 入門 + + + + Before we get on the road, let’s finish installation and cover some details. + 在我們上路之前，讓我們完成安裝並介紹一些細節。 + + + + Connect to Wi-Fi + 連接到無線網絡 + + + + + Back + 回上頁 + + + + Continue without Wi-Fi + 在沒有 Wi-Fi 的情況下繼續 + + + + Waiting for internet + 連接至網路中 + + + + Choose Software to Install + 選擇要安裝的軟體 + + + + Dashcam + 行車記錄器 + + + + Custom Software + 定制的軟體 + + + + Enter URL + 輸入網址 + + + + for Custom Software + 定制的軟體 + + + + Downloading... + 下載中… + + + + Download Failed + 下載失敗 + + + + Ensure the entered URL is valid, and the device’s internet connection is good. + 請確定您輸入的是有效的安裝網址，並且確定設備的網路連線狀態良好。 + + + + Reboot device + 重新啟動 + + + + Start over + 重新開始 + + + + SetupWidget + + + Finish Setup + 完成設置 + + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 將您的設備與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 + + + + Pair device + 配對設備 + + + + Sidebar + + + + CONNECT + 雲端服務 + + + + OFFLINE + 已離線 + + + + + ONLINE + 已連線 + + + + ERROR + 錯誤 + + + + + + TEMP + 溫度 + + + + HIGH + 偏高 + + + + GOOD + 正常 + + + + OK + 一般 + + + + VEHICLE + 車輛通訊 + + + + NO + 未連線 + + + + PANDA + 車輛通訊 + + + + GPS + GPS + + + + SEARCH + 車輛通訊 + + + + -- + -- + + + + Wi-Fi + + + + + ETH + + + + + 2G + + + + + 3G + + + + + LTE + + + + + 5G + + + + + SoftwarePanel + + + Git Branch + Git 分支 + + + + Git Commit + Git 提交 + + + + OS Version + 系統版本 + + + + Version + 版本 + + + + Last Update Check + 上次檢查時間 + + + + The last time openpilot successfully checked for an update. The updater only runs while the car is off. + 上次成功檢查更新的時間。更新系統只會在車子熄火時執行。 + + + + Check for Update + 檢查更新 + + + + CHECKING + 檢查中 + + + + Switch Branch + 切換分支 + + + + ENTER + 切換 + + + + + The new branch will be pulled the next time the updater runs. + 新的分支將會在下次檢查更新時切換過去。 + + + + Enter branch name + 輸入分支名稱 + + + + UNINSTALL + 卸載 + + + + Uninstall %1 + 卸載 %1 + + + + Are you sure you want to uninstall? + 您確定您要卸載嗎？ + + + + failed to fetch update + 下載更新失敗 + + + + + CHECK + 檢查 + + + + SshControl + + + SSH Keys + SSH 密鑰 + + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + 警告：這將授權給 GitHub 帳號中所有公鑰 SSH 訪問權限。切勿輸入非您自己的 GitHub 用戶名。comma 員工「永遠不會」要求您添加他們的 GitHub 用戶名。 + + + + + ADD + 新增 + + + + Enter your GitHub username + 請輸入您 GitHub 的用戶名 + + + + LOADING + 載入中 + + + + REMOVE + 移除 + + + + Username '%1' has no keys on GitHub + GitHub 用戶 '%1' 沒有設定任何密鑰 + + + + Request timed out + 請求超時 + + + + Username '%1' doesn't exist on GitHub + GitHub 用戶 '%1' 不存在 + + + + SshToggle + + + Enable SSH + 啟用 SSH 服務 + + + + TermsPage + + + Terms & Conditions + 條款和條件 + + + + Decline + 拒絕 + + + + Scroll to accept + 滑動至頁尾接受條款 + + + + Agree + 接受 + + + + TogglesPanel + + + Enable openpilot + 啟用 openpilot + + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + 使用 openpilot 的主動式巡航和車道保持功能，開啟後您需要持續集中注意力，設定變更在重新啟動車輛後生效。 + + + + Enable Lane Departure Warnings + 啟用車道偏離警告 + + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + 車速在時速 50 公里 (31 英里) 以上且未打方向燈的情況下，如果偵測到車輛駛出目前車道線時，發出車道偏離警告。 + + + + Use Metric System + 使用公制單位 + + + + Display speed in km/h instead of mph. + 啟用後，速度單位顯示將從 mp/h 改為 km/h。 + + + + Record and Upload Driver Camera + 記錄並上傳駕駛監控影像 + + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + 上傳駕駛監控的錄像來協助我們提升駕駛監控的準確率。 + + + + 🌮 End-to-end longitudinal (extremely alpha) 🌮 + + + + + Experimental openpilot longitudinal control + + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + + + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + + + + + Disengage On Accelerator Pedal + 油門取消控車 + + + + When enabled, pressing the accelerator pedal will disengage openpilot. + 啟用後，踩踏油門將會取消 openpilot 控制。 + + + + Show ETA in 24h Format + 預計到達時間單位改用 24 小時制 + + + + Use 24h format instead of am/pm + 使用 24 小時制。(預設值為 12 小時制) + + + + Show Map on Left Side of UI + 將地圖顯示在畫面的左側 + + + + Show map on left side when in split screen view. + 進入分割畫面後，地圖將會顯示在畫面的左側。 + + + + Updater + + + Update Required + 系統更新 + + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + 設備的操作系統需要更新。請將您的設備連接到 Wi-Fi 以獲得最快的更新體驗。下載大小約為 1GB。 + + + + Connect to Wi-Fi + 連接到無線網絡 + + + + Install + 安裝 + + + + Back + 回上頁 + + + + Loading... + 載入中… + + + + Reboot + 重新啟動 + + + + Update failed + 更新失敗 + + + + WifiUI + + + + Scanning for networks... + 掃描無線網路中... + + + + CONNECTING... + 連線中... + + + + FORGET + 清除 + + + + Forget Wi-Fi Network "%1"? + 清除 Wi-Fi 網路 "%1"? + + + diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui new file mode 100755 index 00000000000000..c9f81c05392442 --- /dev/null +++ b/selfdrive/ui/ui @@ -0,0 +1,5 @@ +#!/bin/sh +cd "$(dirname "$0")" +export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" +export QT_DBL_CLICK_DIST=150 +exec ./_ui diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc new file mode 100644 index 00000000000000..b208945fe21aa2 --- /dev/null +++ b/selfdrive/ui/ui.cc @@ -0,0 +1,344 @@ +#include "selfdrive/ui/ui.h" + +#include +#include + +#include + +#include "common/transformations/orientation.hpp" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "common/watchdog.h" +#include "system/hardware/hw.h" + +#define BACKLIGHT_DT 0.05 +#define BACKLIGHT_TS 10.00 +#define BACKLIGHT_OFFROAD 50 + +// Projects a point in car to space to the corresponding point in full frame +// image space. +static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, QPointF *out) { + const float margin = 500.0f; + const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; + + const vec3 pt = (vec3){{in_x, in_y, in_z}}; + const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); + const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); + + // Project. + QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); + if (clip_region.contains(point)) { + *out = point; + return true; + } + return false; +} + +static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { + const auto line_x = line.getX(); + int max_idx = 0; + for (int i = 1; i < TRAJECTORY_SIZE && line_x[i] <= path_height; ++i) { + max_idx = i; + } + return max_idx; +} + +static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line) { + for (int i = 0; i < 2; ++i) { + auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); + if (lead_data.getStatus()) { + float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; + calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]); + } + } +} + +static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, + float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { + const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); + + QPolygonF left_points, right_points; + left_points.reserve(max_idx + 1); + right_points.reserve(max_idx + 1); + + for (int i = 0; i <= max_idx; i++) { + QPointF left, right; + bool l = calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left); + bool r = calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right); + if (l && r) { + // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts + if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) { + continue; + } + left_points.push_back(left); + right_points.push_front(right); + } + } + *pvd = left_points + right_points; +} + +static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { + UIScene &scene = s->scene; + auto model_position = model.getPosition(); + float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1], + MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); + + // update lane lines + const auto lane_lines = model.getLaneLines(); + const auto lane_line_probs = model.getLaneLineProbs(); + int max_idx = get_path_length_idx(lane_lines[0], max_distance); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + scene.lane_line_probs[i] = lane_line_probs[i]; + update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_idx); + } + + // update road edges + const auto road_edges = model.getRoadEdges(); + const auto road_edge_stds = model.getRoadEdgeStds(); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + scene.road_edge_stds[i] = road_edge_stds[i]; + update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx); + } + + // update path + auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne(); + if (lead_one.getStatus()) { + const float lead_d = lead_one.getDRel() * 2.; + max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); + } + max_idx = get_path_length_idx(model_position, max_distance); + update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); +} + +static void update_sockets(UIState *s) { + s->sm->update(0); +} + +static void update_state(UIState *s) { + SubMaster &sm = *(s->sm); + UIScene &scene = s->scene; + + if (sm.updated("liveCalibration")) { + auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + Eigen::Vector3d rpy; + rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + Eigen::Matrix3d device_from_calib = euler2rot(rpy); + Eigen::Matrix3d view_from_device; + view_from_device << 0,1,0, + 0,0,1, + 1,0,0; + Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); + } + } + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; + } + if (s->worldObjectsVisible()) { + if (sm.updated("modelV2")) { + update_model(s, sm["modelV2"].getModelV2()); + } + if (sm.updated("radarState") && sm.rcv_frame("modelV2") > s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); + } + } + if (sm.updated("pandaStates")) { + auto pandaStates = sm["pandaStates"].getPandaStates(); + if (pandaStates.size() > 0) { + scene.pandaType = pandaStates[0].getPandaType(); + + if (scene.pandaType != cereal::PandaState::PandaType::UNKNOWN) { + scene.ignition = false; + for (const auto& pandaState : pandaStates) { + scene.ignition |= pandaState.getIgnitionLine() || pandaState.getIgnitionCan(); + } + } + } + } else if ((s->sm->frame - s->sm->rcv_frame("pandaStates")) > 5*UI_FREQ) { + scene.pandaType = cereal::PandaState::PandaType::UNKNOWN; + } + if (sm.updated("carParams")) { + scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); + } + if (!scene.started && sm.updated("sensorEvents")) { + for (auto sensor : sm["sensorEvents"].getSensorEvents()) { + if (sensor.which() == cereal::SensorEventData::ACCELERATION) { + auto accel = sensor.getAcceleration().getV(); + if (accel.totalSize().wordCount) { // TODO: sometimes empty lists are received. Figure out why + scene.accel_sensor = accel[2]; + } + } else if (sensor.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) { + auto gyro = sensor.getGyroUncalibrated().getV(); + if (gyro.totalSize().wordCount) { + scene.gyro_sensor = gyro[1]; + } + } + } + } + if (sm.updated("wideRoadCameraState")) { + auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); + + float max_lines = 1618; + float max_gain = 10.0; + float max_ev = max_lines * max_gain / 6; + + float ev = camera_state.getGain() * float(camera_state.getIntegLines()); + + scene.light_sensor = std::clamp(1.0 - (ev / max_ev), 0.0, 1.0); + } + scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; +} + +void ui_update_params(UIState *s) { + auto params = Params(); + s->scene.is_metric = params.getBool("IsMetric"); + s->scene.map_on_left = params.getBool("NavSettingLeftSide"); + s->scene.end_to_end_long = params.getBool("EndToEndLong"); +} + +void UIState::updateStatus() { + if (scene.started && sm->updated("controlsState")) { + auto controls_state = (*sm)["controlsState"].getControlsState(); + auto alert_status = controls_state.getAlertStatus(); + auto state = controls_state.getState(); + if (alert_status == cereal::ControlsState::AlertStatus::USER_PROMPT) { + status = STATUS_WARNING; + } else if (alert_status == cereal::ControlsState::AlertStatus::CRITICAL) { + status = STATUS_ALERT; + } else if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) { + status = STATUS_OVERRIDE; + } else { + status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; + } + } + + // Handle onroad/offroad transition + if (scene.started != started_prev || sm->frame == 1) { + if (scene.started) { + status = STATUS_DISENGAGED; + scene.started_frame = sm->frame; + wide_camera = Params().getBool("WideCameraOnly"); + } + started_prev = scene.started; + emit offroadTransition(!scene.started); + } +} + +UIState::UIState(QObject *parent) : QObject(parent) { + sm = std::make_unique>({ + "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", + "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", + }); + + Params params; + wide_camera = params.getBool("WideCameraOnly"); + prime_type = std::atoi(params.get("PrimeType").c_str()); + + // update timer + timer = new QTimer(this); + QObject::connect(timer, &QTimer::timeout, this, &UIState::update); + timer->start(1000 / UI_FREQ); +} + +void UIState::update() { + update_sockets(this); + update_state(this); + updateStatus(); + + if (sm->frame % UI_FREQ == 0) { + watchdog_kick(nanos_since_boot()); + } + emit uiUpdate(*this); +} + +Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) { + setAwake(true); + resetInteractiveTimout(); + + QObject::connect(uiState(), &UIState::uiUpdate, this, &Device::update); +} + +void Device::update(const UIState &s) { + updateBrightness(s); + updateWakefulness(s); + + // TODO: remove from UIState and use signals + uiState()->awake = awake; +} + +void Device::setAwake(bool on) { + if (on != awake) { + awake = on; + Hardware::set_display_power(awake); + LOGD("setting display power %d", awake); + emit displayPowerChanged(awake); + } +} + +void Device::resetInteractiveTimout() { + interactive_timeout = (ignition_on ? 10 : 30) * UI_FREQ; +} + +void Device::updateBrightness(const UIState &s) { + float clipped_brightness = BACKLIGHT_OFFROAD; + if (s.scene.started) { + // Scale to 0% to 100% + clipped_brightness = 100.0 * s.scene.light_sensor; + + // CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm + if (clipped_brightness <= 8) { + clipped_brightness = (clipped_brightness / 903.3); + } else { + clipped_brightness = std::pow((clipped_brightness + 16.0) / 116.0, 3.0); + } + + // Scale back to 10% to 100% + clipped_brightness = std::clamp(100.0f * clipped_brightness, 10.0f, 100.0f); + } + + int brightness = brightness_filter.update(clipped_brightness); + if (!awake) { + brightness = 0; + } + + if (brightness != last_brightness) { + if (!brightness_future.isRunning()) { + brightness_future = QtConcurrent::run(Hardware::set_brightness, brightness); + last_brightness = brightness; + } + } +} + +bool Device::motionTriggered(const UIState &s) { + static float accel_prev = 0; + static float gyro_prev = 0; + + bool accel_trigger = abs(s.scene.accel_sensor - accel_prev) > 0.2; + bool gyro_trigger = abs(s.scene.gyro_sensor - gyro_prev) > 0.15; + + gyro_prev = s.scene.gyro_sensor; + accel_prev = (accel_prev * (accel_samples - 1) + s.scene.accel_sensor) / accel_samples; + + return (!awake && accel_trigger && gyro_trigger); +} + +void Device::updateWakefulness(const UIState &s) { + bool ignition_just_turned_off = !s.scene.ignition && ignition_on; + ignition_on = s.scene.ignition; + + if (ignition_just_turned_off || motionTriggered(s)) { + resetInteractiveTimout(); + } else if (interactive_timeout > 0 && --interactive_timeout == 0) { + emit interactiveTimout(); + } + + setAwake(s.scene.ignition || interactive_timeout > 0); +} + +UIState *uiState() { + static UIState ui_state; + return &ui_state; +} diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h new file mode 100644 index 00000000000000..08ae16ab248cde --- /dev/null +++ b/selfdrive/ui/ui.h @@ -0,0 +1,181 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/modeldata.h" +#include "common/params.h" +#include "common/timing.h" + +const int bdr_s = 30; +const int header_h = 420; +const int footer_h = 280; + +const int UI_FREQ = 20; // Hz +typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; + +const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; + +struct Alert { + QString text1; + QString text2; + QString type; + cereal::ControlsState::AlertSize size; + AudibleAlert sound; + + bool equal(const Alert &a2) { + return text1 == a2.text1 && text2 == a2.text2 && type == a2.type && sound == a2.sound; + } + + static Alert get(const SubMaster &sm, uint64_t started_frame) { + const cereal::ControlsState::Reader &cs = sm["controlsState"].getControlsState(); + if (sm.updated("controlsState")) { + return {cs.getAlertText1().cStr(), cs.getAlertText2().cStr(), + cs.getAlertType().cStr(), cs.getAlertSize(), + cs.getAlertSound()}; + } else if ((sm.frame - started_frame) > 5 * UI_FREQ) { + const int CONTROLS_TIMEOUT = 5; + const int controls_missing = (nanos_since_boot() - sm.rcv_time("controlsState")) / 1e9; + + // Handle controls timeout + if (sm.rcv_frame("controlsState") < started_frame) { + // car is started, but controlsState hasn't been seen at all + return {"openpilot Unavailable", "Waiting for controls to start", + "controlsWaiting", cereal::ControlsState::AlertSize::MID, + AudibleAlert::NONE}; + } else if (controls_missing > CONTROLS_TIMEOUT && !Hardware::PC()) { + // car is started, but controls is lagging or died + if (cs.getEnabled() && (controls_missing - CONTROLS_TIMEOUT) < 10) { + return {"TAKE CONTROL IMMEDIATELY", "Controls Unresponsive", + "controlsUnresponsive", cereal::ControlsState::AlertSize::FULL, + AudibleAlert::WARNING_IMMEDIATE}; + } else { + return {"Controls Unresponsive", "Reboot Device", + "controlsUnresponsivePermanent", cereal::ControlsState::AlertSize::MID, + AudibleAlert::NONE}; + } + } + } + return {}; + } +}; + +typedef enum UIStatus { + STATUS_DISENGAGED, + STATUS_OVERRIDE, + STATUS_ENGAGED, + STATUS_WARNING, + STATUS_ALERT, +} UIStatus; + +const QColor bg_colors [] = { + [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), + [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), + [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + [STATUS_WARNING] = QColor(0xDA, 0x6F, 0x25, 0xf1), + [STATUS_ALERT] = QColor(0xC9, 0x22, 0x31, 0xf1), +}; + +typedef struct UIScene { + bool calibration_valid = false; + mat3 view_from_calib = DEFAULT_CALIBRATION; + cereal::PandaState::PandaType pandaType; + + // modelV2 + float lane_line_probs[4]; + float road_edge_stds[2]; + QPolygonF track_vertices; + QPolygonF lane_line_vertices[4]; + QPolygonF road_edge_vertices[2]; + + // lead + QPointF lead_vertices[2]; + + float light_sensor, accel_sensor, gyro_sensor; + bool started, ignition, is_metric, map_on_left, longitudinal_control, end_to_end_long; + uint64_t started_frame; +} UIScene; + +class UIState : public QObject { + Q_OBJECT + +public: + UIState(QObject* parent = 0); + void updateStatus(); + inline bool worldObjectsVisible() const { + return sm->rcv_frame("liveCalibration") > scene.started_frame; + }; + inline bool engaged() const { + return scene.started && (*sm)["controlsState"].getControlsState().getEnabled(); + }; + + int fb_w = 0, fb_h = 0; + + std::unique_ptr sm; + + UIStatus status; + UIScene scene = {}; + + bool awake; + int prime_type = 0; + + QTransform car_space_transform; + bool wide_camera; + +signals: + void uiUpdate(const UIState &s); + void offroadTransition(bool offroad); + +private slots: + void update(); + +private: + QTimer *timer; + bool started_prev = false; +}; + +UIState *uiState(); + +// device management class + +class Device : public QObject { + Q_OBJECT + +public: + Device(QObject *parent = 0); + +private: + // auto brightness + const float accel_samples = 5*UI_FREQ; + + bool awake = false; + int interactive_timeout = 0; + bool ignition_on = false; + int last_brightness = 0; + FirstOrderFilter brightness_filter; + QFuture brightness_future; + + void updateBrightness(const UIState &s); + void updateWakefulness(const UIState &s); + bool motionTriggered(const UIState &s); + void setAwake(bool on); + +signals: + void displayPowerChanged(bool on); + void interactiveTimout(); + +public slots: + void resetInteractiveTimout(); + void update(const UIState &s); +}; + +void ui_update_params(UIState *s); diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py deleted file mode 100755 index 7fe0dfbbc9a382..00000000000000 --- a/selfdrive/ui/ui.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -import os -import pyray as rl - -from openpilot.system.hardware import TICI -from openpilot.common.realtime import config_realtime_process, set_core_affinity -from openpilot.system.ui.lib.application import gui_app -from openpilot.selfdrive.ui.layouts.main import MainLayout -from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout -from openpilot.selfdrive.ui.ui_state import ui_state - - -def main(): - cores = {5, } - config_realtime_process(0, 51) - - gui_app.init_window("UI") - if gui_app.big_ui(): - main_layout = MainLayout() - else: - main_layout = MiciMainLayout() - main_layout.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - for should_render in gui_app.render(): - ui_state.update() - if should_render: - main_layout.render() - - # reaffine after power save offlines our core - if TICI and os.sched_getaffinity(0) != cores: - try: - set_core_affinity(list(cores)) - except OSError: - pass - - -if __name__ == "__main__": - main() diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py deleted file mode 100644 index 30a656509551e0..00000000000000 --- a/selfdrive/ui/ui_state.py +++ /dev/null @@ -1,286 +0,0 @@ -import pyray as rl -import numpy as np -import time -import threading -from collections.abc import Callable -from enum import Enum -from cereal import messaging, car, log -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.ui.lib.prime_state import PrimeState -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.hardware import HARDWARE, PC - -BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50 - - -class UIStatus(Enum): - DISENGAGED = "disengaged" - ENGAGED = "engaged" - OVERRIDE = "override" - - -class UIState: - _instance: 'UIState | None' = None - - def __new__(cls): - if cls._instance is None: - cls._instance = super().__new__(cls) - cls._instance._initialize() - return cls._instance - - def _initialize(self): - self.params = Params() - self.sm = messaging.SubMaster( - [ - "modelV2", - "controlsState", - "onroadEvents", - "liveCalibration", - "radarState", - "deviceState", - "pandaStates", - "carParams", - "driverMonitoringState", - "carState", - "driverStateV2", - "roadCameraState", - "wideRoadCameraState", - "managerState", - "selfdriveState", - "longitudinalPlan", - "gpsLocationExternal", - "carOutput", - "carControl", - "liveParameters", - "rawAudioData", - ] - ) - - self.prime_state = PrimeState() - - # UI Status tracking - self.status: UIStatus = UIStatus.DISENGAGED - self.started_frame: int = 0 - self.started_time: float = 0.0 - self._engaged_prev: bool = False - self._started_prev: bool = False - - # Core state variables - self.is_metric: bool = self.params.get_bool("IsMetric") - self.is_release = self.params.get_bool("IsReleaseBranch") - self.always_on_dm: bool = self.params.get_bool("AlwaysOnDM") - self.started: bool = False - self.ignition: bool = False - self.recording_audio: bool = False - self.panda_type: log.PandaState.PandaType = log.PandaState.PandaType.unknown - self.personality: log.LongitudinalPersonality = log.LongitudinalPersonality.standard - self.has_longitudinal_control: bool = False - self.CP: car.CarParams | None = None - self.light_sensor: float = -1.0 - self._param_update_time: float = 0.0 - - # Callbacks - self._offroad_transition_callbacks: list[Callable[[], None]] = [] - self._engaged_transition_callbacks: list[Callable[[], None]] = [] - - self.update_params() - - def add_offroad_transition_callback(self, callback: Callable[[], None]): - self._offroad_transition_callbacks.append(callback) - - def add_engaged_transition_callback(self, callback: Callable[[], None]): - self._engaged_transition_callbacks.append(callback) - - @property - def engaged(self) -> bool: - return self.started and self.sm["selfdriveState"].enabled - - def is_onroad(self) -> bool: - return self.started - - def is_offroad(self) -> bool: - return not self.started - - def update(self) -> None: - self.prime_state.start() # start thread after manager forks ui - self.sm.update(0) - self._update_state() - self._update_status() - if time.monotonic() - self._param_update_time > 5.0: - self.update_params() - device.update() - - def _update_state(self) -> None: - # Handle panda states updates - if self.sm.updated["pandaStates"]: - panda_states = self.sm["pandaStates"] - - if len(panda_states) > 0: - # Get panda type from first panda - self.panda_type = panda_states[0].pandaType - # Check ignition status across all pandas - if self.panda_type != log.PandaState.PandaType.unknown: - self.ignition = any(state.ignitionLine or state.ignitionCan for state in panda_states) - elif self.sm.frame - self.sm.recv_frame["pandaStates"] > 5 * rl.get_fps(): - self.panda_type = log.PandaState.PandaType.unknown - - # Handle wide road camera state updates - if self.sm.updated["wideRoadCameraState"]: - cam_state = self.sm["wideRoadCameraState"] - 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 - - # Update started state - self.started = self.sm["deviceState"].started and self.ignition - - # Update recording audio state - self.recording_audio = self.params.get_bool("RecordAudio") and self.started - - self.is_metric = self.params.get_bool("IsMetric") - self.always_on_dm = self.params.get_bool("AlwaysOnDM") - - def _update_status(self) -> None: - if self.started and self.sm.updated["selfdriveState"]: - ss = self.sm["selfdriveState"] - state = ss.state - - if state in (log.SelfdriveState.OpenpilotState.preEnabled, log.SelfdriveState.OpenpilotState.overriding): - self.status = UIStatus.OVERRIDE - else: - self.status = UIStatus.ENGAGED if ss.enabled else UIStatus.DISENGAGED - - # Check for engagement state changes - if self.engaged != self._engaged_prev: - for callback in self._engaged_transition_callbacks: - callback() - self._engaged_prev = self.engaged - - # Handle onroad/offroad transition - if self.started != self._started_prev or self.sm.frame == 1: - if self.started: - self.status = UIStatus.DISENGAGED - self.started_frame = self.sm.frame - self.started_time = time.monotonic() - - for callback in self._offroad_transition_callbacks: - callback() - - self._started_prev = self.started - - def update_params(self) -> None: - # For slower operations - # Update longitudinal control state - CP_bytes = self.params.get("CarParamsPersistent") - if CP_bytes is not None: - self.CP = messaging.log_from_bytes(CP_bytes, car.CarParams) - if self.CP.alphaLongitudinalAvailable: - self.has_longitudinal_control = self.params.get_bool("AlphaLongitudinalEnabled") - else: - self.has_longitudinal_control = self.CP.openpilotLongitudinalControl - self._param_update_time = time.monotonic() - - -class Device: - def __init__(self): - self._ignition = False - self._interaction_time: float = -1 - self._override_interactive_timeout: int | None = None - self._interactive_timeout_callbacks: list[Callable] = [] - self._prev_timed_out = False - self._awake: bool = True - - self._offroad_brightness: int = BACKLIGHT_OFFROAD - self._last_brightness: int = 0 - self._brightness_filter = FirstOrderFilter(BACKLIGHT_OFFROAD, 10.00, 1 / gui_app.target_fps) - self._brightness_thread: threading.Thread | None = None - - @property - def awake(self) -> bool: - return self._awake - - def set_override_interactive_timeout(self, timeout: int | None) -> None: - # Override the interactive timeout duration temporarily - self._override_interactive_timeout = timeout - self._reset_interactive_timeout() - - @property - def interactive_timeout(self) -> int: - if self._override_interactive_timeout is not None: - return self._override_interactive_timeout - - ignition_timeout = 10 if gui_app.big_ui() else 5 - return ignition_timeout if ui_state.ignition else 30 - - def _reset_interactive_timeout(self) -> None: - self._interaction_time = time.monotonic() + self.interactive_timeout - - def add_interactive_timeout_callback(self, callback: Callable): - self._interactive_timeout_callbacks.append(callback) - - def update(self): - # do initial reset - if self._interaction_time <= 0: - self._reset_interactive_timeout() - - self._update_brightness() - self._update_wakefulness() - - def set_offroad_brightness(self, brightness: int | None): - if brightness is None: - brightness = BACKLIGHT_OFFROAD - self._offroad_brightness = min(max(brightness, 0), 100) - - def _update_brightness(self): - clipped_brightness = self._offroad_brightness - - if ui_state.started and ui_state.light_sensor >= 0: - clipped_brightness = ui_state.light_sensor - - # CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm - if clipped_brightness <= 8: - clipped_brightness = clipped_brightness / 903.3 - else: - clipped_brightness = ((clipped_brightness + 16.0) / 116.0) ** 3.0 - - clipped_brightness = float(np.interp(clipped_brightness, [0, 1], [30, 100])) - - brightness = round(self._brightness_filter.update(clipped_brightness)) - if not self._awake: - brightness = 0 - - if brightness != self._last_brightness: - if self._brightness_thread is None or not self._brightness_thread.is_alive(): - self._brightness_thread = threading.Thread(target=HARDWARE.set_screen_brightness, args=(brightness,)) - self._brightness_thread.start() - self._last_brightness = brightness - - def _update_wakefulness(self): - # Handle interactive timeout - ignition_just_turned_off = not ui_state.ignition and self._ignition - self._ignition = ui_state.ignition - - if ignition_just_turned_off or any(ev.left_down for ev in gui_app.mouse_events): - self._reset_interactive_timeout() - - interaction_timeout = time.monotonic() > self._interaction_time - if interaction_timeout and not self._prev_timed_out: - for callback in self._interactive_timeout_callbacks: - callback() - self._prev_timed_out = interaction_timeout - - self._set_awake(ui_state.ignition or not interaction_timeout or PC) - - def _set_awake(self, on: bool): - if on != self._awake: - self._awake = on - cloudlog.debug(f"setting display power {int(on)}") - HARDWARE.set_display_power(on) - gui_app.set_should_render(on) - - -# Global instance -ui_state = UIState() -device = Device() diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index bded80b2e5bdf0..afd42c3b3add06 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -1,42 +1,38 @@ #!/usr/bin/env python3 -from itertools import chain +import argparse +import json import os -from openpilot.common.basedir import BASEDIR -from openpilot.system.ui.lib.multilang import SYSTEM_UI_DIR, UI_DIR, TRANSLATIONS_DIR, multilang - -LANGUAGES_FILE = os.path.join(str(TRANSLATIONS_DIR), "languages.json") -POT_FILE = os.path.join(str(TRANSLATIONS_DIR), "app.pot") - - -def update_translations(): - files = [] - for root, _, filenames in chain(os.walk(SYSTEM_UI_DIR), - os.walk(os.path.join(UI_DIR, "widgets")), - os.walk(os.path.join(UI_DIR, "layouts")), - os.walk(os.path.join(UI_DIR, "onroad"))): - for filename in filenames: - if filename.endswith(".py"): - files.append(os.path.relpath(os.path.join(root, filename), BASEDIR)) - - # Create main translation file - cmd = ("xgettext -L Python --keyword=tr --keyword=trn:1,2 --keyword=tr_noop --from-code=UTF-8 " + - "--flag=tr:1:python-brace-format --flag=trn:1:python-brace-format --flag=trn:2:python-brace-format " + - f"-D {BASEDIR} -o {POT_FILE} {' '.join(files)}") - - ret = os.system(cmd) - assert ret == 0 - - # Generate/update translation files for each language - for name in multilang.languages.values(): - if os.path.exists(os.path.join(TRANSLATIONS_DIR, f"app_{name}.po")): - cmd = f"msgmerge --update --no-fuzzy-matching --backup=none --sort-output {TRANSLATIONS_DIR}/app_{name}.po {POT_FILE}" - ret = os.system(cmd) - assert ret == 0 - else: - cmd = f"msginit -l {name} --no-translator --input {POT_FILE} --output-file {TRANSLATIONS_DIR}/app_{name}.po" - ret = os.system(cmd) - assert ret == 0 + +from common.basedir import BASEDIR + +UI_DIR = os.path.join(BASEDIR, "selfdrive", "ui") +TRANSLATIONS_DIR = os.path.join(UI_DIR, "translations") +LANGUAGES_FILE = os.path.join(TRANSLATIONS_DIR, "languages.json") + + +def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLATIONS_DIR): + if plural_only is None: + plural_only = [] + + with open(LANGUAGES_FILE, "r") as f: + translation_files = json.load(f) + + for file in translation_files.values(): + tr_file = os.path.join(translations_dir, f"{file}.ts") + args = f"lupdate -locations relative -recursive {UI_DIR} -ts {tr_file}" + if vanish: + args += " -no-obsolete" + if file in plural_only: + args += " -pluralonly" + ret = os.system(args) + assert ret == 0 if __name__ == "__main__": - update_translations() + parser = argparse.ArgumentParser(description="Update translation files for UI", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--vanish", action="store_true", help="Remove translations with source text no longer found") + parser.add_argument("--plural-only", type=str, nargs="*", default=["main_en"], help="Translation codes to only create plural translations for (ie. the base language)") + args = parser.parse_args() + + update_translations(args.vanish, args.plural_only) diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc new file mode 100644 index 00000000000000..d6b5cc67a7b9a3 --- /dev/null +++ b/selfdrive/ui/watch3.cc @@ -0,0 +1,34 @@ +#include +#include + +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + +int main(int argc, char *argv[]) { + initApp(argc, argv); + + QApplication a(argc, argv); + QWidget w; + setMainWindow(&w); + + QVBoxLayout *layout = new QVBoxLayout(&w); + layout->setMargin(0); + layout->setSpacing(0); + + { + QHBoxLayout *hlayout = new QHBoxLayout(); + layout->addLayout(hlayout); + hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false)); + } + + { + QHBoxLayout *hlayout = new QHBoxLayout(); + layout->addLayout(hlayout); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_DRIVER, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); + } + + return a.exec(); +} diff --git a/selfdrive/ui/watch3.py b/selfdrive/ui/watch3.py deleted file mode 100755 index bb64cdc4d543fa..00000000000000 --- a/selfdrive/ui/watch3.py +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python3 -import pyray as rl - -from msgq.visionipc import VisionStreamType -from openpilot.system.ui.lib.application import gui_app -from openpilot.selfdrive.ui.onroad.cameraview import CameraView - - -if __name__ == "__main__": - gui_app.init_window("watch3") - road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) - driver = CameraView("camerad", VisionStreamType.VISION_STREAM_DRIVER) - wide = CameraView("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD) - for _ in gui_app.render(): - road.render(rl.Rectangle(gui_app.width // 4, 0, gui_app.width // 2, gui_app.height // 2)) - driver.render(rl.Rectangle(0, gui_app.height // 2, gui_app.width // 2, gui_app.height // 2)) - wide.render(rl.Rectangle(gui_app.width // 2, gui_app.height // 2, gui_app.width // 2, gui_app.height // 2)) diff --git a/selfdrive/ui/widgets/exp_mode_button.py b/selfdrive/ui/widgets/exp_mode_button.py deleted file mode 100644 index faa3bf877f0b0c..00000000000000 --- a/selfdrive/ui/widgets/exp_mode_button.py +++ /dev/null @@ -1,64 +0,0 @@ -import pyray as rl -from openpilot.common.params import Params -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import Widget - - -class ExperimentalModeButton(Widget): - def __init__(self): - super().__init__() - - self.img_width = 80 - self.horizontal_padding = 25 - self.button_height = 125 - - self.params = Params() - self.experimental_mode = self.params.get_bool("ExperimentalMode") - - self.chill_pixmap = gui_app.texture("icons/couch.png", self.img_width, self.img_width) - self.experimental_pixmap = gui_app.texture("icons/experimental_grey.png", self.img_width, self.img_width) - - def show_event(self): - self.experimental_mode = self.params.get_bool("ExperimentalMode") - - def _get_gradient_colors(self): - alpha = 0xCC if self.is_pressed else 0xFF - - if self.experimental_mode: - return rl.Color(255, 155, 63, alpha), rl.Color(219, 56, 34, alpha) - else: - return rl.Color(20, 255, 171, alpha), rl.Color(35, 149, 255, alpha) - - def _draw_gradient_background(self, rect): - start_color, end_color = self._get_gradient_colors() - rl.draw_rectangle_gradient_h(int(rect.x), int(rect.y), int(rect.width), int(rect.height), - start_color, end_color) - - def _render(self, rect): - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - self._draw_gradient_background(rect) - rl.draw_rectangle_rounded_lines_ex(self._rect, 0.19, 10, 5, rl.BLACK) - rl.end_scissor_mode() - - # Draw vertical separator line - line_x = rect.x + rect.width - self.img_width - (2 * self.horizontal_padding) - separator_color = rl.Color(0, 0, 0, 77) # 0x4d = 77 - rl.draw_line_ex(rl.Vector2(line_x, rect.y), rl.Vector2(line_x, rect.y + rect.height), 3, separator_color) - - # Draw text label (left aligned) - text = tr("EXPERIMENTAL MODE ON") if self.experimental_mode else tr("CHILL MODE ON") - text_x = rect.x + self.horizontal_padding - text_y = rect.y + rect.height / 2 - 45 * FONT_SCALE // 2 # Center vertically - - rl.draw_text_ex(gui_app.font(FontWeight.NORMAL), text, rl.Vector2(int(text_x), int(text_y)), 45, 0, rl.BLACK) - - # Draw icon (right aligned) - icon_x = rect.x + rect.width - self.horizontal_padding - self.img_width - icon_y = rect.y + (rect.height - self.img_width) / 2 - icon_rect = rl.Rectangle(icon_x, icon_y, self.img_width, self.img_width) - - # Draw current mode icon - current_icon = self.experimental_pixmap if self.experimental_mode else self.chill_pixmap - source_rect = rl.Rectangle(0, 0, current_icon.width, current_icon.height) - rl.draw_texture_pro(current_icon, source_rect, icon_rect, rl.Vector2(0, 0), 0, rl.WHITE) diff --git a/selfdrive/ui/widgets/offroad_alerts.py b/selfdrive/ui/widgets/offroad_alerts.py deleted file mode 100644 index 802243ff3eb162..00000000000000 --- a/selfdrive/ui/widgets/offroad_alerts.py +++ /dev/null @@ -1,344 +0,0 @@ -import pyray as rl -from enum import IntEnum -from abc import ABC, abstractmethod -from collections.abc import Callable -from dataclasses import dataclass -from openpilot.common.params import Params -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.html_render import HtmlRenderer -from openpilot.selfdrive.selfdrived.alertmanager import OFFROAD_ALERTS - - -class AlertColors: - HIGH_SEVERITY = rl.Color(226, 44, 44, 255) - LOW_SEVERITY = rl.Color(41, 41, 41, 255) - BACKGROUND = rl.Color(57, 57, 57, 255) - BUTTON = rl.WHITE - BUTTON_PRESSED = rl.Color(200, 200, 200, 255) - BUTTON_TEXT = rl.BLACK - SNOOZE_BG = rl.Color(79, 79, 79, 255) - SNOOZE_BG_PRESSED = rl.Color(100, 100, 100, 255) - TEXT = rl.WHITE - - -class AlertConstants: - MIN_BUTTON_WIDTH = 400 - BUTTON_HEIGHT = 125 - MARGIN = 50 - SPACING = 30 - FONT_SIZE = 48 - BORDER_RADIUS = 30 * 2 # matches Qt's 30px - ALERT_HEIGHT = 120 - ALERT_SPACING = 10 - ALERT_INSET = 60 - - -@dataclass -class AlertData: - key: str - text: str - severity: int - visible: bool = False - - -class ButtonStyle(IntEnum): - LIGHT = 0 - DARK = 1 - - -class ActionButton(Widget): - def __init__(self, text: str | Callable[[], str], style: ButtonStyle = ButtonStyle.LIGHT, - min_width: int = AlertConstants.MIN_BUTTON_WIDTH): - super().__init__() - self._text = text - self._style = style - self._min_width = min_width - self._font = gui_app.font(FontWeight.MEDIUM) - - @property - def text(self) -> str: - return self._text() if callable(self._text) else self._text - - def _render(self, _): - text_size = measure_text_cached(gui_app.font(FontWeight.MEDIUM), self.text, AlertConstants.FONT_SIZE) - self._rect.width = max(text_size.x + 60 * 2, self._min_width) - self._rect.height = AlertConstants.BUTTON_HEIGHT - - roundness = AlertConstants.BORDER_RADIUS / self._rect.height - bg_color = AlertColors.BUTTON if self._style == ButtonStyle.LIGHT else AlertColors.SNOOZE_BG - if self.is_pressed: - bg_color = AlertColors.BUTTON_PRESSED if self._style == ButtonStyle.LIGHT else AlertColors.SNOOZE_BG_PRESSED - - rl.draw_rectangle_rounded(self._rect, roundness, 10, bg_color) - - # center text - color = rl.WHITE if self._style == ButtonStyle.DARK else rl.BLACK - text_x = int(self._rect.x + (self._rect.width - text_size.x) // 2) - text_y = int(self._rect.y + (self._rect.height - text_size.y) // 2) - rl.draw_text_ex(self._font, self.text, rl.Vector2(text_x, text_y), AlertConstants.FONT_SIZE, 0, color) - - -class AbstractAlert(Widget, ABC): - def __init__(self, has_reboot_btn: bool = False): - super().__init__() - self.params = Params() - self.has_reboot_btn = has_reboot_btn - self.dismiss_callback: Callable | None = None - - def snooze_callback(): - self.params.put_bool("SnoozeUpdate", True) - if self.dismiss_callback: - self.dismiss_callback() - - def excessive_actuation_callback(): - self.params.remove("Offroad_ExcessiveActuation") - if self.dismiss_callback: - self.dismiss_callback() - - self.dismiss_btn = ActionButton(lambda: tr("Close")) - - self.snooze_btn = ActionButton(lambda: tr("Snooze Update"), style=ButtonStyle.DARK) - self.snooze_btn.set_click_callback(snooze_callback) - - self.excessive_actuation_btn = ActionButton(lambda: tr("Acknowledge Excessive Actuation"), style=ButtonStyle.DARK, min_width=800) - self.excessive_actuation_btn.set_click_callback(excessive_actuation_callback) - - self.reboot_btn = ActionButton(lambda: tr("Reboot and Update"), min_width=600) - self.reboot_btn.set_click_callback(lambda: HARDWARE.reboot()) - - # TODO: just use a Scroller? - self.content_rect = rl.Rectangle(0, 0, 0, 0) - self.scroll_panel_rect = rl.Rectangle(0, 0, 0, 0) - self.scroll_panel = GuiScrollPanel() - - def show_event(self): - self.scroll_panel.set_offset(0) - - def set_dismiss_callback(self, callback: Callable): - self.dismiss_callback = callback - self.dismiss_btn.set_click_callback(self.dismiss_callback) - - @abstractmethod - def refresh(self) -> bool: - pass - - @abstractmethod - def get_content_height(self) -> float: - pass - - def _render(self, rect: rl.Rectangle): - rl.draw_rectangle_rounded(rect, AlertConstants.BORDER_RADIUS / rect.height, 10, AlertColors.BACKGROUND) - - footer_height = AlertConstants.BUTTON_HEIGHT + AlertConstants.SPACING - content_height = rect.height - 2 * AlertConstants.MARGIN - footer_height - - self.content_rect = rl.Rectangle( - rect.x + AlertConstants.MARGIN, - rect.y + AlertConstants.MARGIN, - rect.width - 2 * AlertConstants.MARGIN, - content_height, - ) - self.scroll_panel_rect = rl.Rectangle( - self.content_rect.x, self.content_rect.y, self.content_rect.width, self.content_rect.height - ) - - self._render_scrollable_content() - self._render_footer(rect) - - def _render_scrollable_content(self): - content_total_height = self.get_content_height() - content_bounds = rl.Rectangle(0, 0, self.scroll_panel_rect.width, content_total_height) - scroll_offset = self.scroll_panel.update(self.scroll_panel_rect, content_bounds) - - rl.begin_scissor_mode( - int(self.scroll_panel_rect.x), - int(self.scroll_panel_rect.y), - int(self.scroll_panel_rect.width), - int(self.scroll_panel_rect.height), - ) - - content_rect_with_scroll = rl.Rectangle( - self.scroll_panel_rect.x, - self.scroll_panel_rect.y + scroll_offset, - self.scroll_panel_rect.width, - content_total_height, - ) - - self._render_content(content_rect_with_scroll) - rl.end_scissor_mode() - - @abstractmethod - def _render_content(self, content_rect: rl.Rectangle): - pass - - def _render_footer(self, rect: rl.Rectangle): - footer_y = rect.y + rect.height - AlertConstants.MARGIN - AlertConstants.BUTTON_HEIGHT - - dismiss_x = rect.x + AlertConstants.MARGIN - self.dismiss_btn.set_position(dismiss_x, footer_y) - self.dismiss_btn.render() - - if self.has_reboot_btn: - reboot_x = rect.x + rect.width - AlertConstants.MARGIN - self.reboot_btn.rect.width - self.reboot_btn.set_position(reboot_x, footer_y) - self.reboot_btn.render() - - elif self.excessive_actuation_btn.is_visible: - actuation_x = rect.x + rect.width - AlertConstants.MARGIN - self.excessive_actuation_btn.rect.width - self.excessive_actuation_btn.set_position(actuation_x, footer_y) - self.excessive_actuation_btn.render() - - elif self.snooze_btn.is_visible: - snooze_x = rect.x + rect.width - AlertConstants.MARGIN - self.snooze_btn.rect.width - self.snooze_btn.set_position(snooze_x, footer_y) - self.snooze_btn.render() - - -class OffroadAlert(AbstractAlert): - def __init__(self): - super().__init__(has_reboot_btn=False) - self.sorted_alerts: list[AlertData] = [] - - def refresh(self): - if not self.sorted_alerts: - self._build_alerts() - - active_count = 0 - connectivity_needed = False - excessive_actuation = False - - for alert_data in self.sorted_alerts: - text = "" - alert_json = self.params.get(alert_data.key) - - if alert_json: - text = alert_json.get("text", "").replace("%1", alert_json.get("extra", "")) - - alert_data.text = text - alert_data.visible = bool(text) - - if alert_data.visible: - active_count += 1 - - if alert_data.key == "Offroad_ConnectivityNeeded" and alert_data.visible: - connectivity_needed = True - - if alert_data.key == "Offroad_ExcessiveActuation" and alert_data.visible: - excessive_actuation = True - - self.excessive_actuation_btn.set_visible(excessive_actuation) - self.snooze_btn.set_visible(connectivity_needed and not excessive_actuation) - return active_count - - def get_content_height(self) -> float: - if not self.sorted_alerts: - return 0 - - total_height = 20 - font = gui_app.font(FontWeight.NORMAL) - - for alert_data in self.sorted_alerts: - if not alert_data.visible: - continue - - text_width = int(self.content_rect.width - (AlertConstants.ALERT_INSET * 2)) - wrapped_lines = wrap_text(font, alert_data.text, AlertConstants.FONT_SIZE, text_width) - line_count = len(wrapped_lines) - text_height = line_count * (AlertConstants.FONT_SIZE * FONT_SCALE) - alert_item_height = max(text_height + (AlertConstants.ALERT_INSET * 2), AlertConstants.ALERT_HEIGHT) - total_height += round(alert_item_height + AlertConstants.ALERT_SPACING) - - if total_height > 20: - total_height = total_height - AlertConstants.ALERT_SPACING + 20 - - return total_height - - def _build_alerts(self): - self.sorted_alerts = [] - for key, config in sorted(OFFROAD_ALERTS.items(), key=lambda x: x[1].get("severity", 0), reverse=True): - severity = config.get("severity", 0) - alert_data = AlertData(key=key, text="", severity=severity) - self.sorted_alerts.append(alert_data) - - def _render_content(self, content_rect: rl.Rectangle): - y_offset = AlertConstants.ALERT_SPACING - font = gui_app.font(FontWeight.NORMAL) - - for alert_data in self.sorted_alerts: - if not alert_data.visible: - continue - - bg_color = AlertColors.HIGH_SEVERITY if alert_data.severity > 0 else AlertColors.LOW_SEVERITY - text_width = int(content_rect.width - (AlertConstants.ALERT_INSET * 2)) - wrapped_lines = wrap_text(font, alert_data.text, AlertConstants.FONT_SIZE, text_width) - line_count = len(wrapped_lines) - text_height = line_count * (AlertConstants.FONT_SIZE * FONT_SCALE) - alert_item_height = max(text_height + (AlertConstants.ALERT_INSET * 2), AlertConstants.ALERT_HEIGHT) - - alert_rect = rl.Rectangle( - content_rect.x + 10, - content_rect.y + y_offset, - content_rect.width - 30, - alert_item_height, - ) - - roundness = AlertConstants.BORDER_RADIUS / min(alert_rect.height, alert_rect.width) - rl.draw_rectangle_rounded(alert_rect, roundness, 10, bg_color) - - text_x = alert_rect.x + AlertConstants.ALERT_INSET - text_y = alert_rect.y + AlertConstants.ALERT_INSET - - for i, line in enumerate(wrapped_lines): - rl.draw_text_ex( - font, - line, - rl.Vector2(text_x, text_y + i * AlertConstants.FONT_SIZE * FONT_SCALE), - AlertConstants.FONT_SIZE, - 0, - AlertColors.TEXT, - ) - - y_offset += round(alert_item_height + AlertConstants.ALERT_SPACING) - - -class UpdateAlert(AbstractAlert): - def __init__(self): - super().__init__(has_reboot_btn=True) - self.release_notes = "" - self._wrapped_release_notes = "" - self._cached_content_height: float = 0.0 - self._html_renderer = HtmlRenderer(text="") - - def refresh(self) -> bool: - update_available: bool = self.params.get_bool("UpdateAvailable") - no_release_notes = "

    " + tr("No release notes available.") + "

    " - - if update_available: - self.release_notes = (self.params.get("UpdaterNewReleaseNotes") or b"").decode("utf8").strip() - self._html_renderer.parse_html_content(self.release_notes or no_release_notes) - self._cached_content_height = 0 - else: - self._html_renderer.parse_html_content(no_release_notes) - - return update_available - - def get_content_height(self) -> float: - if not self.release_notes: - return 100 - - if self._cached_content_height == 0: - self._wrapped_release_notes = self.release_notes - size = measure_text_cached(gui_app.font(FontWeight.NORMAL), self._wrapped_release_notes, AlertConstants.FONT_SIZE) - self._cached_content_height = max(size.y + 60, 100) - - return self._cached_content_height - - def _render_content(self, content_rect: rl.Rectangle): - notes_rect = rl.Rectangle(content_rect.x + 30, content_rect.y + 30, content_rect.width - 60, content_rect.height - 60) - self._html_renderer.render(notes_rect) diff --git a/selfdrive/ui/widgets/pairing_dialog.py b/selfdrive/ui/widgets/pairing_dialog.py deleted file mode 100644 index f960cf723ee3d5..00000000000000 --- a/selfdrive/ui/widgets/pairing_dialog.py +++ /dev/null @@ -1,171 +0,0 @@ -import pyray as rl -import qrcode -import numpy as np -import time - -from openpilot.common.api import Api -from openpilot.common.swaglog import cloudlog -from openpilot.common.params import Params -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.lib.application import FontWeight, gui_app -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets.button import IconButton -from openpilot.selfdrive.ui.ui_state import ui_state - - -class PairingDialog(Widget): - """Dialog for device pairing with QR code.""" - - QR_REFRESH_INTERVAL = 300 # 5 minutes in seconds - - def __init__(self): - super().__init__() - self.params = Params() - self.qr_texture: rl.Texture | None = None - self.last_qr_generation = float('-inf') - self._close_btn = IconButton(gui_app.texture("icons/close.png", 80, 80)) - self._close_btn.set_click_callback(lambda: gui_app.set_modal_overlay(None)) - - def _get_pairing_url(self) -> str: - try: - dongle_id = self.params.get("DongleId") or "" - token = Api(dongle_id).get_token({'pair': True}) - except Exception: - cloudlog.exception("Failed to get pairing token") - token = "" - return f"https://connect.comma.ai/?pair={token}" - - def _generate_qr_code(self) -> None: - try: - qr = qrcode.QRCode(version=1, error_correction=qrcode.constants.ERROR_CORRECT_L, box_size=10, border=4) - qr.add_data(self._get_pairing_url()) - qr.make(fit=True) - - pil_img = qr.make_image(fill_color="black", back_color="white").convert('RGBA') - img_array = np.array(pil_img, dtype=np.uint8) - - if self.qr_texture and self.qr_texture.id != 0: - rl.unload_texture(self.qr_texture) - - rl_image = rl.Image() - rl_image.data = rl.ffi.cast("void *", img_array.ctypes.data) - rl_image.width = pil_img.width - rl_image.height = pil_img.height - rl_image.mipmaps = 1 - rl_image.format = rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 - - self.qr_texture = rl.load_texture_from_image(rl_image) - except Exception: - cloudlog.exception("QR code generation failed") - self.qr_texture = None - - def _check_qr_refresh(self) -> None: - current_time = time.monotonic() - if current_time - self.last_qr_generation >= self.QR_REFRESH_INTERVAL: - self._generate_qr_code() - self.last_qr_generation = current_time - - def _update_state(self): - if ui_state.prime_state.is_paired(): - gui_app.set_modal_overlay(None) - - def _render(self, rect: rl.Rectangle) -> int: - rl.clear_background(rl.Color(224, 224, 224, 255)) - - self._check_qr_refresh() - - margin = 70 - content_rect = rl.Rectangle(rect.x + margin, rect.y + margin, rect.width - 2 * margin, rect.height - 2 * margin) - y = content_rect.y - - # Close button - close_size = 80 - pad = 20 - close_rect = rl.Rectangle(content_rect.x - pad, y - pad, close_size + pad * 2, close_size + pad * 2) - self._close_btn.render(close_rect) - - y += close_size + 40 - - # Title - title = tr("Pair your device to your comma account") - title_font = gui_app.font(FontWeight.NORMAL) - left_width = int(content_rect.width * 0.5 - 15) - - title_wrapped = wrap_text(title_font, title, 75, left_width) - rl.draw_text_ex(title_font, "\n".join(title_wrapped), rl.Vector2(content_rect.x, y), 75, 0.0, rl.BLACK) - y += len(title_wrapped) * 75 + 60 - - # Two columns: instructions and QR code - remaining_height = content_rect.height - (y - content_rect.y) - right_width = content_rect.width // 2 - 20 - - # Instructions - self._render_instructions(rl.Rectangle(content_rect.x, y, left_width, remaining_height)) - - # QR code - qr_size = min(right_width, content_rect.height) - 40 - qr_x = content_rect.x + left_width + 40 + (right_width - qr_size) // 2 - qr_y = content_rect.y - self._render_qr_code(rl.Rectangle(qr_x, qr_y, qr_size, qr_size)) - - return -1 - - def _render_instructions(self, rect: rl.Rectangle) -> None: - instructions = [ - tr("Go to https://connect.comma.ai on your phone"), - tr("Click \"add new device\" and scan the QR code on the right"), - tr("Bookmark connect.comma.ai to your home screen to use it like an app"), - ] - - font = gui_app.font(FontWeight.BOLD) - y = rect.y - - for i, text in enumerate(instructions): - circle_radius = 25 - circle_x = rect.x + circle_radius + 15 - text_x = rect.x + circle_radius * 2 + 40 - text_width = rect.width - (circle_radius * 2 + 40) - - wrapped = wrap_text(font, text, 47, int(text_width)) - text_height = len(wrapped) * 47 - circle_y = y + text_height // 2 - - # Circle and number - rl.draw_circle(int(circle_x), int(circle_y), circle_radius, rl.Color(70, 70, 70, 255)) - number = str(i + 1) - number_size = measure_text_cached(font, number, 30) - rl.draw_text_ex(font, number, (int(circle_x - number_size.x // 2), int(circle_y - number_size.y // 2)), 30, 0, rl.WHITE) - - # Text - rl.draw_text_ex(font, "\n".join(wrapped), rl.Vector2(text_x, y), 47, 0.0, rl.BLACK) - y += text_height + 50 - - def _render_qr_code(self, rect: rl.Rectangle) -> None: - if not self.qr_texture: - rl.draw_rectangle_rounded(rect, 0.1, 20, rl.Color(240, 240, 240, 255)) - error_font = gui_app.font(FontWeight.BOLD) - rl.draw_text_ex( - error_font, tr("QR Code Error"), rl.Vector2(rect.x + 20, rect.y + rect.height // 2 - 15), 30, 0.0, rl.RED - ) - return - - source = rl.Rectangle(0, 0, self.qr_texture.width, self.qr_texture.height) - rl.draw_texture_pro(self.qr_texture, source, rect, rl.Vector2(0, 0), 0, rl.WHITE) - - def __del__(self): - if self.qr_texture and self.qr_texture.id != 0: - rl.unload_texture(self.qr_texture) - - -if __name__ == "__main__": - gui_app.init_window("pairing device") - pairing = PairingDialog() - try: - for _ in gui_app.render(): - result = pairing.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - if result != -1: - break - finally: - del pairing diff --git a/selfdrive/ui/widgets/prime.py b/selfdrive/ui/widgets/prime.py deleted file mode 100644 index e98e4c1e1ccde5..00000000000000 --- a/selfdrive/ui/widgets/prime.py +++ /dev/null @@ -1,63 +0,0 @@ -import pyray as rl - -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import gui_label - - -class PrimeWidget(Widget): - """Widget for displaying comma prime subscription status""" - - PRIME_BG_COLOR = rl.Color(51, 51, 51, 255) - - def _render(self, rect): - if ui_state.prime_state.is_prime(): - self._render_for_prime_user(rect) - else: - self._render_for_non_prime_users(rect) - - def _render_for_non_prime_users(self, rect: rl.Rectangle): - """Renders the advertisement for non-Prime users.""" - - rl.draw_rectangle_rounded(rect, 0.025, 10, self.PRIME_BG_COLOR) - - # Layout - x, y = rect.x + 80, rect.y + 90 - w = rect.width - 160 - - # Title - gui_label(rl.Rectangle(x, y, w, 90), tr("Upgrade Now"), 75, font_weight=FontWeight.BOLD) - - # Description with wrapping - desc_y = y + 140 - font = gui_app.font(FontWeight.NORMAL) - wrapped_text = "\n".join(wrap_text(font, tr("Become a comma prime member at connect.comma.ai"), 56, int(w))) - text_size = measure_text_cached(font, wrapped_text, 56) - rl.draw_text_ex(font, wrapped_text, rl.Vector2(x, desc_y), 56, 0, rl.WHITE) - - # Features section - features_y = desc_y + text_size.y + 50 - gui_label(rl.Rectangle(x, features_y, w, 50), tr("PRIME FEATURES:"), 41, font_weight=FontWeight.BOLD) - - # Feature list - features = [tr("Remote access"), tr("24/7 LTE connectivity"), tr("1 year of drive storage"), tr("Remote snapshots")] - for i, feature in enumerate(features): - item_y = features_y + 80 + i * 65 - gui_label(rl.Rectangle(x, item_y, 100, 60), "✓", 50, color=rl.Color(70, 91, 234, 255)) - gui_label(rl.Rectangle(x + 60, item_y, w - 60, 60), feature, 50) - - def _render_for_prime_user(self, rect: rl.Rectangle): - """Renders the prime user widget with subscription status.""" - - rl.draw_rectangle_rounded(rl.Rectangle(rect.x, rect.y, rect.width, 230), 0.1, 10, self.PRIME_BG_COLOR) - - x = rect.x + 56 - y = rect.y + 40 - - font = gui_app.font(FontWeight.BOLD) - rl.draw_text_ex(font, tr("✓ SUBSCRIBED"), rl.Vector2(x, y), 41, 0, rl.Color(134, 255, 78, 255)) - rl.draw_text_ex(font, tr("comma prime"), rl.Vector2(x, y + 61), 75, 0, rl.WHITE) diff --git a/selfdrive/ui/widgets/setup.py b/selfdrive/ui/widgets/setup.py deleted file mode 100644 index 3c9406688f2541..00000000000000 --- a/selfdrive/ui/widgets/setup.py +++ /dev/null @@ -1,101 +0,0 @@ -import pyray as rl -from openpilot.common.time_helpers import system_time_valid -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.widgets.pairing_dialog import PairingDialog -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.wrap_text import wrap_text -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.confirm_dialog import alert_dialog -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.label import Label - - -class SetupWidget(Widget): - def __init__(self): - super().__init__() - self._open_settings_callback = None - self._pairing_dialog: PairingDialog | None = None - self._pair_device_btn = Button(lambda: tr("Pair device"), self._show_pairing, button_style=ButtonStyle.PRIMARY) - self._open_settings_btn = Button(lambda: tr("Open"), lambda: self._open_settings_callback() if self._open_settings_callback else None, - button_style=ButtonStyle.PRIMARY) - self._firehose_label = Label(lambda: tr("🔥 Firehose Mode 🔥"), font_weight=FontWeight.MEDIUM, font_size=64) - - def set_open_settings_callback(self, callback): - self._open_settings_callback = callback - - def _render(self, rect: rl.Rectangle): - if not ui_state.prime_state.is_paired(): - self._render_registration(rect) - else: - self._render_firehose_prompt(rect) - - def _render_registration(self, rect: rl.Rectangle): - """Render registration prompt.""" - - rl.draw_rectangle_rounded(rl.Rectangle(rect.x, rect.y, rect.width, rect.height), 0.03, 20, rl.Color(51, 51, 51, 255)) - - x = rect.x + 64 - y = rect.y + 48 - w = rect.width - 128 - - # Title - font = gui_app.font(FontWeight.BOLD) - rl.draw_text_ex(font, tr("Finish Setup"), rl.Vector2(x, y), 75, 0, rl.WHITE) - y += 113 # 75 + 38 spacing - - # Description - desc = tr("Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.") - light_font = gui_app.font(FontWeight.NORMAL) - wrapped = wrap_text(light_font, desc, 50, int(w)) - for line in wrapped: - rl.draw_text_ex(light_font, line, rl.Vector2(x, y), 50, 0, rl.WHITE) - y += 50 * FONT_SCALE - - button_rect = rl.Rectangle(x, y + 30, w, 200) - self._pair_device_btn.render(button_rect) - - def _render_firehose_prompt(self, rect: rl.Rectangle): - """Render firehose prompt widget.""" - - rl.draw_rectangle_rounded(rl.Rectangle(rect.x, rect.y, rect.width, 500), 0.04, 20, rl.Color(51, 51, 51, 255)) - - # Content margins (56, 40, 56, 40) - x = rect.x + 56 - y = rect.y + 40 - w = rect.width - 112 - spacing = 42 - - # Title with fire emojis - self._firehose_label.render(rl.Rectangle(rect.x, y, rect.width, 64)) - y += 64 + spacing - - # Description - desc_font = gui_app.font(FontWeight.NORMAL) - desc_text = tr("Maximize your training data uploads to improve openpilot's driving models.") - wrapped_desc = wrap_text(desc_font, desc_text, 40, int(w)) - - for line in wrapped_desc: - rl.draw_text_ex(desc_font, line, rl.Vector2(x, y), 40, 0, rl.WHITE) - y += 40 * FONT_SCALE - - y += spacing - - # Open button - button_height = 48 + 64 # font size + padding - button_rect = rl.Rectangle(x, y, w, button_height) - self._open_settings_btn.render(button_rect) - - def _show_pairing(self): - if not system_time_valid(): - dlg = alert_dialog(tr("Please connect to Wi-Fi to complete initial pairing")) - gui_app.set_modal_overlay(dlg) - return - - if not self._pairing_dialog: - self._pairing_dialog = PairingDialog() - gui_app.set_modal_overlay(self._pairing_dialog, lambda result: setattr(self, '_pairing_dialog', None)) - - def __del__(self): - if self._pairing_dialog: - del self._pairing_dialog diff --git a/selfdrive/ui/widgets/ssh_key.py b/selfdrive/ui/widgets/ssh_key.py deleted file mode 100644 index 88389cb0532ccd..00000000000000 --- a/selfdrive/ui/widgets/ssh_key.py +++ /dev/null @@ -1,131 +0,0 @@ -import pyray as rl -import requests -import threading -import copy -from collections.abc import Callable -from enum import Enum - -from openpilot.common.params import Params -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr, tr_noop -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import DialogResult -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.confirm_dialog import alert_dialog -from openpilot.system.ui.widgets.keyboard import Keyboard -from openpilot.system.ui.widgets.list_view import ( - ItemAction, - ListItem, - BUTTON_HEIGHT, - BUTTON_BORDER_RADIUS, - BUTTON_FONT_SIZE, - BUTTON_WIDTH, -) - -VALUE_FONT_SIZE = 48 - - -class SshKeyActionState(Enum): - LOADING = tr_noop("LOADING") - ADD = tr_noop("ADD") - REMOVE = tr_noop("REMOVE") - - -class SshKeyAction(ItemAction): - HTTP_TIMEOUT = 15 # seconds - MAX_WIDTH = 500 - - def __init__(self): - super().__init__(self.MAX_WIDTH, True) - - self._keyboard = Keyboard(min_text_size=1) - self._params = Params() - self._error_message: str = "" - self._text_font = gui_app.font(FontWeight.NORMAL) - self._button = Button("", click_callback=self._handle_button_click, button_style=ButtonStyle.LIST_ACTION, - border_radius=BUTTON_BORDER_RADIUS, font_size=BUTTON_FONT_SIZE) - - self._refresh_state() - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - super().set_touch_valid_callback(touch_callback) - self._button.set_touch_valid_callback(touch_callback) - - def _refresh_state(self): - self._username = self._params.get("GithubUsername") - self._state = SshKeyActionState.REMOVE if self._params.get("GithubSshKeys") else SshKeyActionState.ADD - - def _render(self, rect: rl.Rectangle) -> bool: - # Show error dialog if there's an error - if self._error_message: - message = copy.copy(self._error_message) - gui_app.set_modal_overlay(alert_dialog(message)) - self._username = "" - self._error_message = "" - - # Draw username if exists - if self._username: - text_size = measure_text_cached(self._text_font, self._username, VALUE_FONT_SIZE) - rl.draw_text_ex( - self._text_font, - self._username, - (rect.x + rect.width - BUTTON_WIDTH - text_size.x - 30, rect.y + (rect.height - text_size.y) / 2), - VALUE_FONT_SIZE, - 1.0, - rl.Color(170, 170, 170, 255), - ) - - # Draw button - button_rect = rl.Rectangle(rect.x + rect.width - BUTTON_WIDTH, rect.y + (rect.height - BUTTON_HEIGHT) / 2, BUTTON_WIDTH, BUTTON_HEIGHT) - self._button.set_rect(button_rect) - self._button.set_text(tr(self._state.value)) - self._button.set_enabled(self._state != SshKeyActionState.LOADING) - self._button.render(button_rect) - return False - - def _handle_button_click(self): - if self._state == SshKeyActionState.ADD: - self._keyboard.reset() - self._keyboard.set_title(tr("Enter your GitHub username")) - gui_app.set_modal_overlay(self._keyboard, callback=self._on_username_submit) - elif self._state == SshKeyActionState.REMOVE: - self._params.remove("GithubUsername") - self._params.remove("GithubSshKeys") - self._refresh_state() - - def _on_username_submit(self, result: DialogResult): - if result != DialogResult.CONFIRM: - return - - username = self._keyboard.text.strip() - if not username: - return - - self._state = SshKeyActionState.LOADING - threading.Thread(target=lambda: self._fetch_ssh_key(username), daemon=True).start() - - def _fetch_ssh_key(self, username: str): - try: - url = f"https://github.com/{username}.keys" - response = requests.get(url, timeout=self.HTTP_TIMEOUT) - response.raise_for_status() - keys = response.text.strip() - if not keys: - raise requests.exceptions.HTTPError(tr("No SSH keys found")) - - # Success - save keys - self._params.put("GithubUsername", username) - self._params.put("GithubSshKeys", keys) - self._state = SshKeyActionState.REMOVE - self._username = username - - except requests.exceptions.Timeout: - self._error_message = tr("Request timed out") - self._state = SshKeyActionState.ADD - except Exception: - self._error_message = tr("No SSH keys found for user '{}'").format(username) - self._state = SshKeyActionState.ADD - - -def ssh_key_item(title: str | Callable[[], str], description: str | Callable[[], str]) -> ListItem: - return ListItem(title=title, description=description, action_item=SshKeyAction()) diff --git a/selfdrive/updated.py b/selfdrive/updated.py new file mode 100755 index 00000000000000..7278ef5a8008ac --- /dev/null +++ b/selfdrive/updated.py @@ -0,0 +1,431 @@ +#!/usr/bin/env python3 + +# Safe Update: A simple service that waits for network access and tries to +# update every 10 minutes. It's intended to make the OP update process more +# robust against Git repository corruption. This service DOES NOT try to fix +# an already-corrupt BASEDIR Git repo, only prevent it from happening. +# +# During normal operation, both onroad and offroad, the update process makes +# no changes to the BASEDIR install of OP. All update attempts are performed +# in a disposable staging area provided by OverlayFS. It assumes the deleter +# process provides enough disk space to carry out the process. +# +# If an update succeeds, a flag is set, and the update is swapped in at the +# next reboot. If an update is interrupted or otherwise fails, the OverlayFS +# upper layer and metadata can be discarded before trying again. +# +# The swap on boot is triggered by launch_chffrplus.sh +# gated on the existence of $FINALIZED/.overlay_consistent and also the +# existence and mtime of $BASEDIR/.overlay_init. +# +# Other than build byproducts, BASEDIR should not be modified while this +# service is running. Developers modifying code directly in BASEDIR should +# disable this service. + +import os +import datetime +import subprocess +import psutil +import shutil +import signal +import fcntl +import time +import threading +from pathlib import Path +from typing import List, Tuple, Optional +from markdown_it import MarkdownIt + +from common.basedir import BASEDIR +from common.params import Params +from system.hardware import AGNOS, HARDWARE +from system.swaglog import cloudlog +from selfdrive.controls.lib.alertmanager import set_offroad_alert +from system.version import is_tested_branch + +LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") +STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") + +OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper") +OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata") +OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged") +FINALIZED = os.path.join(STAGING_ROOT, "finalized") + +DAYS_NO_CONNECTIVITY_MAX = 14 # do not allow to engage after this many days +DAYS_NO_CONNECTIVITY_PROMPT = 10 # send an offroad prompt after this many days + +class WaitTimeHelper: + def __init__(self, proc): + self.proc = proc + self.ready_event = threading.Event() + self.shutdown = False + signal.signal(signal.SIGTERM, self.graceful_shutdown) + signal.signal(signal.SIGINT, self.graceful_shutdown) + signal.signal(signal.SIGHUP, self.update_now) + + def graceful_shutdown(self, signum: int, frame) -> None: + # umount -f doesn't appear effective in avoiding "device busy" on NEOS, + # so don't actually die until the next convenient opportunity in main(). + cloudlog.info("caught SIGINT/SIGTERM, dismounting overlay at next opportunity") + + # forward the signal to all our child processes + child_procs = self.proc.children(recursive=True) + for p in child_procs: + p.send_signal(signum) + + self.shutdown = True + self.ready_event.set() + + def update_now(self, signum: int, frame) -> None: + cloudlog.info("caught SIGHUP, running update check immediately") + self.ready_event.set() + + def sleep(self, t: float) -> None: + self.ready_event.wait(timeout=t) + + +def run(cmd: List[str], cwd: Optional[str] = None, low_priority: bool = False): + if low_priority: + cmd = ["nice", "-n", "19"] + cmd + return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') + + +def set_consistent_flag(consistent: bool) -> None: + os.sync() + consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent")) + if consistent: + consistent_file.touch() + elif not consistent: + consistent_file.unlink(missing_ok=True) + os.sync() + + +def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -> None: + params = Params() + + params.put("UpdateFailedCount", str(failed_count)) + + last_update = datetime.datetime.utcnow() + if failed_count == 0: + t = last_update.isoformat() + params.put("LastUpdateTime", t.encode('utf8')) + else: + try: + t = params.get("LastUpdateTime", encoding='utf8') + last_update = datetime.datetime.fromisoformat(t) + except (TypeError, ValueError): + pass + + if exception is None: + params.remove("LastUpdateException") + else: + params.put("LastUpdateException", exception) + + # Write out release notes for new versions + if new_version: + try: + with open(os.path.join(FINALIZED, "RELEASES.md"), "rb") as f: + r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes + try: + params.put("ReleaseNotes", MarkdownIt().render(r.decode("utf-8"))) + except Exception: + params.put("ReleaseNotes", r + b"\n") + except Exception: + params.put("ReleaseNotes", "") + params.put_bool("UpdateAvailable", True) + + # Handle user prompt + for alert in ("Offroad_UpdateFailed", "Offroad_ConnectivityNeeded", "Offroad_ConnectivityNeededPrompt"): + set_offroad_alert(alert, False) + + now = datetime.datetime.utcnow() + dt = now - last_update + if failed_count > 15 and exception is not None: + if is_tested_branch(): + extra_text = "Ensure the software is correctly installed" + else: + extra_text = exception + set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) + elif dt.days > DAYS_NO_CONNECTIVITY_MAX and failed_count > 1: + set_offroad_alert("Offroad_ConnectivityNeeded", True) + elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1) + set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.") + + +def setup_git_options(cwd: str) -> None: + # We sync FS object atimes (which NEOS doesn't use) and mtimes, but ctimes + # are outside user control. Make sure Git is set up to ignore system ctimes, + # because they change when we make hard links during finalize. Otherwise, + # there is a lot of unnecessary churn. This appears to be a common need on + # OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/ + + # We are using copytree to copy the directory, which also changes + # inode numbers. Ignore those changes too. + + # Set protocol to the new version (default after git 2.26) to reduce data + # usage on git fetch --dry-run from about 400KB to 18KB. + git_cfg = [ + ("core.trustctime", "false"), + ("core.checkStat", "minimal"), + ("protocol.version", "2"), + ("gc.auto", "0"), + ("gc.autoDetach", "false"), + ] + for option, value in git_cfg: + run(["git", "config", option, value], cwd) + + +def dismount_overlay() -> None: + if os.path.ismount(OVERLAY_MERGED): + cloudlog.info("unmounting existing overlay") + run(["sudo", "umount", "-l", OVERLAY_MERGED]) + + +def init_overlay() -> None: + + overlay_init_file = Path(os.path.join(BASEDIR, ".overlay_init")) + + # Re-create the overlay if BASEDIR/.git has changed since we created the overlay + if overlay_init_file.is_file(): + git_dir_path = os.path.join(BASEDIR, ".git") + new_files = run(["find", git_dir_path, "-newer", str(overlay_init_file)]) + if not len(new_files.splitlines()): + # A valid overlay already exists + return + else: + cloudlog.info(".git directory changed, recreating overlay") + + cloudlog.info("preparing new safe staging area") + + params = Params() + params.put_bool("UpdateAvailable", False) + set_consistent_flag(False) + dismount_overlay() + run(["sudo", "rm", "-rf", STAGING_ROOT]) + if os.path.isdir(STAGING_ROOT): + shutil.rmtree(STAGING_ROOT) + + for dirname in [STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED]: + os.mkdir(dirname, 0o755) + + if os.lstat(BASEDIR).st_dev != os.lstat(OVERLAY_MERGED).st_dev: + raise RuntimeError("base and overlay merge directories are on different filesystems; not valid for overlay FS!") + + # Leave a timestamped canary in BASEDIR to check at startup. The device clock + # should be correct by the time we get here. If the init file disappears, or + # critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can + # assume that BASEDIR has used for local development or otherwise modified, + # and skips the update activation attempt. + consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent")) + if consistent_file.is_file(): + consistent_file.unlink() + overlay_init_file.touch() + + os.sync() + overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}" + + mount_cmd = ["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED] + run(["sudo"] + mount_cmd) + run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")]) + + git_diff = run(["git", "diff"], OVERLAY_MERGED, low_priority=True) + params.put("GitDiff", git_diff) + cloudlog.info(f"git diff output:\n{git_diff}") + + +def finalize_update(wait_helper: WaitTimeHelper) -> None: + """Take the current OverlayFS merged view and finalize a copy outside of + OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree""" + + # Remove the update ready flag and any old updates + cloudlog.info("creating finalized version of the overlay") + set_consistent_flag(False) + + # Copy the merged overlay view and set the update ready flag + if os.path.exists(FINALIZED): + shutil.rmtree(FINALIZED) + shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True) + + run(["git", "reset", "--hard"], FINALIZED) + run(["git", "submodule", "foreach", "--recursive", "git", "reset"], FINALIZED) + + cloudlog.info("Starting git gc") + t = time.monotonic() + try: + run(["git", "gc"], FINALIZED) + cloudlog.event("Done git gc", duration=time.monotonic() - t) + except subprocess.CalledProcessError: + cloudlog.exception(f"Failed git gc, took {time.monotonic() - t:.3f} s") + + if wait_helper.shutdown: + cloudlog.info("got interrupted finalizing overlay") + else: + set_consistent_flag(True) + cloudlog.info("done finalizing overlay") + + +def handle_agnos_update(wait_helper: WaitTimeHelper) -> None: + from system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number + + cur_version = HARDWARE.get_os_version() + updated_version = run(["bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \ + echo -n $AGNOS_VERSION"], OVERLAY_MERGED).strip() + + cloudlog.info(f"AGNOS version check: {cur_version} vs {updated_version}") + if cur_version == updated_version: + return + + # prevent an openpilot getting swapped in with a mismatched or partially downloaded agnos + set_consistent_flag(False) + + cloudlog.info(f"Beginning background installation for AGNOS {updated_version}") + set_offroad_alert("Offroad_NeosUpdate", True) + + manifest_path = os.path.join(OVERLAY_MERGED, "system/hardware/tici/agnos.json") + target_slot_number = get_target_slot_number() + flash_agnos_update(manifest_path, target_slot_number, cloudlog) + set_offroad_alert("Offroad_NeosUpdate", False) + + +def check_git_fetch_result(fetch_txt: str) -> bool: + err_msg = "Failed to add the host to the list of known hosts (/data/data/com.termux/files/home/.ssh/known_hosts).\n" + return len(fetch_txt) > 0 and (fetch_txt != err_msg) + + +def check_for_update() -> Tuple[bool, bool]: + setup_git_options(OVERLAY_MERGED) + try: + git_fetch_output = run(["git", "fetch", "--dry-run"], OVERLAY_MERGED, low_priority=True) + return True, check_git_fetch_result(git_fetch_output) + except subprocess.CalledProcessError: + return False, False + + +def fetch_update(wait_helper: WaitTimeHelper) -> bool: + cloudlog.info("attempting git fetch inside staging overlay") + + setup_git_options(OVERLAY_MERGED) + + git_fetch_output = run(["git", "fetch"], OVERLAY_MERGED, low_priority=True) + cloudlog.info("git fetch success: %s", git_fetch_output) + + cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip() + upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip() + new_version: bool = cur_hash != upstream_hash + git_fetch_result = check_git_fetch_result(git_fetch_output) + + new_branch = Params().get("SwitchToBranch", encoding='utf8') + if new_branch is not None: + new_version = True + + cloudlog.info(f"comparing {cur_hash} to {upstream_hash}") + if new_version or git_fetch_result: + cloudlog.info("Running update") + + if new_version: + cloudlog.info("git reset in progress") + cmds = [ + ["git", "reset", "--hard", "@{u}"], + ["git", "clean", "-xdf"], + ["git", "submodule", "init"], + ["git", "submodule", "update"], + ] + if new_branch is not None: + cloudlog.info(f"switching to branch {repr(new_branch)}") + cmds.insert(0, ["git", "checkout", "-f", new_branch]) + r = [run(cmd, OVERLAY_MERGED, low_priority=True) for cmd in cmds] + cloudlog.info("git reset success: %s", '\n'.join(r)) + + if AGNOS: + handle_agnos_update(wait_helper) + + # Create the finalized, ready-to-swap update + finalize_update(wait_helper) + cloudlog.info("openpilot update successful!") + else: + cloudlog.info("nothing new from git at this time") + + return new_version + + +def main() -> None: + params = Params() + + if params.get_bool("DisableUpdates"): + cloudlog.warning("updates are disabled by the DisableUpdates param") + exit(0) + + ov_lock_fd = open(LOCK_FILE, 'w') + try: + fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB) + except OSError as e: + raise RuntimeError("couldn't get overlay lock; is another instance running?") from e + + # Set low io priority + proc = psutil.Process() + if psutil.LINUX: + proc.ionice(psutil.IOPRIO_CLASS_BE, value=7) + + # Check if we just performed an update + if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir(): + cloudlog.event("update installed") + + if not params.get("InstallDate"): + t = datetime.datetime.utcnow().isoformat() + params.put("InstallDate", t.encode('utf8')) + + overlay_init = Path(os.path.join(BASEDIR, ".overlay_init")) + overlay_init.unlink(missing_ok=True) + + update_failed_count = 0 # TODO: Load from param? + wait_helper = WaitTimeHelper(proc) + + # Run the update loop + while not wait_helper.shutdown: + wait_helper.ready_event.clear() + + # Attempt an update + exception = None + new_version = False + update_failed_count += 1 + try: + init_overlay() + + # TODO: still needed? skip this and just fetch? + # Lightweight internt check + internet_ok, update_available = check_for_update() + if internet_ok and not update_available: + update_failed_count = 0 + + # Fetch update + if internet_ok: + new_version = fetch_update(wait_helper) + update_failed_count = 0 + except subprocess.CalledProcessError as e: + cloudlog.event( + "update process failed", + cmd=e.cmd, + output=e.output, + returncode=e.returncode + ) + exception = f"command failed: {e.cmd}\n{e.output}" + overlay_init.unlink(missing_ok=True) + except Exception as e: + cloudlog.exception("uncaught updated exception, shouldn't happen") + exception = str(e) + overlay_init.unlink(missing_ok=True) + + if not wait_helper.shutdown: + try: + set_params(new_version, update_failed_count, exception) + except Exception: + cloudlog.exception("uncaught updated exception while setting params, shouldn't happen") + + # infrequent attempts if we successfully updated recently + wait_helper.sleep(5*60 if update_failed_count > 0 else 90*60) + + dismount_overlay() + + +if __name__ == "__main__": + main() diff --git a/site_scons/site_tools/cython.py b/site_scons/site_tools/cython.py index f11db1d71bebca..c2914755336b40 100644 --- a/site_scons/site_tools/cython.py +++ b/site_scons/site_tools/cython.py @@ -2,17 +2,14 @@ import SCons from SCons.Action import Action from SCons.Scanner import Scanner -import numpy as np pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M) pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M) cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M) -np_version = SCons.Script.Value(np.__version__) def pyx_scan(node, env, path, arg=None): contents = node.get_text_contents() - env.Depends(str(node).split('.')[0] + env['CYTHONCFILESUFFIX'], np_version) # from cimport ... matches = pyx_from_import_re.findall(contents) diff --git a/system/athena/athenad.py b/system/athena/athenad.py deleted file mode 100755 index b52ef21ba63702..00000000000000 --- a/system/athena/athenad.py +++ /dev/null @@ -1,858 +0,0 @@ -#!/usr/bin/env python3 -from __future__ import annotations - -import base64 -import hashlib -import io -import json -import os -import queue -import random -import select -import socket -import sys -import tempfile -import threading -import time -from dataclasses import asdict, dataclass, replace -from datetime import datetime -from functools import partial, total_ordering -from queue import Queue -from typing import cast -from collections.abc import Callable - -import requests -from requests.adapters import HTTPAdapter, DEFAULT_POOLBLOCK -from jsonrpc import JSONRPCResponseManager, dispatcher -from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException, - create_connection) - -import cereal.messaging as messaging -from cereal import log -from cereal.services import SERVICE_LIST -from openpilot.common.api import Api, get_key_pair -from openpilot.common.utils import CallbackReader, get_upload_stream -from openpilot.common.params import Params -from openpilot.common.realtime import set_core_affinity -from openpilot.system.hardware import HARDWARE, PC -from openpilot.system.loggerd.xattr_cache import getxattr, setxattr -from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_build_metadata -from openpilot.system.hardware.hw import Paths - - -ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') -HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) -LOCAL_PORT_WHITELIST = {22, } # SSH - -LOG_ATTR_NAME = 'user.upload' -LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder) -RECONNECT_TIMEOUT_S = 70 - -RETRY_DELAY = 10 # seconds -MAX_RETRY_COUNT = 30 # Try for at most 5 minutes if upload fails immediately -MAX_AGE = 31 * 24 * 3600 # seconds -WS_FRAME_SIZE = 4096 -DEVICE_STATE_UPDATE_INTERVAL = 1.0 # in seconds -DEFAULT_UPLOAD_PRIORITY = 99 # higher number = lower priority - -# https://bytesolutions.com/dscp-tos-cos-precedence-conversion-chart, -# https://en.wikipedia.org/wiki/Differentiated_services -UPLOAD_TOS = 0x20 # CS1, low priority background traffic -SSH_TOS = 0x90 # AF42, DSCP of 36/HDD_LINUX_AC_VI with the minimum delay flag - -NetworkType = log.DeviceState.NetworkType - -UploadFileDict = dict[str, str | int | float | bool] -UploadItemDict = dict[str, str | bool | int | float | dict[str, str]] - -UploadFilesToUrlResponse = dict[str, int | list[UploadItemDict] | list[str]] - - -class UploadTOSAdapter(HTTPAdapter): - def init_poolmanager(self, connections, maxsize, block=DEFAULT_POOLBLOCK, **pool_kwargs): - pool_kwargs["socket_options"] = [(socket.IPPROTO_IP, socket.IP_TOS, UPLOAD_TOS)] - super().init_poolmanager(connections, maxsize, block, **pool_kwargs) - - -UPLOAD_SESS = requests.Session() -UPLOAD_SESS.mount("http://", UploadTOSAdapter()) -UPLOAD_SESS.mount("https://", UploadTOSAdapter()) - - -@dataclass -class UploadFile: - fn: str - url: str - headers: dict[str, str] - allow_cellular: bool - priority: int = DEFAULT_UPLOAD_PRIORITY - - @classmethod - def from_dict(cls, d: dict) -> UploadFile: - return cls(d.get("fn", ""), d.get("url", ""), d.get("headers", {}), d.get("allow_cellular", False), d.get("priority", DEFAULT_UPLOAD_PRIORITY)) - - -@dataclass -@total_ordering -class UploadItem: - path: str - url: str - headers: dict[str, str] - created_at: int - id: str | None - retry_count: int = 0 - current: bool = False - progress: float = 0 - allow_cellular: bool = False - priority: int = DEFAULT_UPLOAD_PRIORITY - - @classmethod - def from_dict(cls, d: dict) -> UploadItem: - return cls(d["path"], d["url"], d["headers"], d["created_at"], d["id"], d["retry_count"], d["current"], - d["progress"], d["allow_cellular"], d["priority"]) - - def __lt__(self, other): - if not isinstance(other, UploadItem): - return NotImplemented - return self.priority < other.priority - - def __eq__(self, other): - if not isinstance(other, UploadItem): - return NotImplemented - return self.priority == other.priority - - -dispatcher["echo"] = lambda s: s -recv_queue: Queue[str] = queue.Queue() -send_queue: Queue[str] = queue.Queue() -upload_queue: Queue[UploadItem] = queue.PriorityQueue() -low_priority_send_queue: Queue[str] = queue.Queue() -log_recv_queue: Queue[str] = queue.Queue() -cancelled_uploads: set[str] = set() - -cur_upload_items: dict[int, UploadItem | None] = {} - - -def strip_zst_extension(fn: str) -> str: - if fn.endswith('.zst'): - return fn[:-4] - return fn - - -class AbortTransferException(Exception): - pass - - -class UploadQueueCache: - - @staticmethod - def initialize(upload_queue: Queue[UploadItem]) -> None: - try: - upload_queue_json = Params().get("AthenadUploadQueue") - if upload_queue_json is not None: - for item in upload_queue_json: - upload_queue.put(UploadItem.from_dict(item)) - except Exception: - cloudlog.exception("athena.UploadQueueCache.initialize.exception") - - @staticmethod - def cache(upload_queue: Queue[UploadItem]) -> None: - try: - queue: list[UploadItem | None] = list(upload_queue.queue) - items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] - Params().put("AthenadUploadQueue", items) - except Exception: - cloudlog.exception("athena.UploadQueueCache.cache.exception") - - -def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None: - end_event = threading.Event() - - threads = [ - threading.Thread(target=ws_manage, args=(ws, end_event), name='ws_manage'), - threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'), - threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'), - threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'), - threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler2'), - threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler3'), - threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler4'), - threading.Thread(target=log_handler, args=(end_event,), name='log_handler'), - threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'), - ] + [ - threading.Thread(target=jsonrpc_handler, args=(end_event,), name=f'worker_{x}') - for x in range(HANDLER_THREADS) - ] - - for thread in threads: - thread.start() - try: - while not end_event.wait(0.1): - if exit_event is not None and exit_event.is_set(): - end_event.set() - except (KeyboardInterrupt, SystemExit): - end_event.set() - raise - finally: - for thread in threads: - cloudlog.debug(f"athena.joining {thread.name}") - thread.join() - - -def jsonrpc_handler(end_event: threading.Event) -> None: - dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event) - while not end_event.is_set(): - try: - data = recv_queue.get(timeout=1) - if "method" in data: - cloudlog.event("athena.jsonrpc_handler.call_method", data=data) - response = JSONRPCResponseManager.handle(data, dispatcher) - send_queue.put_nowait(response.json) - elif "id" in data and ("result" in data or "error" in data): - log_recv_queue.put_nowait(data) - else: - raise Exception("not a valid request or response") - except queue.Empty: - pass - except Exception as e: - cloudlog.exception("athena jsonrpc handler failed") - send_queue.put_nowait(json.dumps({"error": str(e)})) - - -def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None: - item = cur_upload_items[tid] - if item is not None and item.retry_count < MAX_RETRY_COUNT: - new_retry_count = item.retry_count + 1 if increase_count else item.retry_count - - item = replace( - item, - retry_count=new_retry_count, - progress=0, - current=False - ) - upload_queue.put_nowait(item) - UploadQueueCache.cache(upload_queue) - - cur_upload_items[tid] = None - - for _ in range(RETRY_DELAY): - time.sleep(1) - if end_event.is_set(): - break - - -def cb(sm, item, tid, end_event: threading.Event, sz: int, cur: int) -> None: - # Abort transfer if connection changed to metered after starting upload - # or if athenad is shutting down to re-connect the websocket - if not item.allow_cellular: - if (time.monotonic() - sm.recv_time['deviceState']) > DEVICE_STATE_UPDATE_INTERVAL: - sm.update(0) - if sm['deviceState'].networkMetered: - raise AbortTransferException - - if end_event.is_set(): - raise AbortTransferException - - cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1) - - -def upload_handler(end_event: threading.Event) -> None: - sm = messaging.SubMaster(['deviceState']) - tid = threading.get_ident() - - while not end_event.is_set(): - cur_upload_items[tid] = None - - try: - cur_upload_items[tid] = item = replace(upload_queue.get(timeout=1), current=True) - - if item.id in cancelled_uploads: - cancelled_uploads.remove(item.id) - continue - - # Remove item if too old - age = datetime.now() - datetime.fromtimestamp(item.created_at / 1000) - if age.total_seconds() > MAX_AGE: - cloudlog.event("athena.upload_handler.expired", item=item, error=True) - continue - - # Check if uploading over metered connection is allowed - sm.update(0) - metered = sm['deviceState'].networkMetered - network_type = sm['deviceState'].networkType.raw - if metered and (not item.allow_cellular): - retry_upload(tid, end_event, False) - continue - - try: - fn = item.path - try: - sz = os.path.getsize(fn) - except OSError: - sz = -1 - - cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count) - - with _do_upload(item, partial(cb, sm, item, tid, end_event)) as response: - if response.status_code not in (200, 201, 401, 403, 412): - cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered) - retry_upload(tid, end_event) - else: - cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered) - - UploadQueueCache.cache(upload_queue) - except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError): - cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered) - retry_upload(tid, end_event) - except AbortTransferException: - cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered) - retry_upload(tid, end_event, False) - - except queue.Empty: - pass - except Exception: - cloudlog.exception("athena.upload_handler.exception") - - -def _do_upload(upload_item: UploadItem, callback: Callable | None = None) -> requests.Response: - path = upload_item.path - compress = False - - # If file does not exist, but does exist without the .zst extension we will compress on the fly - if not os.path.exists(path) and os.path.exists(strip_zst_extension(path)): - path = strip_zst_extension(path) - compress = True - - stream = None - try: - stream, content_length = get_upload_stream(path, compress) - response = UPLOAD_SESS.put(upload_item.url, - data=CallbackReader(stream, callback, content_length) if callback else stream, - headers={**upload_item.headers, 'Content-Length': str(content_length)}, - timeout=30) - return response - finally: - if stream: - stream.close() - - -# security: user should be able to request any message from their car -@dispatcher.add_method -def getMessage(service: str, timeout: int = 1000) -> dict: - if service is None or service not in SERVICE_LIST: - raise Exception("invalid service") - - socket = messaging.sub_sock(service, timeout=timeout) - try: - ret = messaging.recv_one(socket) - - if ret is None: - raise TimeoutError - - # this is because capnp._DynamicStructReader doesn't have typing information - return cast(dict, ret.to_dict()) - finally: - del socket - - -@dispatcher.add_method -def getVersion() -> dict[str, str]: - build_metadata = get_build_metadata() - return { - "version": build_metadata.openpilot.version, - "remote": build_metadata.openpilot.git_normalized_origin, - "branch": build_metadata.channel, - "commit": build_metadata.openpilot.git_commit, - } - - -def scan_dir(path: str, prefix: str) -> list[str]: - files = [] - # only walk directories that match the prefix - # (glob and friends traverse entire dir tree) - with os.scandir(path) as i: - for e in i: - rel_path = os.path.relpath(e.path, Paths.log_root()) - if e.is_dir(follow_symlinks=False): - # add trailing slash - rel_path = os.path.join(rel_path, '') - # if prefix is a partial dir name, current dir will start with prefix - # if prefix is a partial file name, prefix with start with dir name - if rel_path.startswith(prefix) or prefix.startswith(rel_path): - files.extend(scan_dir(e.path, prefix)) - else: - if rel_path.startswith(prefix): - files.append(rel_path) - return files - -@dispatcher.add_method -def listDataDirectory(prefix='') -> list[str]: - return scan_dir(Paths.log_root(), prefix) - - -@dispatcher.add_method -def uploadFileToUrl(fn: str, url: str, headers: dict[str, str]) -> UploadFilesToUrlResponse: - # this is because mypy doesn't understand that the decorator doesn't change the return type - response: UploadFilesToUrlResponse = uploadFilesToUrls([{ - "fn": fn, - "url": url, - "headers": headers, - }]) - return response - - -@dispatcher.add_method -def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlResponse: - files = map(UploadFile.from_dict, files_data) - - items: list[UploadItemDict] = [] - failed: list[str] = [] - for file in files: - if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0: - failed.append(file.fn) - continue - - path = os.path.join(Paths.log_root(), file.fn) - if not os.path.exists(path) and not os.path.exists(strip_zst_extension(path)): - failed.append(file.fn) - continue - - # Skip item if already in queue - url = file.url.split('?')[0] - if any(url == item['url'].split('?')[0] for item in listUploadQueue()): - continue - - item = UploadItem( - path=path, - url=file.url, - headers=file.headers, - created_at=int(time.time() * 1000), # noqa: TID251 - id=None, - allow_cellular=file.allow_cellular, - priority=file.priority, - ) - upload_id = hashlib.sha1(str(item).encode()).hexdigest() - item = replace(item, id=upload_id) - upload_queue.put_nowait(item) - items.append(asdict(item)) - - UploadQueueCache.cache(upload_queue) - - resp: UploadFilesToUrlResponse = {"enqueued": len(items), "items": items} - if failed: - cloudlog.event("athena.uploadFilesToUrls.failed", failed=failed, error=True) - resp["failed"] = failed - - return resp - - -@dispatcher.add_method -def listUploadQueue() -> list[UploadItemDict]: - items = list(upload_queue.queue) + list(cur_upload_items.values()) - return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)] - - -@dispatcher.add_method -def cancelUpload(upload_id: str | list[str]) -> dict[str, int | str]: - if not isinstance(upload_id, list): - upload_id = [upload_id] - - uploading_ids = {item.id for item in list(upload_queue.queue)} - cancelled_ids = uploading_ids.intersection(upload_id) - if len(cancelled_ids) == 0: - return {"success": 0, "error": "not found"} - - cancelled_uploads.update(cancelled_ids) - return {"success": 1} - -@dispatcher.add_method -def setRouteViewed(route: str) -> dict[str, int | str]: - # maintain a list of the last 10 routes viewed in connect - params = Params() - - r = params.get("AthenadRecentlyViewedRoutes") - routes = [] if r is None else r.split(",") - routes.append(route) - - # remove duplicates - routes = list(dict.fromkeys(routes)) - - params.put("AthenadRecentlyViewedRoutes", ",".join(routes[-10:])) - return {"success": 1} - - -def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> dict[str, int]: - try: - # migration, can be removed once 0.9.8 is out for a while - if local_port == 8022: - local_port = 22 - - if local_port not in LOCAL_PORT_WHITELIST: - raise Exception("Requested local port not whitelisted") - - cloudlog.debug("athena.startLocalProxy.starting") - - dongle_id = Params().get("DongleId") - identity_token = Api(dongle_id).get_token() - ws = create_connection(remote_ws_uri, - cookie="jwt=" + identity_token, - enable_multithread=True) - - # Set TOS to keep connection responsive while under load. - ws.sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, SSH_TOS) - - ssock, csock = socket.socketpair() - local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - local_sock.connect(('127.0.0.1', local_port)) - local_sock.setblocking(False) - - proxy_end_event = threading.Event() - threads = [ - threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)), - threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event)) - ] - for thread in threads: - thread.start() - - cloudlog.debug("athena.startLocalProxy.started") - return {"success": 1} - except Exception as e: - cloudlog.exception("athenad.startLocalProxy.exception") - raise e - - -@dispatcher.add_method -def getPublicKey() -> str | None: - _, _, public_key = get_key_pair() - return public_key - - -@dispatcher.add_method -def getSshAuthorizedKeys() -> str: - return cast(str, Params().get("GithubSshKeys") or "") - - -@dispatcher.add_method -def getGithubUsername() -> str: - return cast(str, Params().get("GithubUsername") or "") - -@dispatcher.add_method -def getSimInfo(): - return HARDWARE.get_sim_info() - - -@dispatcher.add_method -def getNetworkType(): - return HARDWARE.get_network_type() - - -@dispatcher.add_method -def getNetworkMetered() -> bool: - network_type = HARDWARE.get_network_type() - return HARDWARE.get_network_metered(network_type) - - -@dispatcher.add_method -def getNetworks(): - return HARDWARE.get_networks() - - -@dispatcher.add_method -def takeSnapshot() -> str | dict[str, str] | None: - from openpilot.system.camerad.snapshot import jpeg_write, snapshot - ret = snapshot() - if ret is not None: - def b64jpeg(x): - if x is not None: - f = io.BytesIO() - jpeg_write(f, x) - return base64.b64encode(f.getvalue()).decode("utf-8") - else: - return None - return {'jpegBack': b64jpeg(ret[0]), - 'jpegFront': b64jpeg(ret[1])} - else: - raise Exception("not available while camerad is started") - - -def get_logs_to_send_sorted() -> list[str]: - # TODO: scan once then use inotify to detect file creation/deletion - curr_time = int(time.time()) # noqa: TID251 - logs = [] - for log_entry in os.listdir(Paths.swaglog_root()): - log_path = os.path.join(Paths.swaglog_root(), log_entry) - time_sent = 0 - try: - value = getxattr(log_path, LOG_ATTR_NAME) - if value is not None: - time_sent = int.from_bytes(value, sys.byteorder) - except (ValueError, TypeError): - pass - # assume send failed and we lost the response if sent more than one hour ago - if not time_sent or curr_time - time_sent > 3600: - logs.append(log_entry) - # excluding most recent (active) log file - return sorted(logs)[:-1] - - -def log_handler(end_event: threading.Event) -> None: - if PC: - return - - log_files = [] - last_scan = 0. - while not end_event.is_set(): - try: - curr_scan = time.monotonic() - if curr_scan - last_scan > 10: - log_files = get_logs_to_send_sorted() - last_scan = curr_scan - - # send one log - curr_log = None - if len(log_files) > 0: - log_entry = log_files.pop() # newest log file - cloudlog.debug(f"athena.log_handler.forward_request {log_entry}") - try: - curr_time = int(time.time()) # noqa: TID251 - log_path = os.path.join(Paths.swaglog_root(), log_entry) - setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder)) - with open(log_path) as f: - jsonrpc = { - "method": "forwardLogs", - "params": { - "logs": f.read() - }, - "jsonrpc": "2.0", - "id": log_entry - } - low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) - curr_log = log_entry - except OSError: - pass # file could be deleted by log rotation - - # wait for response up to ~100 seconds - # always read queue at least once to process any old responses that arrive - for _ in range(100): - if end_event.is_set(): - break - try: - log_resp = json.loads(log_recv_queue.get(timeout=1)) - log_entry = log_resp.get("id") - log_success = "result" in log_resp and log_resp["result"].get("success") - cloudlog.debug(f"athena.log_handler.forward_response {log_entry} {log_success}") - if log_entry and log_success: - log_path = os.path.join(Paths.swaglog_root(), log_entry) - try: - setxattr(log_path, LOG_ATTR_NAME, LOG_ATTR_VALUE_MAX_UNIX_TIME) - except OSError: - pass # file could be deleted by log rotation - if curr_log == log_entry: - break - except queue.Empty: - if curr_log is None: - break - - except Exception: - cloudlog.exception("athena.log_handler.exception") - - -def stat_handler(end_event: threading.Event) -> None: - STATS_DIR = Paths.stats_root() - last_scan = 0.0 - - while not end_event.is_set(): - curr_scan = time.monotonic() - try: - if curr_scan - last_scan > 10: - stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR))) - if len(stat_filenames) > 0: - stat_path = os.path.join(STATS_DIR, stat_filenames[0]) - with open(stat_path) as f: - jsonrpc = { - "method": "storeStats", - "params": { - "stats": f.read() - }, - "jsonrpc": "2.0", - "id": stat_filenames[0] - } - low_priority_send_queue.put_nowait(json.dumps(jsonrpc)) - os.remove(stat_path) - last_scan = curr_scan - except Exception: - cloudlog.exception("athena.stat_handler.exception") - time.sleep(0.1) - - -def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket, end_event: threading.Event, global_end_event: threading.Event) -> None: - while not (end_event.is_set() or global_end_event.is_set()): - try: - r = select.select((ws.sock,), (), (), 30) - if r[0]: - data = ws.recv() - if isinstance(data, str): - data = data.encode("utf-8") - local_sock.sendall(data) - except WebSocketTimeoutException: - pass - except Exception: - cloudlog.exception("athenad.ws_proxy_recv.exception") - break - - cloudlog.debug("athena.ws_proxy_recv closing sockets") - ssock.close() - local_sock.close() - ws.close() - cloudlog.debug("athena.ws_proxy_recv done closing sockets") - - end_event.set() - - -def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket.socket, end_event: threading.Event) -> None: - while not end_event.is_set(): - try: - r, _, _ = select.select((local_sock, signal_sock), (), ()) - if r: - if r[0].fileno() == signal_sock.fileno(): - # got end signal from ws_proxy_recv - end_event.set() - break - data = local_sock.recv(4096) - if not data: - # local_sock is dead - end_event.set() - break - - ws.send(data, ABNF.OPCODE_BINARY) - except Exception: - cloudlog.exception("athenad.ws_proxy_send.exception") - end_event.set() - - cloudlog.debug("athena.ws_proxy_send closing sockets") - signal_sock.close() - cloudlog.debug("athena.ws_proxy_send done closing sockets") - - -def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: - last_ping = int(time.monotonic() * 1e9) - while not end_event.is_set(): - try: - opcode, data = ws.recv_data(control_frame=True) - if opcode in (ABNF.OPCODE_TEXT, ABNF.OPCODE_BINARY): - if opcode == ABNF.OPCODE_TEXT: - data = data.decode("utf-8") - recv_queue.put_nowait(data) - elif opcode == ABNF.OPCODE_PING: - last_ping = int(time.monotonic() * 1e9) - Params().put("LastAthenaPingTime", last_ping) - except WebSocketTimeoutException: - ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping - if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9: - cloudlog.exception("athenad.ws_recv.timeout") - end_event.set() - except Exception: - cloudlog.exception("athenad.ws_recv.exception") - end_event.set() - - -def ws_send(ws: WebSocket, end_event: threading.Event) -> None: - while not end_event.is_set(): - try: - try: - data = send_queue.get_nowait() - except queue.Empty: - data = low_priority_send_queue.get(timeout=1) - for i in range(0, len(data), WS_FRAME_SIZE): - frame = data[i:i+WS_FRAME_SIZE] - last = i + WS_FRAME_SIZE >= len(data) - opcode = ABNF.OPCODE_TEXT if i == 0 else ABNF.OPCODE_CONT - ws.send_frame(ABNF.create_frame(frame, opcode, last)) - except queue.Empty: - pass - except Exception: - cloudlog.exception("athenad.ws_send.exception") - end_event.set() - - -def ws_manage(ws: WebSocket, end_event: threading.Event) -> None: - params = Params() - onroad_prev = None - sock = ws.sock - - while True: - onroad = params.get_bool("IsOnroad") - if onroad != onroad_prev: - onroad_prev = onroad - - if sock is not None: - # While not sending data, onroad, we can expect to time out in 7 + (7 * 2) = 21s - # offroad, we can expect to time out in 30 + (10 * 3) = 60s - # FIXME: TCP_USER_TIMEOUT is effectively 2x for some reason (32s), so it's mostly unused - if sys.platform == 'linux': - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_USER_TIMEOUT, 16000 if onroad else 0) - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30) - elif sys.platform == 'darwin': - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPALIVE, 7 if onroad else 30) - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10) - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 2 if onroad else 3) - - if end_event.wait(5): - break - - -def backoff(retries: int) -> int: - return random.randrange(0, min(128, int(2 ** retries))) - - -def main(exit_event: threading.Event | None = None): - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("failed to set core affinity") - - params = Params() - dongle_id = params.get("DongleId") - UploadQueueCache.initialize(upload_queue) - - ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id - api = Api(dongle_id) - - conn_start = None - conn_retries = 0 - while exit_event is None or not exit_event.is_set(): - try: - if conn_start is None: - conn_start = time.monotonic() - - cloudlog.event("athenad.main.connecting_ws", ws_uri=ws_uri, retries=conn_retries) - ws = create_connection(ws_uri, - cookie="jwt=" + api.get_token(), - enable_multithread=True, - timeout=30.0) - cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri, retries=conn_retries, - duration=time.monotonic() - conn_start) - conn_start = None - - conn_retries = 0 - cur_upload_items.clear() - - handle_long_poll(ws, exit_event) - - ws.close() - except (KeyboardInterrupt, SystemExit): - break - except (ConnectionError, TimeoutError, WebSocketException): - conn_retries += 1 - params.remove("LastAthenaPingTime") - except Exception: - cloudlog.exception("athenad.main.exception") - - conn_retries += 1 - params.remove("LastAthenaPingTime") - - time.sleep(backoff(conn_retries)) - - -if __name__ == "__main__": - main() diff --git a/system/athena/manage_athenad.py b/system/athena/manage_athenad.py deleted file mode 100755 index ee63606b666c1a..00000000000000 --- a/system/athena/manage_athenad.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python3 - -import time -from multiprocessing import Process - -from openpilot.common.params import Params -from openpilot.system.manager.process import launcher -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import HARDWARE -from openpilot.system.version import get_build_metadata - -ATHENA_MGR_PID_PARAM = "AthenadPid" - - -def main(): - params = Params() - dongle_id = params.get("DongleId") - build_metadata = get_build_metadata() - - cloudlog.bind_global(dongle_id=dongle_id, - version=build_metadata.openpilot.version, - origin=build_metadata.openpilot.git_normalized_origin, - branch=build_metadata.channel, - commit=build_metadata.openpilot.git_commit, - dirty=build_metadata.openpilot.is_dirty, - device=HARDWARE.get_device_type()) - - try: - while 1: - cloudlog.info("starting athena daemon") - proc = Process(name='athenad', target=launcher, args=('system.athena.athenad', 'athenad')) - proc.start() - proc.join() - cloudlog.event("athenad exited", exitcode=proc.exitcode) - time.sleep(5) - except Exception: - cloudlog.exception("manage_athenad.exception") - finally: - params.remove(ATHENA_MGR_PID_PARAM) - - -if __name__ == '__main__': - main() diff --git a/system/athena/registration.py b/system/athena/registration.py deleted file mode 100755 index 405b2423f26d08..00000000000000 --- a/system/athena/registration.py +++ /dev/null @@ -1,104 +0,0 @@ -#!/usr/bin/env python3 -import time -import json -import jwt -from typing import cast -from pathlib import Path - -from datetime import datetime, timedelta, UTC -from openpilot.common.api import api_get, get_key_pair -from openpilot.common.params import Params -from openpilot.common.spinner import Spinner -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.hardware import HARDWARE, PC -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import cloudlog - - -UNREGISTERED_DONGLE_ID = "UnregisteredDevice" - -def is_registered_device() -> bool: - dongle = Params().get("DongleId") - return dongle not in (None, UNREGISTERED_DONGLE_ID) - - -def register(show_spinner=False) -> str | None: - """ - All devices built since March 2024 come with all - info stored in /persist/. This is kept around - only for devices built before then. - - With a backend update to take serial number instead - of dongle ID to some endpoints, this can be removed - entirely. - """ - params = Params() - - dongle_id: str | None = params.get("DongleId") - if dongle_id is None and Path(Paths.persist_root()+"/comma/dongle_id").is_file(): - # not all devices will have this; added early in comma 3X production (2/28/24) - with open(Paths.persist_root()+"/comma/dongle_id") as f: - dongle_id = f.read().strip() - - # Create registration token, in the future, this key will make JWTs directly - jwt_algo, private_key, public_key = get_key_pair() - - if not public_key: - dongle_id = UNREGISTERED_DONGLE_ID - cloudlog.warning("missing public key") - elif dongle_id is None: - if show_spinner: - spinner = Spinner() - spinner.update("registering device") - - # Block until we get the imei - serial = HARDWARE.get_serial() - start_time = time.monotonic() - imei1: str | None = None - imei2: str | None = None - while imei1 is None and imei2 is None: - try: - imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) - except Exception: - cloudlog.exception("Error getting imei, trying again...") - time.sleep(1) - - if time.monotonic() - start_time > 60 and show_spinner: - spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") - - backoff = 0 - start_time = time.monotonic() - while True: - try: - register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)}, - cast(str, private_key), algorithm=jwt_algo) - cloudlog.info("getting pilotauth") - resp = api_get("v2/pilotauth/", method='POST', timeout=15, - imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) - - if resp.status_code in (402, 403): - cloudlog.info(f"Unable to register device, got {resp.status_code}") - dongle_id = UNREGISTERED_DONGLE_ID - else: - dongleauth = json.loads(resp.text) - dongle_id = dongleauth["dongle_id"] - break - except Exception: - cloudlog.exception("failed to authenticate") - backoff = min(backoff + 1, 15) - time.sleep(backoff) - - if time.monotonic() - start_time > 60 and show_spinner: - spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") - - if show_spinner: - spinner.close() - - if dongle_id: - params.put("DongleId", dongle_id) - set_offroad_alert("Offroad_UnregisteredHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC) - return dongle_id - - -if __name__ == "__main__": - print(register()) diff --git a/system/athena/tests/helpers.py b/system/athena/tests/helpers.py deleted file mode 100644 index a0a9cccdc1d187..00000000000000 --- a/system/athena/tests/helpers.py +++ /dev/null @@ -1,70 +0,0 @@ -import http.server -import socket - - -class MockResponse: - def __init__(self, json, status_code): - self.json = json - self.text = json - self.status_code = status_code - - -class EchoSocket: - def __init__(self, port): - self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.socket.bind(('127.0.0.1', port)) - self.socket.listen(1) - - def run(self): - conn, _ = self.socket.accept() - conn.settimeout(5.0) - - try: - while True: - data = conn.recv(4096) - if data: - print(f'EchoSocket got {data}') - conn.sendall(data) - else: - break - finally: - conn.shutdown(0) - conn.close() - self.socket.shutdown(0) - self.socket.close() - - -class MockApi: - def __init__(self, dongle_id): - pass - - def get_token(self): - return "fake-token" - - -class MockWebsocket: - sock = socket.socket() - - def __init__(self, recv_queue, send_queue): - self.recv_queue = recv_queue - self.send_queue = send_queue - - def recv(self): - data = self.recv_queue.get() - if isinstance(data, Exception): - raise data - return data - - def send(self, data, opcode): - self.send_queue.put_nowait((data, opcode)) - - def close(self): - pass - - -class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): - def do_PUT(self): - length = int(self.headers['Content-Length']) - self.rfile.read(length) - self.send_response(201, "Created") - self.end_headers() diff --git a/system/athena/tests/test_athenad.py b/system/athena/tests/test_athenad.py deleted file mode 100644 index 5b3e0b41f48bdd..00000000000000 --- a/system/athena/tests/test_athenad.py +++ /dev/null @@ -1,448 +0,0 @@ -import pytest -from functools import wraps -import json -import multiprocessing -import os -import requests -import shutil -import time -import threading -import queue -from dataclasses import asdict, replace -from datetime import datetime, timedelta - -from websocket import ABNF -from websocket._exceptions import WebSocketConnectionClosedException - -from cereal import messaging - -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.system.athena import athenad -from openpilot.system.athena.athenad import MAX_RETRY_COUNT, UPLOAD_SESS, dispatcher -from openpilot.system.athena.tests.helpers import HTTPRequestHandler, MockWebsocket, MockApi, EchoSocket -from openpilot.selfdrive.test.helpers import http_server_context -from openpilot.system.hardware.hw import Paths - - -def seed_athena_server(host, port): - with Timeout(2, 'HTTP Server seeding failed'): - while True: - try: - UPLOAD_SESS.put(f'http://{host}:{port}/qlog.zst', data='', timeout=10) - break - except requests.exceptions.ConnectionError: - time.sleep(0.1) - -def with_upload_handler(func): - @wraps(func) - def wrapper(*args, **kwargs): - end_event = threading.Event() - thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) - thread.start() - try: - return func(*args, **kwargs) - finally: - end_event.set() - thread.join() - return wrapper - -@pytest.fixture -def mock_create_connection(mocker): - return mocker.patch('openpilot.system.athena.athenad.create_connection') - -@pytest.fixture -def host(): - with http_server_context(handler=HTTPRequestHandler, setup=seed_athena_server) as (host, port): - yield f"http://{host}:{port}" - -class TestAthenadMethods: - @classmethod - def setup_class(cls): - cls.SOCKET_PORT = 45454 - athenad.Api = MockApi - athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT} - - def setup_method(self): - self.default_params = { - "DongleId": "0000000000000000", - "GithubSshKeys": "ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501 - "GithubUsername": "commaci", - "AthenadUploadQueue": [], - } - - self.params = Params() - for k, v in self.default_params.items(): - self.params.put(k, v) - self.params.put_bool("GsmMetered", True) - - athenad.upload_queue = queue.PriorityQueue() - athenad.cur_upload_items.clear() - athenad.cancelled_uploads.clear() - - for i in os.listdir(Paths.log_root()): - p = os.path.join(Paths.log_root(), i) - if os.path.isdir(p): - shutil.rmtree(p) - else: - os.unlink(p) - - # *** test helpers *** - - @staticmethod - def _wait_for_upload(): - now = time.monotonic() - while time.monotonic() - now < 5: - if athenad.upload_queue.qsize() == 0: - break - - @staticmethod - def _create_file(file: str, parent: str | None = None, data: bytes = b'') -> str: - fn = os.path.join(Paths.log_root() if parent is None else parent, file) - os.makedirs(os.path.dirname(fn), exist_ok=True) - with open(fn, 'wb') as f: - f.write(data) - return fn - - - # *** test cases *** - - def test_echo(self): - assert dispatcher["echo"]("bob") == "bob" - - def test_get_message(self): - with pytest.raises(TimeoutError) as _: - dispatcher["getMessage"]("controlsState") - - end_event = multiprocessing.Event() - - pub_sock = messaging.pub_sock("deviceState") - - def send_deviceState(): - while not end_event.is_set(): - msg = messaging.new_message('deviceState') - pub_sock.send(msg.to_bytes()) - time.sleep(0.01) - - p = multiprocessing.Process(target=send_deviceState) - p.start() - time.sleep(0.1) - try: - deviceState = dispatcher["getMessage"]("deviceState") - assert deviceState['deviceState'] - finally: - end_event.set() - p.join() - - def test_list_data_directory(self): - route = '2021-03-29--13-32-47' - segments = [0, 1, 2, 3, 11] - - filenames = ['qlog.zst', 'qcamera.ts', 'rlog.zst', 'fcamera.hevc', 'ecamera.hevc', 'dcamera.hevc'] - files = [f'{route}--{s}/{f}' for s in segments for f in filenames] - for file in files: - self._create_file(file) - - resp = dispatcher["listDataDirectory"]() - assert resp, 'list empty!' - assert len(resp) == len(files) - - resp = dispatcher["listDataDirectory"](f'{route}--123') - assert len(resp) == 0 - - prefix = f'{route}' - expected = list(filter(lambda f: f.startswith(prefix), files)) - resp = dispatcher["listDataDirectory"](prefix) - assert resp, 'list empty!' - assert len(resp) == len(expected) - - prefix = f'{route}--1' - expected = list(filter(lambda f: f.startswith(prefix), files)) - resp = dispatcher["listDataDirectory"](prefix) - assert resp, 'list empty!' - assert len(resp) == len(expected) - - prefix = f'{route}--1/' - expected = list(filter(lambda f: f.startswith(prefix), files)) - resp = dispatcher["listDataDirectory"](prefix) - assert resp, 'list empty!' - assert len(resp) == len(expected) - - prefix = f'{route}--1/q' - expected = list(filter(lambda f: f.startswith(prefix), files)) - resp = dispatcher["listDataDirectory"](prefix) - assert resp, 'list empty!' - assert len(resp) == len(expected) - - def test_strip_extension(self): - # any requested log file with an invalid extension won't return as existing - fn = self._create_file('qlog.bz2') - if fn.endswith('.bz2'): - assert athenad.strip_zst_extension(fn) == fn - - fn = self._create_file('qlog.zst') - if fn.endswith('.zst'): - assert athenad.strip_zst_extension(fn) == fn[:-4] - - @pytest.mark.parametrize("compress", [True, False]) - def test_do_upload(self, host, compress): - # random bytes to ensure rather large object post-compression - fn = self._create_file('qlog', data=os.urandom(10000 * 1024)) - - upload_fn = fn + ('.zst' if compress else '') - item = athenad.UploadItem(path=upload_fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') # noqa: TID251 - with pytest.raises(requests.exceptions.ConnectionError): - athenad._do_upload(item) - - item = athenad.UploadItem(path=upload_fn, url=f"{host}/qlog.zst", headers={}, created_at=int(time.time()*1000), id='') # noqa: TID251 - resp = athenad._do_upload(item) - assert resp.status_code == 201 - - def test_upload_file_to_url(self, host): - fn = self._create_file('qlog.zst') - - resp = dispatcher["uploadFileToUrl"]("qlog.zst", f"{host}/qlog.zst", {}) - assert resp['enqueued'] == 1 - assert 'failed' not in resp - assert {"path": fn, "url": f"{host}/qlog.zst", "headers": {}}.items() <= resp['items'][0].items() - assert resp['items'][0].get('id') is not None - assert athenad.upload_queue.qsize() == 1 - - def test_upload_file_to_url_duplicate(self, host): - self._create_file('qlog.zst') - - url1 = f"{host}/qlog.zst?sig=sig1" - dispatcher["uploadFileToUrl"]("qlog.zst", url1, {}) - - # Upload same file again, but with different signature - url2 = f"{host}/qlog.zst?sig=sig2" - resp = dispatcher["uploadFileToUrl"]("qlog.zst", url2, {}) - assert resp == {'enqueued': 0, 'items': []} - - def test_upload_file_to_url_does_not_exist(self, host): - not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.zst", "http://localhost:1238", {}) - assert not_exists_resp == {'enqueued': 0, 'items': [], 'failed': ['does_not_exist.zst']} - - @with_upload_handler - def test_upload_handler(self, host): - fn = self._create_file('qlog.zst') - item = athenad.UploadItem(path=fn, url=f"{host}/qlog.zst", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) # noqa: TID251 - - athenad.upload_queue.put_nowait(item) - self._wait_for_upload() - time.sleep(0.1) - - # TODO: verify that upload actually succeeded - # TODO: also check that end_event and metered network raises AbortTransferException - assert athenad.upload_queue.qsize() == 0 - - @pytest.mark.parametrize("status,retry", [(500,True), (412,False)]) - @with_upload_handler - def test_upload_handler_retry(self, mocker, host, status, retry): - mock_put = mocker.patch('openpilot.system.athena.athenad.UPLOAD_SESS.put') - mock_put.return_value.__enter__.return_value.status_code = status - fn = self._create_file('qlog.zst') - item = athenad.UploadItem(path=fn, url=f"{host}/qlog.zst", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) # noqa: TID251 - - athenad.upload_queue.put_nowait(item) - self._wait_for_upload() - time.sleep(0.1) - - assert athenad.upload_queue.qsize() == (1 if retry else 0) - - if retry: - assert athenad.upload_queue.get().retry_count == 1 - - @with_upload_handler - def test_upload_handler_timeout(self): - """When an upload times out or fails to connect it should be placed back in the queue""" - fn = self._create_file('qlog.zst') - item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.zst", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) # noqa: TID251 - item_no_retry = replace(item, retry_count=MAX_RETRY_COUNT) - - athenad.upload_queue.put_nowait(item_no_retry) - self._wait_for_upload() - time.sleep(0.1) - - # Check that upload with retry count exceeded is not put back - assert athenad.upload_queue.qsize() == 0 - - athenad.upload_queue.put_nowait(item) - self._wait_for_upload() - time.sleep(0.1) - - # Check that upload item was put back in the queue with incremented retry count - assert athenad.upload_queue.qsize() == 1 - assert athenad.upload_queue.get().retry_count == 1 - - @with_upload_handler - def test_cancel_upload(self): - item = athenad.UploadItem(path="qlog.zst", url="http://localhost:44444/qlog.zst", headers={}, - created_at=int(time.time()*1000), id='id', allow_cellular=True) # noqa: TID251 - athenad.upload_queue.put_nowait(item) - dispatcher["cancelUpload"](item.id) - - assert item.id in athenad.cancelled_uploads - - self._wait_for_upload() - time.sleep(0.1) - - assert athenad.upload_queue.qsize() == 0 - assert len(athenad.cancelled_uploads) == 0 - - @with_upload_handler - def test_cancel_expiry(self): - t_future = datetime.now() - timedelta(days=40) - ts = int(t_future.strftime("%s")) * 1000 - - # Item that would time out if actually uploaded - fn = self._create_file('qlog.zst') - item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.zst", headers={}, created_at=ts, id='', allow_cellular=True) - - athenad.upload_queue.put_nowait(item) - self._wait_for_upload() - time.sleep(0.1) - - assert athenad.upload_queue.qsize() == 0 - - def test_list_upload_queue_empty(self): - items = dispatcher["listUploadQueue"]() - assert len(items) == 0 - - @with_upload_handler - def test_list_upload_queue_current(self, host: str): - fn = self._create_file('qlog.zst') - item = athenad.UploadItem(path=fn, url=f"{host}/qlog.zst", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) # noqa: TID251 - - athenad.upload_queue.put_nowait(item) - self._wait_for_upload() - - items = dispatcher["listUploadQueue"]() - assert len(items) == 1 - assert items[0]['current'] - - def test_list_upload_queue_priority(self): - priorities = (25, 50, 99, 75, 0) - - for i in priorities: - fn = f'qlog_{i}.zst' - fp = self._create_file(fn) - item = athenad.UploadItem( - path=fp, - url=f"http://localhost:44444/{fn}", - headers={}, - created_at=int(time.time()*1000), # noqa: TID251 - id='', - allow_cellular=True, - priority=i - ) - athenad.upload_queue.put_nowait(item) - - for i in sorted(priorities): - assert athenad.upload_queue.get_nowait().priority == i - - def test_list_upload_queue(self): - item = athenad.UploadItem(path="qlog.zst", url="http://localhost:44444/qlog.zst", headers={}, - created_at=int(time.time()*1000), id='id', allow_cellular=True) # noqa: TID251 - athenad.upload_queue.put_nowait(item) - - items = dispatcher["listUploadQueue"]() - assert len(items) == 1 - assert items[0] == asdict(item) - assert not items[0]['current'] - - athenad.cancelled_uploads.add(item.id) - items = dispatcher["listUploadQueue"]() - assert len(items) == 0 - - def test_upload_queue_persistence(self): - item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1') # noqa: TID251 - item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2') # noqa: TID251 - - athenad.upload_queue.put_nowait(item1) - athenad.upload_queue.put_nowait(item2) - - # Ensure canceled items are not persisted - athenad.cancelled_uploads.add(item2.id) - - # serialize item - athenad.UploadQueueCache.cache(athenad.upload_queue) - - # deserialize item - athenad.upload_queue.queue.clear() - athenad.UploadQueueCache.initialize(athenad.upload_queue) - - assert athenad.upload_queue.qsize() == 1 - assert asdict(athenad.upload_queue.queue[-1]) == asdict(item1) - - def test_start_local_proxy(self, mock_create_connection): - end_event = threading.Event() - - ws_recv = queue.Queue() - ws_send = queue.Queue() - mock_ws = MockWebsocket(ws_recv, ws_send) - mock_create_connection.return_value = mock_ws - - echo_socket = EchoSocket(self.SOCKET_PORT) - socket_thread = threading.Thread(target=echo_socket.run) - socket_thread.start() - - athenad.startLocalProxy(end_event, 'ws://localhost:1234', self.SOCKET_PORT) - - ws_recv.put_nowait(b'ping') - try: - recv = ws_send.get(timeout=5) - assert recv == (b'ping', ABNF.OPCODE_BINARY), recv - finally: - # signal websocket close to athenad.ws_proxy_recv - ws_recv.put_nowait(WebSocketConnectionClosedException()) - socket_thread.join() - - def test_get_ssh_authorized_keys(self): - keys = dispatcher["getSshAuthorizedKeys"]() - assert keys == self.default_params["GithubSshKeys"] - - def test_get_github_username(self): - keys = dispatcher["getGithubUsername"]() - assert keys == self.default_params["GithubUsername"] - - def test_get_version(self): - resp = dispatcher["getVersion"]() - keys = ["version", "remote", "branch", "commit"] - assert list(resp.keys()) == keys - for k in keys: - assert isinstance(resp[k], str), f"{k} is not a string" - assert len(resp[k]) > 0, f"{k} has no value" - - def test_jsonrpc_handler(self): - end_event = threading.Event() - thread = threading.Thread(target=athenad.jsonrpc_handler, args=(end_event,)) - thread.daemon = True - thread.start() - try: - # with params - athenad.recv_queue.put_nowait(json.dumps({"method": "echo", "params": ["hello"], "jsonrpc": "2.0", "id": 0})) - resp = athenad.send_queue.get(timeout=3) - assert json.loads(resp) == {'result': 'hello', 'id': 0, 'jsonrpc': '2.0'} - # without params - athenad.recv_queue.put_nowait(json.dumps({"method": "getNetworkType", "jsonrpc": "2.0", "id": 0})) - resp = athenad.send_queue.get(timeout=3) - assert json.loads(resp) == {'result': 1, 'id': 0, 'jsonrpc': '2.0'} - # log forwarding - athenad.recv_queue.put_nowait(json.dumps({'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'})) - resp = athenad.log_recv_queue.get(timeout=3) - assert json.loads(resp) == {'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'} - finally: - end_event.set() - thread.join() - - def test_get_logs_to_send_sorted(self): - fl = list() - for i in range(10): - file = f'swaglog.{i:010}' - self._create_file(file, Paths.swaglog_root()) - fl.append(file) - - # ensure the list is all logs except most recent - sl = athenad.get_logs_to_send_sorted() - assert sl == fl[:-1] diff --git a/system/athena/tests/test_athenad_ping.py b/system/athena/tests/test_athenad_ping.py deleted file mode 100644 index 8ff1e37a5de7c9..00000000000000 --- a/system/athena/tests/test_athenad_ping.py +++ /dev/null @@ -1,102 +0,0 @@ -import pytest -import subprocess -import threading -import time -from typing import cast - -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.system.athena import athenad -from openpilot.system.manager.helpers import write_onroad_params -from openpilot.system.hardware import TICI - -TIMEOUT_TOLERANCE = 20 # seconds - - -def wifi_radio(on: bool) -> None: - if not TICI: - return - print(f"wifi {'on' if on else 'off'}") - subprocess.run(["nmcli", "radio", "wifi", "on" if on else "off"], check=True) - - -class TestAthenadPing: - params: Params - dongle_id: str - - athenad: threading.Thread - exit_event: threading.Event - - def _get_ping_time(self) -> str | None: - return cast(str | None, self.params.get("LastAthenaPingTime")) - - def _clear_ping_time(self) -> None: - self.params.remove("LastAthenaPingTime") - - def _received_ping(self) -> bool: - return self._get_ping_time() is not None - - @classmethod - def teardown_class(cls) -> None: - wifi_radio(True) - - def setup_method(self) -> None: - self.params = Params() - self.dongle_id = self.params.get("DongleId") - - wifi_radio(True) - self._clear_ping_time() - - self.exit_event = threading.Event() - self.athenad = threading.Thread(target=athenad.main, args=(self.exit_event,)) - - def teardown_method(self) -> None: - if self.athenad.is_alive(): - self.exit_event.set() - self.athenad.join() - - def assertTimeout(self, reconnect_time: float, subtests, mocker) -> None: - self.athenad.start() - - mock_create_connection = mocker.patch('openpilot.system.athena.athenad.create_connection', - new_callable=lambda: mocker.MagicMock(wraps=athenad.create_connection)) - - time.sleep(1) - mock_create_connection.assert_called_once() - mock_create_connection.reset_mock() - - # check normal behavior, server pings on connection - with subtests.test("Wi-Fi: receives ping"), Timeout(70, "no ping received"): - while not self._received_ping(): - time.sleep(0.1) - print("ping received") - - mock_create_connection.assert_not_called() - - # websocket should attempt reconnect after short time - with subtests.test("LTE: attempt reconnect"): - wifi_radio(False) - print("waiting for reconnect attempt") - start_time = time.monotonic() - with Timeout(reconnect_time, "no reconnect attempt"): - while not mock_create_connection.called: - time.sleep(0.1) - print(f"reconnect attempt after {time.monotonic() - start_time:.2f}s") - - self._clear_ping_time() - - # check ping received after reconnect - with subtests.test("LTE: receives ping"), Timeout(70, "no ping received"): - while not self._received_ping(): - time.sleep(0.1) - print("ping received") - - @pytest.mark.skipif(not TICI, reason="only run on desk") - def test_offroad(self, subtests, mocker) -> None: - write_onroad_params(False, self.params) - self.assertTimeout(60 + TIMEOUT_TOLERANCE, subtests, mocker) # based using TCP keepalive settings - - @pytest.mark.skipif(not TICI, reason="only run on desk") - def test_onroad(self, subtests, mocker) -> None: - write_onroad_params(True, self.params) - self.assertTimeout(21 + TIMEOUT_TOLERANCE, subtests, mocker) diff --git a/system/athena/tests/test_registration.py b/system/athena/tests/test_registration.py deleted file mode 100644 index c20722659abaf5..00000000000000 --- a/system/athena/tests/test_registration.py +++ /dev/null @@ -1,76 +0,0 @@ -import json -from Crypto.PublicKey import RSA -from pathlib import Path - -from openpilot.common.params import Params -from openpilot.system.athena.registration import register, UNREGISTERED_DONGLE_ID -from openpilot.system.athena.tests.helpers import MockResponse -from openpilot.system.hardware.hw import Paths - - -class TestRegistration: - - def setup_method(self): - # clear params and setup key paths - self.params = Params() - - persist_dir = Path(Paths.persist_root()) / "comma" - persist_dir.mkdir(parents=True, exist_ok=True) - - self.priv_key = persist_dir / "id_rsa" - self.pub_key = persist_dir / "id_rsa.pub" - self.dongle_id = persist_dir / "dongle_id" - - def _generate_keys(self): - self.pub_key.touch() - k = RSA.generate(2048) - with open(self.priv_key, "wb") as f: - f.write(k.export_key()) - with open(self.pub_key, "wb") as f: - f.write(k.publickey().export_key()) - - def test_valid_cache(self, mocker): - # if all params are written, return the cached dongle id. - # should work with a dongle ID on either /persist/ or normal params - self._generate_keys() - - dongle = "DONGLE_ID_123" - m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) - for persist, params in [(True, True), (True, False), (False, True)]: - self.params.put("DongleId", dongle if params else "") - with open(self.dongle_id, "w") as f: - f.write(dongle if persist else "") - assert register() == dongle - assert not m.called - - def test_no_keys(self, mocker): - # missing pubkey - m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) - dongle = register() - assert m.call_count == 0 - assert dongle == UNREGISTERED_DONGLE_ID - assert self.params.get("DongleId") == dongle - - def test_missing_cache(self, mocker): - # keys exist but no dongle id - self._generate_keys() - m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) - dongle = "DONGLE_ID_123" - m.return_value = MockResponse(json.dumps({'dongle_id': dongle}), 200) - assert register() == dongle - assert m.call_count == 1 - - # call again, shouldn't hit the API this time - assert register() == dongle - assert m.call_count == 1 - assert self.params.get("DongleId") == dongle - - def test_unregistered(self, mocker): - # keys exist, but unregistered - self._generate_keys() - m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) - m.return_value = MockResponse(None, 402) - dongle = register() - assert m.call_count == 1 - assert dongle == UNREGISTERED_DONGLE_ID - assert self.params.get("DongleId") == dongle diff --git a/system/camerad/SConscript b/system/camerad/SConscript index e288c6d8b02816..ddc763b53d1240 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -1,11 +1,18 @@ -Import('env', 'arch', 'messaging', 'common', 'visionipc') +Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') -libs = [common, 'OpenCL', messaging, visionipc] +libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] -if arch != "Darwin": - camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc', - 'cameras/cdm.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) - env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) +cenv = env.Clone() +cenv['CPPPATH'].append('include/') -if GetOption("extras") and arch == "x86_64": - env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs) +camera_obj = cenv.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) +cenv.Program('camerad', [ + 'main.cc', + camera_obj, + ], LIBS=libs) + +if GetOption("test") and arch == "x86_64": + cenv.Program('test/ae_gray_test', [ + 'test/ae_gray_test.cc', + camera_obj, + ], LIBS=libs) diff --git a/system/camerad/cameras/bps_blobs.h b/system/camerad/cameras/bps_blobs.h deleted file mode 100644 index 54941b8d76a6ec..00000000000000 --- a/system/camerad/cameras/bps_blobs.h +++ /dev/null @@ -1,30 +0,0 @@ -#include -#include - - -/* ********************************************************** - THIS FILE IS AUTO-GENERATED, DO NOT EDIT DIRECTLY! - ********************************************************** */ - - -unsigned char bps_cfg[4][768] = { -{ /* placeholder */ }, - {0x3, 0x0, 0x0, 0x0, 0x88, 0x7, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0xB, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x14, 0x0, 0x0, 0x0, 0x88, 0x7, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0xC0, 0x4, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x60, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, - {0x3, 0x0, 0x0, 0x0, 0x88, 0x7, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0xB, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x14, 0x0, 0x0, 0x0, 0x88, 0x7, 0x0, 0x0, 0xB8, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0xC0, 0x4, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x60, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, - {0x3, 0x0, 0x0, 0x0, 0x40, 0x5, 0x0, 0x0, 0xF8, 0x2, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE0, 0x7, 0x0, 0x0, 0xF8, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x14, 0x0, 0x0, 0x0, 0x40, 0x5, 0x0, 0x0, 0xF8, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x80, 0x5, 0x0, 0x0, 0x0, 0x3, 0x0, 0x0, 0x80, 0x5, 0x0, 0x0, 0x80, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, -}; - -unsigned char bps_striping_output[4][0x9a0] = { -{ /* placeholder */ }, - {0x5, 0x0, 0x6, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xFF, 0xFF, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x5B, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x5B, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x4, 0x5C, 0x2, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x5, 0x0, 0x0, 0x0, 0x70, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0x89, 0x23, 0x0, 0x1, 0x0, 0x0, 0x0, 0xCC, 0x5, 0x87, 0x7, 0x0, 0x0, 0x0, 0x0, 0xD0, 0x5, 0x87, 0x7, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x2, 0xA4, 0xFD, 0x50, 0xB1, 0x9, 0x0, 0x8, 0x0, 0x33, 0x0, 0x8, 0x2, 0xA4, 0xFD, 0x50, 0xB1, 0x9, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE, 0x2, 0xA6, 0xFD, 0x68, 0xC0, 0x9, 0x0, 0x8, 0x0, 0x34, 0x0, 0xD0, 0x5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0x14, 0x4, 0xD3, 0x5, 0x0, 0x0, 0x0, 0x0, 0x18, 0x4, 0xCF, 0x5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0xA, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x56, 0x0, 0xA6, 0xFD, 0x88, 0xA4, 0x5, 0x0, 0x8, 0x0, 0x34, 0x0, 0x18, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0x5C, 0x2, 0x1B, 0x4, 0x0, 0x0, 0x0, 0x0, 0x60, 0x2, 0x17, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x98, 0xFE, 0xA4, 0xFD, 0x50, 0x8B, 0x7, 0x0, 0x8, 0x0, 0x33, 0x0, 0x98, 0xFE, 0xA4, 0xFD, 0x50, 0x8B, 0x7, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9E, 0xFE, 0xA6, 0xFD, 0x28, 0x71, 0x7, 0x0, 0x8, 0x0, 0x34, 0x0, 0x60, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0xC0, 0x0, 0x63, 0x2, 0x0, 0x0, 0x0, 0x0, 0xC4, 0x0, 0x5F, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0x9C, 0x1, 0xCE, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFC, 0xFC, 0xA4, 0xFD, 0x20, 0xA9, 0xE, 0x0, 0x8, 0x0, 0x33, 0x0, 0xFC, 0xFC, 0xA4, 0xFD, 0x20, 0xA9, 0xE, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0xFD, 0xA6, 0xFD, 0xA8, 0x7B, 0xE, 0x0, 0x8, 0x0, 0x34, 0x0, 0xC4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x20, 0x98, 0x7, 0x0, 0x0, 0x0, 0xC7, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xC4, 0x0, 0x62, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x3C, 0xFC, 0xA4, 0xFD, 0x20, 0xBF, 0x13, 0x0, 0x8, 0x0, 0x33, 0x0, 0x3C, 0xFC, 0xA4, 0xFD, 0x20, 0xBF, 0x13, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x3E, 0xFC, 0xA6, 0xFD, 0xA8, 0xA6, 0x13, 0x0, 0x8, 0x0, 0x34, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE0, 0x9C, 0x3, 0x0, 0x20, 0x4, 0x1, 0x0, 0x5D, 0x59, 0xAB, 0x0, 0xC8, 0x8C, 0xFD, 0xF4, 0x3, 0x0, 0x0, 0x0, 0xB8, 0x13, 0xFD, 0xFF, 0xAC, 0x5F, 0x8C, 0xF5, 0x0, 0x20, 0x4E, 0x0, 0xAB, 0xAA, 0xAA, 0xAA, 0x40, 0x69, 0xFD, 0xF4, 0x0, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x38, 0x6B, 0x8B, 0xF5, 0x11, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x9F, 0x9, 0x0, 0x0, 0xB8, 0x2B, 0x6B, 0x15, 0xC0, 0x6A, 0x8C, 0xF5, 0x0, 0x0, 0x0, 0x0, 0xB8, 0x13, 0xFD, 0xFF, 0x2A, 0x5, 0x1, 0x0, 0xC0, 0x13, 0xFD, 0xFF, 0x2C, 0x14, 0xFD, 0xFF, 0x14, 0x14, 0xFD, 0xFF, 0xCC, 0xE8, 0x89, 0xF5, 0xC0, 0x13, 0xFD, 0xFF, 0x64, 0x6A, 0x8C, 0xF5, 0x3, 0x0, 0x0, 0x0, 0xA0, 0x6B, 0x8B, 0xF5, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x8, 0x69, 0x8C, 0xF5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0x6A, 0x8C, 0xF5, 0x8, 0x69, 0x8C, 0xF5, 0xFF, 0xFF, 0xFF, 0xFF, 0xA0, 0x9, 0x0, 0x0, 0xF8, 0xB2, 0xFD, 0xF4, 0x50, 0x35, 0x8C, 0xF5, 0xD0, 0x14, 0xC1, 0xF4, 0x1, 0xD0, 0x3B, 0xF5, 0x64, 0x1F, 0xFD, 0xFF, 0xD0, 0xAD, 0x4, 0xF5, 0x2C, 0x30, 0x1, 0x0, 0x0, 0x10, 0x0, 0x0, 0x2C, 0x0, 0x0, 0x0, 0x14, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC, 0x30, 0x1, 0x0, 0xD0, 0x14, 0xC1, 0xF4, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x10, 0x1F, 0xFD, 0xFF, 0x3C, 0x23, 0xFD, 0x34, 0x31, 0x32, 0x31, 0x36, 0xA0, 0x6B, 0x8B, 0xF5, 0x60, 0x14, 0xFD, 0xFF, 0x0, 0x30, 0x1, 0x0, 0x14, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x10, 0x1F, 0xFD, 0xFF, 0x3C, 0x23, 0xFD, 0xFF, 0xC0, 0xDC, 0x1, 0xF5, 0x4C, 0x0, 0x0, 0x0, 0x54, 0x14, 0xFD, 0xFF, 0x50, 0x1D, 0x1, 0x0, 0x30, 0x14, 0x1, 0x0, 0x98, 0x1D, 0x1, 0x0, 0x0, 0xA1, 0x0, 0x0, 0x0, 0xA1, 0x0, 0x0, 0x98, 0x1D, 0x1, 0x0, 0x6C, 0x1F, 0xFD, 0xFF, 0x64, 0x1F, 0xFD, 0xFF, 0x68, 0x1F, 0xFD, 0xFF, 0x0, 0x0, 0x0, 0x0}, - {0x5, 0x0, 0x6, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xFF, 0xFF, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x5B, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x5B, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x4, 0x5C, 0x2, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x5, 0x0, 0x0, 0x0, 0x70, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0x89, 0x23, 0x0, 0x1, 0x0, 0x0, 0x0, 0xCC, 0x5, 0x87, 0x7, 0x0, 0x0, 0x0, 0x0, 0xD0, 0x5, 0x87, 0x7, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x2, 0xA4, 0xFD, 0x50, 0xB1, 0x9, 0x0, 0x8, 0x0, 0x33, 0x0, 0x8, 0x2, 0xA4, 0xFD, 0x50, 0xB1, 0x9, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE, 0x2, 0xA6, 0xFD, 0x68, 0xC0, 0x9, 0x0, 0x8, 0x0, 0x34, 0x0, 0xD0, 0x5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0x14, 0x4, 0xD3, 0x5, 0x0, 0x0, 0x0, 0x0, 0x18, 0x4, 0xCF, 0x5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0xA, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x56, 0x0, 0xA6, 0xFD, 0x88, 0xA4, 0x5, 0x0, 0x8, 0x0, 0x34, 0x0, 0x18, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0x5C, 0x2, 0x1B, 0x4, 0x0, 0x0, 0x0, 0x0, 0x60, 0x2, 0x17, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x98, 0xFE, 0xA4, 0xFD, 0x50, 0x8B, 0x7, 0x0, 0x8, 0x0, 0x33, 0x0, 0x98, 0xFE, 0xA4, 0xFD, 0x50, 0x8B, 0x7, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9E, 0xFE, 0xA6, 0xFD, 0x28, 0x71, 0x7, 0x0, 0x8, 0x0, 0x34, 0x0, 0x60, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1C, 0x8, 0x0, 0xC0, 0x0, 0x63, 0x2, 0x0, 0x0, 0x0, 0x0, 0xC4, 0x0, 0x5F, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x9B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0x9C, 0x1, 0xCE, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFC, 0xFC, 0xA4, 0xFD, 0x20, 0xA9, 0xE, 0x0, 0x8, 0x0, 0x33, 0x0, 0xFC, 0xFC, 0xA4, 0xFD, 0x20, 0xA9, 0xE, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0xFD, 0xA6, 0xFD, 0xA8, 0x7B, 0xE, 0x0, 0x8, 0x0, 0x34, 0x0, 0xC4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x20, 0x98, 0x7, 0x0, 0x0, 0x0, 0xC7, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xC4, 0x0, 0x62, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x3C, 0xFC, 0xA4, 0xFD, 0x20, 0xBF, 0x13, 0x0, 0x8, 0x0, 0x33, 0x0, 0x3C, 0xFC, 0xA4, 0xFD, 0x20, 0xBF, 0x13, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x3E, 0xFC, 0xA6, 0xFD, 0xA8, 0xA6, 0x13, 0x0, 0x8, 0x0, 0x34, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE0, 0x9C, 0x3, 0x0, 0x20, 0x4, 0x1, 0x0, 0xA0, 0x9, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x60, 0x5D, 0x1, 0x0, 0x0, 0x0, 0x0, 0x7, 0x0, 0x0, 0x0, 0xFC, 0x57, 0x12, 0xF5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0x12, 0xF5, 0x0, 0x0, 0x0, 0x0, 0x9F, 0x9, 0x0, 0x0, 0x5E, 0x0, 0x0, 0x0, 0x5F, 0x0, 0x0, 0x0, 0x6E, 0x0, 0x0, 0x0, 0x77, 0x0, 0x0, 0x0, 0x7C, 0x0, 0x0, 0x0, 0x35, 0x1, 0x0, 0x0, 0x26, 0x0, 0x0, 0x0, 0xA4, 0x61, 0x5D, 0x1, 0x58, 0x3, 0x0, 0x0, 0xC0, 0x13, 0xFD, 0xFF, 0x64, 0x6A, 0x8C, 0xF5, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0x12, 0xF5, 0x9F, 0x9, 0x0, 0x0, 0xA0, 0x9, 0x0, 0x0, 0xFC, 0x57, 0x12, 0xF5, 0x33, 0x1, 0x0, 0x0, 0xD0, 0x14, 0xC1, 0xF4, 0x1, 0xD0, 0x3B, 0xF5, 0x64, 0x1F, 0xFD, 0xFF, 0xD0, 0xAD, 0x4, 0xF5, 0x2C, 0x30, 0x1, 0x0, 0x0, 0x10, 0x0, 0x0, 0x2C, 0x0, 0x0, 0x0, 0x14, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4C, 0x27, 0x5E, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0x0, 0x0, 0x0, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x34, 0x31, 0x32, 0x31, 0x36, 0x10, 0x20, 0xFD, 0xFF, 0x60, 0x14, 0xFD, 0xFF, 0x0, 0x30, 0x1, 0x0, 0xA0, 0x9, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x10, 0x1F, 0xFD, 0xFF, 0x3C, 0x23, 0xFD, 0xFF, 0xC0, 0xDC, 0x1, 0xF5, 0x0, 0x30, 0x1, 0x0, 0x54, 0x14, 0xFD, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x30, 0x14, 0x1, 0x0, 0x98, 0x1D, 0x1, 0x0, 0x0, 0xA1, 0x0, 0x0, 0x0, 0xA1, 0x0, 0x0, 0x98, 0x1D, 0x1, 0x0, 0x6C, 0x1F, 0xFD, 0xFF, 0x64, 0x1F, 0xFD, 0xFF, 0x68, 0x1F, 0xFD, 0xFF, 0x0, 0x0, 0x0, 0x0}, - {0x4, 0x0, 0x4, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0xFF, 0xFF, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0x7B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0x7B, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF7, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xF8, 0x2, 0x7C, 0x1, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4, 0x0, 0x2, 0x0, 0x70, 0x1, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x50, 0x0, 0xA4, 0xFD, 0x10, 0xAA, 0x5, 0x0, 0x8, 0x0, 0x33, 0x0, 0x0, 0x0, 0x0, 0x0, 0xA, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x56, 0x0, 0xA6, 0xFD, 0x88, 0xA4, 0x5, 0x0, 0x8, 0x0, 0x34, 0x0, 0x18, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x24, 0x0, 0x0, 0x0, 0x78, 0x14, 0x5E, 0x1, 0xB8, 0x5D, 0x12, 0xF5, 0xDC, 0x3B, 0x12, 0xF5, 0x24, 0x0, 0x0, 0x0, 0xFC, 0xF9, 0x3, 0xF5, 0x0, 0x96, 0xF, 0x0, 0x1, 0x5, 0x0, 0x0, 0x84, 0x3, 0x3F, 0x5, 0x0, 0x0, 0x0, 0x0, 0x88, 0x3, 0x3F, 0x5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE4, 0x0, 0x84, 0xFE, 0x20, 0xFF, 0x2, 0x0, 0x7, 0x0, 0x38, 0x0, 0xE4, 0x0, 0x84, 0xFE, 0x20, 0xFF, 0x2, 0x0, 0x7, 0x0, 0x38, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xEA, 0x0, 0x86, 0xFE, 0x8, 0x4, 0x3, 0x0, 0x7, 0x0, 0x38, 0x0, 0x88, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1A, 0x5, 0x0, 0xCC, 0x1, 0x8B, 0x3, 0x0, 0x0, 0x0, 0x0, 0xD0, 0x1, 0x87, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0xB7, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xB8, 0x1, 0xDC, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2C, 0xFF, 0x84, 0xFE, 0xA0, 0xE3, 0x2, 0x0, 0x7, 0x0, 0x38, 0x0, 0x2C, 0xFF, 0x84, 0xFE, 0xA0, 0xE3, 0x2, 0x0, 0x7, 0x0, 0x38, 0x0, 0x0, 0x0, 0x0, 0x0, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x32, 0xFF, 0x86, 0xFE, 0xE8, 0xD3, 0x2, 0x0, 0x7, 0x0, 0x38, 0x0, 0xD0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x1A, 0x5, 0x0, 0xC0, 0x0, 0xD3, 0x1, 0x0, 0x0, 0x0, 0x0, 0xC4, 0x0, 0xCF, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0xB, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xC, 0x1, 0x86, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x20, 0xFE, 0x84, 0xFE, 0x10, 0xB8, 0x5, 0x0, 0x7, 0x0, 0x38, 0x0, 0x20, 0xFE, 0x84, 0xFE, 0x10, 0xB8, 0x5, 0x0, 0x7, 0x0, 0x38, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x26, 0xFE, 0x86, 0xFE, 0xC8, 0x9B, 0x5, 0x0, 0x7, 0x0, 0x38, 0x0, 0xC4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x8, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xA0, 0x1B, 0x3, 0x0, 0x0, 0x0, 0xC7, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0xB, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0xC3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x40, 0x0, 0x1, 0x0, 0xC4, 0x0, 0x62, 0x0, 0x0, 0x0, 0x3, 0x0, 0x2, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF5, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x60, 0xFD, 0x84, 0xFE, 0x10, 0x18, 0x9, 0x0, 0x7, 0x0, 0x38, 0x0, 0x60, 0xFD, 0x84, 0xFE, 0x10, 0x18, 0x9, 0x0, 0x7, 0x0, 0x38, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x62, 0xFD, 0x86, 0xFE, 0xA8, 0x7, 0x9, 0x0, 0x7, 0x0, 0x38, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xE0, 0x45, 0x2, 0x0}, -}; - -unsigned char bps_settings[4][684] = { -{ /* placeholder */ }, - {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, - {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, - {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}, -}; - diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 88bca7f775bf35..d033d8e6b4e8f2 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -1,63 +1,180 @@ #include "system/camerad/cameras/camera_common.h" +#include + #include -#include +#include +#include +#include + +#include "libyuv.h" +#include +#include "system/camerad/imgproc/utils.h" +#include "common/clutil.h" +#include "common/modeldata.h" #include "common/swaglog.h" -#include "system/camerad/cameras/spectra.h" +#include "common/util.h" +#include "system/hardware/hw.h" +#include "msm_media_info.h" + +#include "system/camerad/cameras/camera_qcom2.h" +#ifdef QCOM2 +#include "CL/cl_ext_qcom.h" +#endif + +ExitHandler do_exit; + +class Debayer { +public: + Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { + char args[4096]; + const CameraInfo *ci = &s->ci; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " + "-DIS_OX=%d -DCAM_NUM=%d%s", + ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, + b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, + s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + const char *cl_file = "cameras/real_debayer.cl"; + cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); + krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); + CL_CHECK(clReleaseProgram(prg_debayer)); + } + + void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, cl_event *debayer_event) { + CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl)); + CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl)); + + const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)}; + const int debayer_local_worksize = 16; + const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize}; + CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event)); + } + ~Debayer() { + CL_CHECK(clReleaseKernel(krnl_)); + } + +private: + cl_kernel krnl_; +}; -void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type) { +void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType init_yuv_type) { vipc_server = v; - stream_type = type; - frame_buf_count = frame_cnt; + this->yuv_type = init_yuv_type; - const SensorInfo *sensor = cam->sensor.get(); + const CameraInfo *ci = &s->ci; + camera_state = s; + frame_buf_count = frame_cnt; - // RAW frames from ISP - if (cam->cc.output_type != ISP_IFE_PROCESSED) { - camera_bufs_raw = std::make_unique(frame_buf_count); + // RAW frame + const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; + camera_bufs = std::make_unique(frame_buf_count); + camera_bufs_metadata = std::make_unique(frame_buf_count); - const int raw_frame_size = (sensor->frame_height + sensor->extra_height) * sensor->frame_stride; - for (int i = 0; i < frame_buf_count; i++) { - camera_bufs_raw[i].allocate(raw_frame_size); - camera_bufs_raw[i].init_cl(device_id, context); - } - LOGD("allocated %d CL buffers", frame_buf_count); + for (int i = 0; i < frame_buf_count; i++) { + camera_bufs[i].allocate(frame_size); + camera_bufs[i].init_cl(device_id, context); } + LOGD("allocated %d CL buffers", frame_buf_count); + + rgb_width = ci->frame_width; + rgb_height = ci->frame_height; + + yuv_transform = get_model_yuv_transform(); + + int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width); + int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height); + assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width)); + assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height)); + size_t nv12_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage + size_t nv12_uv_offset = nv12_width * nv12_height; + vipc_server->create_buffers_with_sizes(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset); + LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height); + + debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); - vipc_server->create_buffers_with_sizes(stream_type, VIPC_BUFFER_COUNT, out_img_width, out_img_height, cam->yuv_size, cam->stride, cam->uv_offset); - LOGD("created %d YUV vipc buffers with size %dx%d", VIPC_BUFFER_COUNT, cam->stride, cam->y_height); +#ifdef __APPLE__ + q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); +#else + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); +#endif } CameraBuf::~CameraBuf() { - if (camera_bufs_raw != nullptr) { - for (int i = 0; i < frame_buf_count; i++) { - camera_bufs_raw[i].free(); - } + for (int i = 0; i < frame_buf_count; i++) { + camera_bufs[i].free(); } + if (debayer) delete debayer; + if (q) CL_CHECK(clReleaseCommandQueue(q)); } -void CameraBuf::sendFrameToVipc() { - assert(cur_buf_idx >=0 && cur_buf_idx < frame_buf_count); +bool CameraBuf::acquire() { + if (!safe_queue.try_pop(cur_buf_idx, 50)) return false; - if (camera_bufs_raw) { - cur_camera_buf = &camera_bufs_raw[cur_buf_idx]; + if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) { + LOGE("no frame data? wtf"); + release(); + return false; } - cur_yuv_buf = vipc_server->get_buffer(stream_type, cur_buf_idx); + cur_frame_data = camera_bufs_metadata[cur_buf_idx]; + cur_yuv_buf = vipc_server->get_buffer(yuv_type); + cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl; + cl_event event; + + double start_time = millis_since_boot(); + + cur_camera_buf = &camera_bufs[cur_buf_idx]; + + debayer->queue(q, camrabuf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event); + + clWaitForEvents(1, &event); + CL_CHECK(clReleaseEvent(event)); + + cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0; VisionIpcBufExtra extra = { - cur_frame_data.frame_id, - cur_frame_data.timestamp_sof, - cur_frame_data.timestamp_eof, + cur_frame_data.frame_id, + cur_frame_data.timestamp_sof, + cur_frame_data.timestamp_eof, }; cur_yuv_buf->set_frame_id(cur_frame_data.frame_id); vipc_server->send(cur_yuv_buf, &extra); + + return true; +} + +void CameraBuf::release() { + // Empty +} + +void CameraBuf::queue(size_t buf_idx) { + safe_queue.push(buf_idx); } // common functions +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data) { + framed.setFrameId(frame_data.frame_id); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setTimestampSof(frame_data.timestamp_sof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGain(frame_data.gain); + framed.setHighConversionGain(frame_data.high_conversion_gain); + framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction); + framed.setTargetGreyFraction(frame_data.target_grey_fraction); + framed.setLensPos(frame_data.lens_pos); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setProcessingTime(frame_data.processing_time); +} + kj::Array get_raw_frame_image(const CameraBuf *b) { const uint8_t *dat = (const uint8_t *)b->cur_camera_buf->addr; @@ -69,20 +186,110 @@ kj::Array get_raw_frame_image(const CameraBuf *b) { return kj::mv(frame_image); } -float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip) { +static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) { + int downscale = b->cur_yuv_buf->width / thumbnail_width; + assert(downscale * thumbnail_height == b->cur_yuv_buf->height); + int in_stride = b->cur_yuv_buf->stride; + + // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used. + std::unique_ptr buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]); + uint8_t *y_plane = buf.get(); + uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height; + uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4; + { + // subsampled conversion from nv12 to yuv + for (int hy = 0; hy < thumbnail_height/2; hy++) { + for (int hx = 0; hx < thumbnail_width/2; hx++) { + int ix = hx * downscale + (downscale-1)/2; + int iy = hy * downscale + (downscale-1)/2; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 1]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 1]; + u_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 0]; + v_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 1]; + } + } + } + + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + + uint8_t *thumbnail_buffer = nullptr; + size_t thumbnail_len = 0; + jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + + cinfo.image_width = thumbnail_width; + cinfo.image_height = thumbnail_height; + cinfo.input_components = 3; + + jpeg_set_defaults(&cinfo); + jpeg_set_colorspace(&cinfo, JCS_YCbCr); + // configure sampling factors for yuv420. + cinfo.comp_info[0].h_samp_factor = 2; // Y + cinfo.comp_info[0].v_samp_factor = 2; + cinfo.comp_info[1].h_samp_factor = 1; // U + cinfo.comp_info[1].v_samp_factor = 1; + cinfo.comp_info[2].h_samp_factor = 1; // V + cinfo.comp_info[2].v_samp_factor = 1; + cinfo.raw_data_in = TRUE; + + jpeg_set_quality(&cinfo, 50, TRUE); + jpeg_start_compress(&cinfo, TRUE); + + JSAMPROW y[16], u[8], v[8]; + JSAMPARRAY planes[3]{y, u, v}; + + for (int line = 0; line < cinfo.image_height; line += 16) { + for (int i = 0; i < 16; ++i) { + y[i] = y_plane + (line + i) * cinfo.image_width; + if (i % 2 == 0) { + int offset = (cinfo.image_width / 2) * ((i + line) / 2); + u[i / 2] = u_plane + offset; + v[i / 2] = v_plane + offset; + } + } + jpeg_write_raw_data(&cinfo, planes, 16); + } + + jpeg_finish_compress(&cinfo); + jpeg_destroy_compress(&cinfo); + + kj::Array dat = kj::heapArray(thumbnail_buffer, thumbnail_len); + free(thumbnail_buffer); + return dat; +} + +static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) { + auto thumbnail = yuv420_to_jpeg(b, b->rgb_width / 4, b->rgb_height / 4); + if (thumbnail.size() == 0) return; + + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initThumbnail(); + thumbnaild.setFrameId(b->cur_frame_data.frame_id); + thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof); + thumbnaild.setThumbnail(thumbnail); + + pm->send("thumbnail", msg); +} + +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { int lum_med; uint32_t lum_binning[256] = {0}; const uint8_t *pix_ptr = b->cur_yuv_buf->y; unsigned int lum_total = 0; - for (int y = ae_xywh.y; y < ae_xywh.y + ae_xywh.h; y += y_skip) { - for (int x = ae_xywh.x; x < ae_xywh.x + ae_xywh.w; x += x_skip) { - uint8_t lum = pix_ptr[(y * b->out_img_width) + x]; + for (int y = y_start; y < y_end; y += y_skip) { + for (int x = x_start; x < x_end; x += x_skip) { + uint8_t lum = pix_ptr[(y * b->rgb_width) + x]; lum_binning[lum]++; lum_total += 1; } } + // Find mean lumimance value unsigned int lum_cur = 0; for (lum_med = 255; lum_med >= 0; lum_med--) { @@ -96,6 +303,59 @@ float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int return lum_med / 256.0; } +void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + const char *thread_name = nullptr; + if (cs == &cameras->road_cam) { + thread_name = "RoadCamera"; + } else if (cs == &cameras->driver_cam) { + thread_name = "DriverCamera"; + } else { + thread_name = "WideRoadCamera"; + } + util::set_thread_name(thread_name); + + uint32_t cnt = 0; + while (!do_exit) { + if (!cs->buf.acquire()) continue; + + callback(cameras, cs, cnt); + + if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) { + // this takes 10ms??? + publish_thumbnail(cameras->pm, &(cs->buf)); + } + cs->buf.release(); + ++cnt; + } + return NULL; +} + +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + return std::thread(processing_thread, cameras, cs, callback); +} + +void camerad_thread() { + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); +#ifdef QCOM2 + const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; + cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); +#else + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); +#endif + + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); + + cameras_open(&cameras); + cameras_init(&vipc_server, &cameras, device_id, context); + + vipc_server.start_listener(); + + cameras_run(&cameras); + + CL_CHECK(clReleaseContext(context)); +} + int open_v4l_by_name_and_index(const char name[], int index, int flags) { for (int v4l_index = 0; /**/; ++v4l_index) { std::string v4l_name = util::read_file(util::string_format("/sys/class/video4linux/v4l-subdev%d/name", v4l_index)); diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index c26859cbc40a36..7bbb13c75fc024 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -1,46 +1,128 @@ #pragma once +#include +#include #include +#include #include "cereal/messaging/messaging.h" -#include "msgq/visionipc/visionipc_server.h" -#include "common/util.h" +#include "cereal/visionipc/visionbuf.h" +#include "cereal/visionipc/visionipc.h" +#include "cereal/visionipc/visionipc_server.h" +#include "common/mat.h" +#include "common/queue.h" +#include "common/swaglog.h" +#include "system/hardware/hw.h" +#define CAMERA_ID_IMX298 0 +#define CAMERA_ID_IMX179 1 +#define CAMERA_ID_S5K3P8SP 2 +#define CAMERA_ID_OV8865 3 +#define CAMERA_ID_IMX298_FLIPPED 4 +#define CAMERA_ID_OV10640 5 +#define CAMERA_ID_LGC920 6 +#define CAMERA_ID_LGC615 7 +#define CAMERA_ID_AR0231 8 +#define CAMERA_ID_OX03C10 9 +#define CAMERA_ID_MAX 10 -const int VIPC_BUFFER_COUNT = 18; +const int YUV_BUFFER_COUNT = 40; + +enum CameraType { + RoadCam = 0, + DriverCam, + WideRoadCam +}; + +// for debugging +const bool env_disable_road = getenv("DISABLE_ROAD") != NULL; +const bool env_disable_wide_road = getenv("DISABLE_WIDE_ROAD") != NULL; +const bool env_disable_driver = getenv("DISABLE_DRIVER") != NULL; +const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; +const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; +const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL; + +typedef struct CameraInfo { + uint32_t frame_width, frame_height; + uint32_t frame_stride; + uint32_t frame_offset = 0; + uint32_t extra_height = 0; + int registers_offset = -1; + int stats_offset = -1; +} CameraInfo; typedef struct FrameMetadata { uint32_t frame_id; - uint32_t request_id; - uint64_t timestamp_sof; + unsigned int frame_length; + + // Timestamps + uint64_t timestamp_sof; // only set on tici uint64_t timestamp_eof; + + // Exposure + unsigned int integ_lines; + bool high_conversion_gain; + float gain; + float measured_grey_fraction; + float target_grey_fraction; + + // Focus + unsigned int lens_pos; + float lens_err; + float lens_true_pos; + float processing_time; } FrameMetadata; -class SpectraCamera; +struct MultiCameraState; +struct CameraState; +class Debayer; class CameraBuf { private: - int frame_buf_count; - -public: VisionIpcServer *vipc_server; - VisionStreamType stream_type; + CameraState *camera_state; + Debayer *debayer = nullptr; + + VisionStreamType yuv_type; int cur_buf_idx; + + SafeQueue safe_queue; + + int frame_buf_count; + +public: + cl_command_queue q; FrameMetadata cur_frame_data; VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; - std::unique_ptr camera_bufs_raw; - uint32_t out_img_width, out_img_height; + std::unique_ptr camera_bufs; + std::unique_ptr camera_bufs_metadata; + int rgb_width, rgb_height, rgb_stride; + + mat3 yuv_transform; CameraBuf() = default; ~CameraBuf(); - void init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type); - void sendFrameToVipc(); + void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType yuv_type); + bool acquire(); + void release(); + void queue(size_t buf_idx); }; -void camerad_thread(); +typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); + +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data); kj::Array get_raw_frame_image(const CameraBuf *b); -float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip); +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_open(MultiCameraState *s); +void cameras_run(MultiCameraState *s); +void cameras_close(MultiCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); +void camerad_thread(); + int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index d741e13cf3b41e..b2432bdd724ed4 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -1,132 +1,1026 @@ -#include "system/camerad/cameras/camera_common.h" -#include "system/camerad/cameras/spectra.h" +#include "system/camerad/cameras/camera_qcom2.h" +#include #include #include +#include +#include -#include +#include #include #include #include +#include #include -#include -#include - -#ifdef __TICI__ -#include "CL/cl_ext_qcom.h" -#else -#define CL_PRIORITY_HINT_HIGH_QCOM NULL -#define CL_CONTEXT_PRIORITY_HINT_QCOM NULL -#endif +#include "media/cam_defs.h" +#include "media/cam_isp.h" +#include "media/cam_isp_ife.h" +#include "media/cam_sensor.h" #include "media/cam_sensor_cmn_header.h" - -#include "common/clutil.h" -#include "common/params.h" +#include "media/cam_sync.h" #include "common/swaglog.h" +#include "system/camerad/cameras/sensor2_i2c.h" + +// For debugging: +// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl + +extern ExitHandler do_exit; + +const size_t FRAME_WIDTH = 1928; +const size_t FRAME_HEIGHT = 1208; +const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) + +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 int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_AR0231] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, + .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + .registers_offset = 0, + .frame_offset = AR0231_REGISTERS_HEIGHT, + .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + }, + [CAMERA_ID_OX03C10] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, // (0xa80*12//8) + .extra_height = 16, // this right? + }, +}; + +const float DC_GAIN_AR0231 = 2.5; +const float DC_GAIN_OX03C10 = 7.32; + +const float DC_GAIN_ON_GREY_AR0231= 0.2; +const float DC_GAIN_OFF_GREY_AR0231 = 0.3; +const float DC_GAIN_ON_GREY_OX03C10= 0.3; +const float DC_GAIN_OFF_GREY_OX03C10 = 0.375; + +const int DC_GAIN_MIN_WEIGHT = 0; +const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 32; + +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 + +// similar gain curve to AR +const float sensor_analog_gains_OX03C10[] = { + 1.0, 1.25, 1.3125, 1.5625, + 1.6875, 2.0, 2.25, 2.625, + 3.125, 3.625, 4.5, 5.0, + 7.25, 8.5, 12.0, 15.5}; +const uint32_t ox03c10_analog_gains_reg[] = { + 0x100, 0x140, 0x150, 0x190, + 0x1B0, 0x200, 0x240, 0x2A0, + 0x320, 0x3A0, 0x480, 0x500, + 0x740, 0x880, 0xC00, 0xF80}; -ExitHandler do_exit; +const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x +const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x +const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x -// for debugging -const bool env_debug_frames = getenv("DEBUG_FRAMES") != nullptr; -const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != nullptr; -const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != nullptr; +const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; +const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x5; // 2x +const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0xF; +const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss +const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms -class CameraState { -public: - SpectraCamera camera; - int exposure_time = 5; - bool dc_gain_enabled = false; - int dc_gain_weight = 0; - int gain_idx = 0; - float analog_gain_frac = 0; +const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x +const int EXPOSURE_TIME_MAX_OX03C10 = 2016; +const uint32_t VS_TIME_MIN_OX03C10 = 1; +const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 - float cur_ev[3] = {}; - float best_ev_score = 0; - int new_exp_g = 0; - int new_exp_t = 0; +int CameraState::clear_req_queue() { + struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; + req_mgr_flush_request.session_hdl = session_handle; + req_mgr_flush_request.link_hdl = link_handle; + req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL; + int ret; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); + // LOGD("flushed all req: %d", ret); + return ret; +} - Rect ae_xywh = {}; - float measured_grey_fraction = 0; - float target_grey_fraction = 0.125; +// ************** high level camera helpers **************** - float fl_pix = 0; - std::unique_ptr pm; +void CameraState::sensors_start() { + if (!enabled) return; + LOGD("starting sensor %d", camera_num); + if (camera_id == CAMERA_ID_AR0231) { + sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } else { + assert(false); + } +} - CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config) {}; - ~CameraState(); - void init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx); - void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); - void set_camera_exposure(float grey_frac); - void set_exposure_rect(); - void sendState(); +void CameraState::sensors_poke(int request_id) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 0; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = CAM_SENSOR_PACKET_OPCODE_SENSOR_NOP; + pkt->header.request_id = request_id; - float get_gain_factor() const { - return (1 + dc_gain_weight * (camera.sensor->dc_gain_factor-1) / camera.sensor->dc_gain_max_weight); + int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); + if (ret != 0) { + LOGE("** sensor %d FAILED poke, disabling", camera_num); + enabled = false; + return; } + + mm.free(pkt); +} + +void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { + // LOGD("sensors_i2c: %d", len); + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = op_code; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); + buf_desc[0].type = CAM_CMD_BUF_I2C; + + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + i2c_random_wr->header.count = len; + i2c_random_wr->header.op_code = 1; + i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; + i2c_random_wr->header.data_type = data_word ? CAMERA_SENSOR_I2C_TYPE_WORD : CAMERA_SENSOR_I2C_TYPE_BYTE; + i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload)); + + int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); + if (ret != 0) { + LOGE("** sensor %d FAILED i2c, disabling", camera_num); + enabled = false; + return; + } + + mm.free(i2c_random_wr); + mm.free(pkt); +} + +static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { + cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings))); + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = delay_ms; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + return (struct cam_cmd_power *)(unconditional_wait + 1); }; -void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx) { - camera.camera_open(v, device_id, ctx); +int CameraState::sensors_init() { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = -1; + pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - if (!camera.enabled) return; + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); + buf_desc[0].type = CAM_CMD_BUF_LEGACY; + struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + auto probe = (struct cam_cmd_probe *)(i2c_info + 1); - fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale; - set_exposure_rect(); + probe->camera_id = camera_num; + switch (camera_num) { + case 0: + // port 0 + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2 + break; + case 1: + // port 1 + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20; + break; + case 2: + // port 2 + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; + break; + } + + // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz + //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; + i2c_info->i2c_freq_mode = I2C_FAST_MODE; + i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO; + + probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->op_code = 3; // don't care? + probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; + if (camera_id == CAMERA_ID_AR0231) { + probe->reg_addr = 0x3000; + probe->expected_data = 0x354; + } else if (camera_id == CAMERA_ID_OX03C10) { + probe->reg_addr = 0x300a; + probe->expected_data = 0x5803; + } else { + assert(false); + } + probe->data_mask = 0; + + //buf_desc[1].size = buf_desc[1].length = 148; + buf_desc[1].size = buf_desc[1].length = 196; + buf_desc[1].type = CAM_CMD_BUF_I2C; + struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + memset(power_settings, 0, buf_desc[1].size); + + // power on + struct cam_cmd_power *power = power_settings; + power->count = 4; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 3; // clock?? + power->power_settings[1].power_seq_type = 1; // analog + power->power_settings[2].power_seq_type = 2; // digital + power->power_settings[3].power_seq_type = 8; // reset low + power = power_set_wait(power, 1); + + // set clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz + power = power_set_wait(power, 1); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + // wait 650000 cycles @ 19.2 mhz = 33.8 ms + power = power_set_wait(power, 34); + + // probe happens here + + // disable clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 0; + power = power_set_wait(power, 1); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = power_set_wait(power, 1); + + // reset low + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 0; + power = power_set_wait(power, 1); - dc_gain_weight = camera.sensor->dc_gain_min_weight; - gain_idx = camera.sensor->analog_gain_rec_idx; - cur_ev[0] = cur_ev[1] = cur_ev[2] = get_gain_factor() * camera.sensor->sensor_analog_gains[gain_idx] * exposure_time; + // power off + power->count = 3; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 1; + power->power_settings[2].power_seq_type = 3; - pm = std::make_unique(std::vector{camera.cc.publish_name}); + int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); + LOGD("probing the sensor: %d", ret); + + mm.free(i2c_info); + mm.free(power_settings); + mm.free(pkt); + + return ret; } -CameraState::~CameraState() {} +void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + if (io_mem_handle != 0) { + size += sizeof(struct cam_buf_io_cfg); + } + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = 0; + // YUV has kmd_cmd_buf_offset = 1780 + // I guess this is the ISP command + // YUV also has patch_offset = 0x1030 and num_patches = 10 + + if (io_mem_handle != 0) { + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; + pkt->num_io_configs = 1; + } + + if (io_mem_handle != 0) { + pkt->header.op_code = 0xf000001; + pkt->header.request_id = request_id; + } else { + pkt->header.op_code = 0xf000000; + } + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset); + + // TODO: support MMU + buf_desc[0].size = 65624; + buf_desc[0].length = 0; + buf_desc[0].type = CAM_CMD_BUF_DIRECT; + buf_desc[0].meta_data = 3; + buf_desc[0].mem_handle = buf0_mem_handle; + buf_desc[0].offset = buf0_offset; + + // parsed by cam_isp_packet_generic_blob_handler + struct isp_packet { + uint32_t type_0; + cam_isp_resource_hfr_config resource_hfr; + + uint32_t type_1; + cam_isp_clock_config clock; + uint64_t extra_rdi_hz[3]; -void CameraState::set_exposure_rect() { - // set areas for each camera, shouldn't be changed - std::vector> ae_targets = { - // (Rect, F) - std::make_pair((Rect){96, 400, 1734, 524}, 567.0), // wide - std::make_pair((Rect){96, 160, 1734, 986}, 2648.0), // road - std::make_pair((Rect){96, 242, 1736, 906}, 567.0) // driver + uint32_t type_2; + cam_isp_bw_config bw; + struct cam_isp_bw_vote extra_rdi_vote[6]; + } __attribute__((packed)) tmp; + memset(&tmp, 0, sizeof(tmp)); + + tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG; + tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8; + static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20); + tmp.resource_hfr = { + .num_ports = 1, // 10 for YUV (but I don't think we need them) + .port_hfr_config[0] = { + .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV + .subsample_pattern = 1, + .subsample_period = 0, + .framedrop_pattern = 1, + .framedrop_period = 0, + }}; + + tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG; + tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8; + static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38); + tmp.clock = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_hz = 404000000, + .right_pix_hz = 404000000, + .rdi_hz[0] = 404000000, }; - int h_ref = 1208; - /* - exposure target intrinsics is - [ - [F, 0, 0.5*ae_xywh[2]] - [0, F, 0.5*H-ae_xywh[1]] - [0, 0, 1] - ] - */ - auto ae_target = ae_targets[camera.cc.camera_num]; - Rect xywh_ref = ae_target.first; - float fl_ref = ae_target.second; - - ae_xywh = (Rect){ - std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), - std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) + + + tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG; + tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8; + static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0); + tmp.bw = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_vote = { + .resource_id = 0, + .cam_bw_bps = 450000000, + .ext_bw_bps = 450000000, + }, + .rdi_vote[0] = { + .resource_id = 0, + .cam_bw_bps = 8706200000, + .ext_bw_bps = 8706200000, + }, }; + + static_assert(offsetof(struct isp_packet, type_2) == 0x60); + + buf_desc[1].size = sizeof(tmp); + buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0; + buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; + buf_desc[1].type = CAM_CMD_BUF_GENERIC; + buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; + uint32_t *buf2 = (uint32_t *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + memcpy(buf2, &tmp, sizeof(tmp)); + + if (io_mem_handle != 0) { + io_cfg[0].mem_handle[0] = io_mem_handle; + io_cfg[0].planes[0] = (struct cam_plane_cfg){ + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, + .plane_stride = ci.frame_stride, + .slice_height = ci.frame_height + ci.extra_height, + .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) + .meta_size = 0x0, + .meta_offset = 0x0, + .packer_config = 0x0, // 0xb for YUV + .mode_config = 0x0, // 0x9ef for YUV + .tile_config = 0x0, + .h_init = 0x0, + .v_init = 0x0, + }; + io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12; // CAM_FORMAT_UBWC_TP10 for YUV + io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV + io_cfg[0].color_pattern = 0x5; // 0x0 for YUV + io_cfg[0].bpp = 0xc; + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV + io_cfg[0].fence = fence; + io_cfg[0].direction = CAM_BUF_OUTPUT; + io_cfg[0].subsample_pattern = 0x1; + io_cfg[0].framedrop_pattern = 0x1; + } + + int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle); + assert(ret == 0); + if (ret != 0) { + LOGE("isp config failed"); + } + + mm.free(buf2); + mm.free(pkt); } -void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { - float score = camera.sensor->getExposureScore(desired_ev, exp_t, exp_g_idx, exp_gain, gain_idx); - if (score < best_ev_score) { - new_exp_t = exp_t; - new_exp_g = exp_g_idx; - best_ev_score = score; +void CameraState::enqueue_buffer(int i, bool dp) { + int ret; + int request_id = request_ids[i]; + + if (buf_handle[i] && sync_objs[i]) { + // wait + struct cam_sync_wait sync_wait = {0}; + sync_wait.sync_obj = sync_objs[i]; + sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + if (ret != 0) { + LOGE("failed to wait for sync: %d %d", ret, sync_wait.sync_obj); + // TODO: handle frame drop cleanly + } + + buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof + if (dp) buf.queue(i); + + // destroy old output fence + struct cam_sync_info sync_destroy = {0}; + sync_destroy.sync_obj = sync_objs[i]; + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + if (ret != 0) { + LOGE("failed to destroy sync object: %d %d", ret, sync_destroy.sync_obj); + } + } + + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + if (ret != 0) { + LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj) } + sync_objs[i] = sync_create.sync_obj; + + // schedule request with camera request manager + struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; + req_mgr_sched_request.session_hdl = session_handle; + req_mgr_sched_request.link_hdl = link_handle; + req_mgr_sched_request.req_id = request_id; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); + if (ret != 0) { + LOGE("failed to schedule cam mgr request: %d %d", ret, request_id); + } + + // poke sensor, must happen after schedule + sensors_poke(request_id); + + // submit request to the ife + config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1)); } -void CameraState::set_camera_exposure(float grey_frac) { - if (!camera.enabled) return; - std::vector target_grey_minimums = {0.1, 0.1, 0.125}; // wide, road, driver +void CameraState::enqueue_req_multi(int start, int n, bool dp) { + for (int i=start;idevice_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + int ret = do_cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + enqueue_req_multi(1, FRAME_BUF_COUNT, 0); +} + +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { + if (!enabled) return; + camera_id = camera_id_; + + LOGD("camera init %d", camera_num); + assert(camera_id < std::size(cameras_supported)); + ci = cameras_supported[camera_id]; + assert(ci.frame_width != 0); + + request_id_last = 0; + skipped = true; + + camera_set_parameters(); + + buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); + camera_map_bufs(s); +} + +void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num_, bool enabled_) { + multi_cam_state = multi_cam_state_; + camera_num = camera_num_; + enabled = enabled_; + if (!enabled) return; + + int ret; + sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num); + assert(sensor_fd >= 0); + LOGD("opened sensor for %d", camera_num); + + // init memorymanager for this camera + mm.init(multi_cam_state->video0_fd); + + // probe the sensor + LOGD("-- Probing sensor %d", camera_num); + camera_id = CAMERA_ID_AR0231; + ret = sensors_init(); + if (ret != 0) { + // TODO: use build flag instead? + LOGD("AR0231 init failed, trying OX03C10"); + camera_id = CAMERA_ID_OX03C10; + ret = sensors_init(); + } + LOGD("-- Probing sensor %d done with %d", camera_num, ret); + if (ret != 0) { + LOGE("** sensor %d FAILED bringup, disabling", camera_num); + enabled = false; + return; + } + + // create session + struct cam_req_mgr_session_info session_info = {}; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); + LOGD("get session: %d 0x%X", ret, session_info.session_hdl); + session_handle = session_info.session_hdl; + + // access the sensor + LOGD("-- Accessing sensor"); + auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr); + assert(sensor_dev_handle_); + sensor_dev_handle = *sensor_dev_handle_; + LOGD("acquire sensor dev"); + + LOG("-- Configuring sensor"); + uint32_t dt; + if (camera_id == CAMERA_ID_AR0231) { + sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + // one is 0x2a, two are 0x2b + dt = 0x2c; + } else { + assert(false); + } + printf("dt is %x\n", dt); + + // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c + // If you don't do this, the strobe GPIO is an output (even in reset it seems!) + if (!enabled) return; + + struct cam_isp_in_port_info in_port_info = { + .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num], + + .lane_type = CAM_ISP_LANE_TYPE_DPHY, + .lane_num = 4, + .lane_cfg = 0x3210, + + .vc = 0x0, + .dt = dt, + .format = CAM_FORMAT_MIPI_RAW_12, + + .test_pattern = 0x2, // 0x3? + .usage_type = 0x0, + + .left_start = 0, + .left_stop = ci.frame_width - 1, + .left_width = ci.frame_width, + + .right_start = 0, + .right_stop = ci.frame_width - 1, + .right_width = ci.frame_width, + .line_start = 0, + .line_stop = ci.frame_height + ci.extra_height - 1, + .height = ci.frame_height + ci.extra_height, + + .pixel_clk = 0x0, + .batch_size = 0x0, + .dsp_mode = CAM_ISP_DSP_MODE_NONE, + .hbi_cnt = 0x0, + .custom_csid = 0x0, + + .num_out_res = 0x1, + .data[0] = (struct cam_isp_out_port_info){ + .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, + .format = CAM_FORMAT_MIPI_RAW_12, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, + .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, + }, + }; + struct cam_isp_resource isp_resource = { + .resource_id = CAM_ISP_RES_ID_PORT, + .handle_type = CAM_HANDLE_USER_POINTER, + .res_hdl = (uint64_t)&in_port_info, + .length = sizeof(in_port_info), + }; + + auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource); + assert(isp_dev_handle_); + isp_dev_handle = *isp_dev_handle_; + LOGD("acquire isp dev"); + + csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num); + assert(csiphy_fd >= 0); + LOGD("opened csiphy for %d", camera_num); + + struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0}; + auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info); + assert(csiphy_dev_handle_); + csiphy_dev_handle = *csiphy_dev_handle_; + LOGD("acquire csiphy dev"); + + // config ISP + alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu); + config_isp(0, 0, 1, buf0_handle, 0); + + // config csiphy + LOG("-- Config CSI PHY"); + { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); + buf_desc[0].type = CAM_CMD_BUF_GENERIC; + + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + csiphy_info->lane_mask = 0x1f; + csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? + csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane + csiphy_info->combo_mode = 0x0; + csiphy_info->lane_cnt = 0x4; + csiphy_info->secure_mode = 0x0; + csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL; + csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py + + int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); + assert(ret_ == 0); + + mm.free(csiphy_info); + mm.free(pkt); + } + + // link devices + LOG("-- Link devices"); + struct cam_req_mgr_link_info req_mgr_link_info = {0}; + req_mgr_link_info.session_hdl = session_handle; + req_mgr_link_info.num_devices = 2; + req_mgr_link_info.dev_hdls[0] = isp_dev_handle; + req_mgr_link_info.dev_hdls[1] = sensor_dev_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); + link_handle = req_mgr_link_info.link_hdl; + LOGD("link: %d session: 0x%X isp: 0x%X sensors: 0x%X link: 0x%X", ret, session_handle, isp_dev_handle, sensor_dev_handle, link_handle); + + struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE; + req_mgr_link_control.session_hdl = session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control: %d", ret); + + ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle); + LOGD("start csiphy: %d", ret); + ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); + LOGD("start isp: %d", ret); + + // TODO: this is unneeded, should we be doing the start i2c in a different way? + //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); + //LOGD("start sensor: %d", ret); +} + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { + s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); + + s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); +} + +void cameras_open(MultiCameraState *s) { + int ret; + + LOG("-- Opening devices"); + // video0 is req_mgr, the target of many ioctls + s->video0_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->video0_fd >= 0); + LOGD("opened video0"); + + // video1 is cam_sync, the target of some ioctls + s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->cam_sync_fd >= 0); + LOGD("opened video1 (cam_sync)"); + + // looks like there's only one of these + s->isp_fd = open_v4l_by_name_and_index("cam-isp"); + assert(s->isp_fd >= 0); + LOGD("opened isp"); + + // query icp for MMU handles + LOG("-- Query ICP for MMU handles"); + static struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; + static struct cam_query_cap_cmd query_cap_cmd = {0}; + query_cap_cmd.handle_type = 1; + query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; + query_cap_cmd.size = sizeof(isp_query_cap_cmd); + ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); + assert(ret == 0); + LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); + LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); + s->device_iommu = isp_query_cap_cmd.device_iommu.non_secure; + s->cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; + + // subscribe + LOG("-- Subscribing"); + static struct v4l2_event_subscription sub = {0}; + sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; + sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; + ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); + LOGD("req mgr subscribe: %d", ret); + + s->driver_cam.camera_open(s, 2, !env_disable_driver); + LOGD("driver camera opened"); + s->road_cam.camera_open(s, 1, !env_disable_road); + LOGD("road camera opened"); + s->wide_road_cam.camera_open(s, 0, !env_disable_wide_road); + LOGD("wide road camera opened"); +} + +void CameraState::camera_close() { + int ret; + + // stop devices + LOG("-- Stop devices %d", camera_num); + + if (enabled) { + // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle); + // LOGD("stop sensor: %d", ret); + ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle); + LOGD("stop isp: %d", ret); + ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle); + LOGD("stop csiphy: %d", ret); + // link control stop + LOG("-- Stop link control"); + static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE; + req_mgr_link_control.session_hdl = session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control stop: %d", ret); + + // unlink + LOG("-- Unlink"); + static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; + req_mgr_unlink_info.session_hdl = session_handle; + req_mgr_unlink_info.link_hdl = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); + LOGD("unlink: %d", ret); + + // release devices + LOGD("-- Release devices"); + ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle); + LOGD("release isp: %d", ret); + ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); + LOGD("release csiphy: %d", ret); + + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + release(multi_cam_state->video0_fd, buf_handle[i]); + } + LOGD("released buffers"); + } + + ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); + LOGD("release sensor: %d", ret); + + // destroyed session + struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle}; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info)); + LOGD("destroyed session %d: %d", camera_num, ret); +} + +void cameras_close(MultiCameraState *s) { + s->driver_cam.camera_close(); + s->road_cam.camera_close(); + s->wide_road_cam.camera_close(); + + delete s->pm; +} + +std::map> CameraState::ar0231_build_register_lut(uint8_t *data) { + // This function builds a lookup table from register address, to a pair of indices in the + // buffer where to read this address. The buffer contains padding bytes, + // as well as markers to indicate the type of the next byte. + // + // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. + // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional + // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. + + int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; + auto get_next_idx = [](int cur_idx) { + return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding + }; + + std::map> registers; + for (int register_row = 0; register_row < 2; register_row++) { + uint8_t *registers_raw = data + ci.frame_stride * register_row; + assert(registers_raw[0] == 0x0a); // Start of line + + int value_tag_count = 0; + int first_val_idx = 0; + uint16_t cur_addr = 0; + + for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { + int val_idx = get_next_idx(i); + + uint8_t tag = registers_raw[i]; + uint16_t val = registers_raw[val_idx]; + + if (tag == 0xAA) { // Register MSB tag + cur_addr = val << 8; + } else if (tag == 0xA5) { // Register LSB tag + cur_addr |= val; + cur_addr -= 2; // Next value tag will increment address again + } else if (tag == 0x5A) { // Value tag + + // First tag + if (value_tag_count % 2 == 0) { + cur_addr += 2; + first_val_idx = val_idx; + } else { + registers[cur_addr] = std::make_pair(first_val_idx + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +std::map CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list addrs) { + if (ar0231_register_lut.empty()) { + ar0231_register_lut = ar0231_build_register_lut(data); + } + + std::map registers; + for (uint16_t addr : addrs) { + auto offset = ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + return registers; +} + +void CameraState::handle_camera_event(void *evdat) { + if (!enabled) return; + struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; + assert(event_data->session_hdl == session_handle); + assert(event_data->u.frame_msg.link_hdl == link_handle); + + uint64_t timestamp = event_data->u.frame_msg.timestamp; + int main_id = event_data->u.frame_msg.frame_id; + int real_id = event_data->u.frame_msg.request_id; + + if (real_id != 0) { // next ready + if (real_id == 1) {idx_offset = main_id;} + int buf_idx = (real_id - 1) % FRAME_BUF_COUNT; + + // check for skipped frames + if (main_id > frame_id_last + 1 && !skipped) { + LOGE("camera %d realign", camera_num); + clear_req_queue(); + enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0); + skipped = true; + } else if (main_id == frame_id_last + 1) { + skipped = false; + } + + // check for dropped requests + if (real_id > request_id_last + 1) { + LOGE("camera %d dropped requests %d %d", camera_num, real_id, request_id_last); + enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0); + } + + // metas + frame_id_last = main_id; + request_id_last = real_id; + + auto &meta_data = buf.camera_bufs_metadata[buf_idx]; + meta_data.frame_id = main_id - idx_offset; + meta_data.timestamp_sof = timestamp; + exp_lock.lock(); + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + meta_data.high_conversion_gain = dc_gain_enabled; + meta_data.integ_lines = exposure_time; + meta_data.measured_grey_fraction = measured_grey_fraction; + meta_data.target_grey_fraction = target_grey_fraction; + exp_lock.unlock(); + + // dispatch + enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1); + } else { // not ready + if (main_id > frame_id_last + 10) { + LOGE("camera %d reset after half second of no response", camera_num); + clear_req_queue(); + enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0); + frame_id_last = main_id; + skipped = true; + } + } +} + +void CameraState::set_camera_exposure(float grey_frac) { + if (!enabled) return; const float dt = 0.05; const float ts_grey = 10.0; @@ -140,41 +1034,38 @@ void CameraState::set_camera_exposure(float grey_frac) { // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action. // TODO: Lower latency to 2 frames, by using the histogram outputted by the sensor we can do AE before the debayering is complete - const auto &sensor = camera.sensor; - // Offset idx by one to not get stuck in self loop - const float cur_ev_ = cur_ev[(camera.buf.cur_frame_data.frame_id - 1) % 3] * sensor->ev_scale; + const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; - // Scale target grey between min and 0.4 depending on lighting conditions - float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + sensor->target_grey_factor*cur_ev_) / log2(6000.0), target_grey_minimums[camera.cc.camera_num], 0.4); + // Scale target grey between 0.1 and 0.4 depending on lighting conditions + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4); float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; - float desired_ev = std::clamp(cur_ev_ / sensor->ev_scale * target_grey / grey_frac, sensor->min_ev, sensor->max_ev); + float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev); float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); - best_ev_score = 1e6; - new_exp_g = 0; - new_exp_t = 0; + float best_ev_score = 1e6; + int new_g = 0; + int new_t = 0; // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < sensor->dc_gain_on_grey) { + if (!enable_dc_gain && target_grey < dc_gain_on_grey) { enable_dc_gain = true; - dc_gain_weight = sensor->dc_gain_min_weight; - } else if (enable_dc_gain && target_grey > sensor->dc_gain_off_grey) { + dc_gain_weight = DC_GAIN_MIN_WEIGHT; + } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { enable_dc_gain = false; - dc_gain_weight = sensor->dc_gain_max_weight; + dc_gain_weight = dc_gain_max_weight; } - if (enable_dc_gain && dc_gain_weight < sensor->dc_gain_max_weight) {dc_gain_weight += 1;} - if (!enable_dc_gain && dc_gain_weight > sensor->dc_gain_min_weight) {dc_gain_weight -= 1;} + if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > DC_GAIN_MIN_WEIGHT) {dc_gain_weight -= 1;} std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { - static Params params; - gain_bytes = params.get("CameraDebugExpGain"); - time_bytes = params.get("CameraDebugExpTime"); + gain_bytes = Params().get("CameraDebugExpGain"); + time_bytes = Params().get("CameraDebugExpTime"); } if (gain_bytes.size() > 0 && time_bytes.size() > 0) { @@ -182,109 +1073,196 @@ void CameraState::set_camera_exposure(float grey_frac) { gain_idx = std::stoi(gain_bytes); exposure_time = std::stoi(time_bytes); - new_exp_g = gain_idx; - new_exp_t = exposure_time; + new_g = gain_idx; + new_t = exposure_time; enable_dc_gain = false; } else { - // Simple brute force optimizer to choose sensor parameters to reach desired EV - int min_g = std::max(gain_idx - 1, sensor->analog_gain_min_idx); - int max_g = std::min(gain_idx + 1, sensor->analog_gain_max_idx); - for (int g = min_g; g <= max_g; g++) { - float gain = sensor->sensor_analog_gains[g] * get_gain_factor(); + // Simple brute force optimizer to choose sensor parameters + // to reach desired EV + for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { + float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), sensor->exposure_time_min, sensor->exposure_time_max); + int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); // Only go below recommended gain when absolutely necessary to not overexpose - if (g < sensor->analog_gain_rec_idx && t > 20 && g < gain_idx) { + if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { continue; } - update_exposure_score(desired_ev, t, g, gain); + // Compute error to desired ev + float score = std::abs(desired_ev - (t * gain)) * 10; + + // Going below recommended gain needs lower penalty to not overexpose + float m = g > analog_gain_rec_idx ? 5.0 : 0.1; + score += std::abs(g - (int)analog_gain_rec_idx) * m; + + // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); + + // Small penalty on changing gain + score += std::abs(g - gain_idx) * (score + 1.0) / 10.0; + + if (score < best_ev_score) { + new_t = t; + new_g = g; + best_ev_score = score; + } } } + exp_lock.lock(); + measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = sensor->sensor_analog_gains[new_exp_g]; - gain_idx = new_exp_g; - exposure_time = new_exp_t; + analog_gain_frac = sensor_analog_gains[new_g]; + gain_idx = new_g; + exposure_time = new_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * get_gain_factor(); - cur_ev[camera.buf.cur_frame_data.frame_id % 3] = exposure_time * gain; + float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; + + exp_lock.unlock(); + + // Processing a frame takes right about 50ms, so we need to wait a few ms + // so we don't send i2c commands around the frame start. + int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000; + if (ms < 60) { + util::sleep_for(60 - ms); + } + // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); + + if (camera_id == CAMERA_ID_AR0231) { + uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; + struct i2c_random_wr_payload exp_reg_array[] = { + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; + sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + } else if (camera_id == CAMERA_ID_OX03C10) { + // t_HCG + t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = std::max((dc_gain_weight * exposure_time / dc_gain_max_weight), 0); + uint32_t lcg_time = std::max(((dc_gain_max_weight - dc_gain_weight) * exposure_time / dc_gain_max_weight), 0); + uint32_t spd_time = std::max(hcg_time / 16, (uint32_t)exposure_time_min); + uint32_t vs_time = std::min(std::max(hcg_time / 64, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + + uint32_t real_gain = ox03c10_analog_gains_reg[new_g]; + struct i2c_random_wr_payload exp_reg_array[] = { - // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera.cc.camera_num, 1e-9 * nanos_since_boot(), 1e-9 * camera.buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - camera.buf.cur_frame_data.timestamp_sof)); + {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, + {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, + {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, + {0x35c1, vs_time>>8}, {0x35c2, vs_time&0xFF}, - auto exp_reg_array = sensor->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled); - camera.sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, camera.sensor->data_word); + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + {0x3588, real_gain>>8}, {0x3589, real_gain&0xFF}, + {0x3548, real_gain>>8}, {0x3549, real_gain&0xFF}, + {0x35c8, real_gain>>8}, {0x35c9, real_gain&0xFF}, + }; + sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } } -void CameraState::sendState() { - camera.buf.sendFrameToVipc(); +void camera_autoexposure(CameraState *s, float grey_frac) { + s->set_camera_exposure(grey_frac); +} - MessageBuilder msg; - auto framed = (msg.initEvent().*camera.cc.init_camera_state)(); - const FrameMetadata &meta = camera.buf.cur_frame_data; - framed.setFrameId(meta.frame_id); - framed.setRequestId(meta.request_id); - framed.setTimestampEof(meta.timestamp_eof); - framed.setTimestampSof(meta.timestamp_sof); - framed.setIntegLines(exposure_time); - framed.setGain(analog_gain_frac * get_gain_factor()); - framed.setHighConversionGain(dc_gain_enabled); - framed.setMeasuredGreyFraction(measured_grey_fraction); - framed.setTargetGreyFraction(target_grey_fraction); - framed.setProcessingTime(meta.processing_time); - - const float ev = cur_ev[meta.frame_id % 3]; - const float perc = util::map_val(ev, camera.sensor->min_ev, camera.sensor->max_ev, 0.0f, 100.0f); - framed.setExposureValPercent(perc); - framed.setSensor(camera.sensor->image_sensor); - - // Log raw frames for road camera - if (env_log_raw_frames && camera.cc.stream_type == VISION_STREAM_ROAD && meta.frame_id % 100 == 5) { // no overlap with qlog decimation - framed.setImage(get_raw_frame_image(&camera.buf)); - } - - set_camera_exposure(calculate_exposure_value(&camera.buf, ae_xywh, 2, camera.cc.stream_type != VISION_STREAM_DRIVER ? 2 : 4)); - - // Send the message - pm->send(camera.cc.publish_name, msg); +static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { + // See AR0231 Developer Guide - page 36 + float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); + float t0 = 55.0 - slope * (float)calib2; + return t0 + slope * (float)data_reg; } -void camerad_thread() { - // TODO: centralize enabled handling +static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){ + const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; + uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + + if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0){ + LOGE("unexpected register data found"); + return; + } - cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; - cl_context ctx = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); + auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); - VisionIpcServer v("camerad", device_id, ctx); + uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; + framed.setFrameIdSensor(frame_id); - // *** initial ISP init *** - SpectraMaster m; - m.init(); + float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); + float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); + framed.setTemperaturesC({temp_0, temp_1}); +} - // *** per-cam init *** - std::vector> cams; - for (const auto &config : ALL_CAMERA_CONFIGS) { - auto cam = std::make_unique(&m, config); - cam->init(&v, device_id, ctx); - cams.emplace_back(std::move(cam)); +static void driver_cam_auto_exposure(CameraState *c) { + struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; + const CameraBuf *b = &c->buf; + static ExpRect rect = {96, 1832, 2, 242, 1148, 4}; + camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip)); +} + +static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { + driver_cam_auto_exposure(c); + + MessageBuilder msg; + auto framed = msg.initEvent().initDriverCameraState(); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); + fill_frame_data(framed, c->buf.cur_frame_data); + + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, c, framed); } + s->pm->send("driverCameraState", msg); +} - v.start_listener(); +void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { + const CameraBuf *b = &c->buf; + + MessageBuilder msg; + auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState(); + fill_frame_data(framed, b->cur_frame_data); + if (env_log_raw_frames && c == &s->road_cam && cnt % 100 == 5) { // no overlap with qlog decimation + framed.setImage(get_raw_frame_image(b)); + } + LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); + if (c == &s->road_cam) { + framed.setTransform(b->yuv_transform.v); + LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera"); + } + + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, c, framed); + } + + s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); + + const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); + const int skip = 2; + camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip)); +} + +void cameras_run(MultiCameraState *s) { + LOG("-- Starting threads"); + std::vector threads; + if (s->driver_cam.enabled) threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); + if (s->road_cam.enabled) threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + if (s->wide_road_cam.enabled) threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); // start devices LOG("-- Starting devices"); - for (auto &cam : cams) cam->camera.sensors_start(); + s->driver_cam.sensors_start(); + s->road_cam.sensors_start(); + s->wide_road_cam.sensors_start(); // poll events LOG("-- Dequeueing Video events"); while (!do_exit) { - struct pollfd fds[1] = {{.fd = m.video0_fd, .events = POLLPRI}}; + struct pollfd fds[1] = {{0}}; + + fds[0].fd = s->video0_fd; + fds[0].events = POLLPRI; + int ret = poll(fds, std::size(fds), 1000); if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; @@ -292,32 +1270,37 @@ void camerad_thread() { break; } - if (!(fds[0].revents & POLLPRI)) continue; + if (!fds[0].revents) continue; struct v4l2_event ev = {0}; ret = HANDLE_EINTR(ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev)); if (ret == 0) { if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) { struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data; + // LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); if (env_debug_frames) { - printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, - event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status); - do_exit = do_exit || event_data->u.frame_msg.frame_id > (1*20); + printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status); } - for (auto &cam : cams) { - if (event_data->session_hdl == cam->camera.session_handle) { - if (cam->camera.handle_camera_event(event_data)) { - cam->sendState(); - } - break; - } + if (event_data->session_hdl == s->road_cam.session_handle) { + s->road_cam.handle_camera_event(event_data); + } else if (event_data->session_hdl == s->wide_road_cam.session_handle) { + s->wide_road_cam.handle_camera_event(event_data); + } else if (event_data->session_hdl == s->driver_cam.session_handle) { + s->driver_cam.handle_camera_event(event_data); + } else { + LOGE("Unknown vidioc event source"); + assert(false); } - } else { - LOGE("unhandled event %d\n", ev.type); } } else { LOGE("VIDIOC_DQEVENT failed, errno=%d", errno); } } + + LOG(" ************** STOPPING **************"); + + for (auto &t : threads) t.join(); + + cameras_close(s); } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h new file mode 100644 index 00000000000000..1b792e7e967867 --- /dev/null +++ b/system/camerad/cameras/camera_qcom2.h @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include + +#include + +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_util.h" +#include "common/params.h" +#include "common/util.h" + +#define FRAME_BUF_COUNT 4 + +class CameraState { +public: + MultiCameraState *multi_cam_state; + CameraInfo ci; + bool enabled; + + std::mutex exp_lock; + + int exposure_time; + bool dc_gain_enabled; + int dc_gain_weight; + int gain_idx; + float analog_gain_frac; + + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[16]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + + float cur_ev[3]; + float min_ev, max_ev; + + float measured_grey_fraction; + float target_grey_fraction; + + unique_fd sensor_fd; + unique_fd csiphy_fd; + + int camera_num; + + void handle_camera_event(void *evdat); + void set_camera_exposure(float grey_frac); + + void sensors_start(); + + void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); + void camera_set_parameters(); + void camera_map_bufs(MultiCameraState *s); + void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); + void camera_close(); + + std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); + + int32_t session_handle; + int32_t sensor_dev_handle; + int32_t isp_dev_handle; + int32_t csiphy_dev_handle; + + int32_t link_handle; + + int buf0_handle; + int buf_handle[FRAME_BUF_COUNT]; + int sync_objs[FRAME_BUF_COUNT]; + int request_ids[FRAME_BUF_COUNT]; + int request_id_last; + int frame_id_last; + int idx_offset; + bool skipped; + int camera_id; + + CameraBuf buf; + MemoryManager mm; + +private: + void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); + void enqueue_req_multi(int start, int n, bool dp); + void enqueue_buffer(int i, bool dp); + int clear_req_queue(); + + int sensors_init(); + void sensors_poke(int request_id); + void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + + // Register parsing + std::map> ar0231_register_lut; + std::map> ar0231_build_register_lut(uint8_t *data); +}; + +typedef struct MultiCameraState { + unique_fd video0_fd; + unique_fd cam_sync_fd; + unique_fd isp_fd; + int device_iommu; + int cdm_iommu; + + CameraState road_cam; + CameraState wide_road_cam; + CameraState driver_cam; + + PubMaster *pm; +} MultiCameraState; diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc new file mode 100644 index 00000000000000..6d139590e48845 --- /dev/null +++ b/system/camerad/cameras/camera_util.cc @@ -0,0 +1,135 @@ +#include "system/camerad/cameras/camera_util.h" + +#include + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +// ************** low level camera helpers **************** +int do_cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { + camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); + if (ret == -1) { + LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); + } + return ret; +} + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { + struct cam_acquire_dev_cmd cmd = { + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, + }; + int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); + return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; +}; + +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { + struct cam_config_dev_cmd cmd = { + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, + }; + return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; + return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { + struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; + mem_mgr_alloc_cmd.len = len; + mem_mgr_alloc_cmd.align = align; + mem_mgr_alloc_cmd.flags = flags; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void release(int video0_fd, uint32_t handle) { + int ret; + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} + +void release_fd(int video0_fd, uint32_t handle) { + // handle to fd + close(handle>>16); + release(video0_fd, handle); +} + +void *MemoryManager::alloc(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h new file mode 100644 index 00000000000000..e408f6c0e2c5c4 --- /dev/null +++ b/system/camerad/cameras/camera_util.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include + +#include + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); +int device_control(int fd, int op_code, int session_handle, int dev_handle); +int do_cam_control(int fd, int op_code, void *handle, int size); +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, + int mmu_hdl = 0, int mmu_hdl2 = 0); +void release(int video0_fd, uint32_t handle); + +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + void *alloc(int len, uint32_t *handle); + void free(void *ptr); + ~MemoryManager(); + private: + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; diff --git a/system/camerad/cameras/cdm.cc b/system/camerad/cameras/cdm.cc deleted file mode 100644 index d4ef20c48cd2e1..00000000000000 --- a/system/camerad/cameras/cdm.cc +++ /dev/null @@ -1,47 +0,0 @@ -#include "cdm.h" -#include "stddef.h" - -int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel) { - struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst; - cmd->cmd = CAM_CDM_CMD_DMI_32; - cmd->length = length - 1; - cmd->reserved = 0; - cmd->addr = 0; // gets patched in - cmd->DMIAddr = dmi_addr; - cmd->DMISel = sel; - - *addr = (uint64_t)(dst + offsetof(struct cdm_dmi_cmd, addr)); - return sizeof(struct cdm_dmi_cmd); -} - -int write_cont(uint8_t *dst, uint32_t reg, const std::vector &vals) { - struct cdm_regcontinuous_cmd *cmd = (struct cdm_regcontinuous_cmd*)dst; - cmd->cmd = CAM_CDM_CMD_REG_CONT; - cmd->count = vals.size(); - cmd->offset = reg; - cmd->reserved0 = 0; - cmd->reserved1 = 0; - - uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regcontinuous_cmd)); - for (int i = 0; i < vals.size(); i++) { - *vd = vals[i]; - vd++; - } - - return sizeof(struct cdm_regcontinuous_cmd) + vals.size()*sizeof(uint32_t); -} - -int write_random(uint8_t *dst, const std::vector &vals) { - struct cdm_regrandom_cmd *cmd = (struct cdm_regrandom_cmd*)dst; - cmd->cmd = CAM_CDM_CMD_REG_RANDOM; - cmd->count = vals.size() / 2; - cmd->reserved = 0; - - uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regrandom_cmd)); - for (int i = 0; i < vals.size(); i++) { - *vd = vals[i]; - vd++; - } - - return sizeof(struct cdm_regrandom_cmd) + vals.size()*sizeof(uint32_t); -} diff --git a/system/camerad/cameras/cdm.h b/system/camerad/cameras/cdm.h deleted file mode 100644 index adda6004006ad4..00000000000000 --- a/system/camerad/cameras/cdm.h +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -// our helpers -int write_random(uint8_t *dst, const std::vector &vals); -int write_cont(uint8_t *dst, uint32_t reg, const std::vector &vals); -int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel); - -// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h} - -enum cam_cdm_command { - CAM_CDM_CMD_UNUSED = 0x0, - CAM_CDM_CMD_DMI = 0x1, - CAM_CDM_CMD_NOT_DEFINED = 0x2, - CAM_CDM_CMD_REG_CONT = 0x3, - CAM_CDM_CMD_REG_RANDOM = 0x4, - CAM_CDM_CMD_BUFF_INDIRECT = 0x5, - CAM_CDM_CMD_GEN_IRQ = 0x6, - CAM_CDM_CMD_WAIT_EVENT = 0x7, - CAM_CDM_CMD_CHANGE_BASE = 0x8, - CAM_CDM_CMD_PERF_CTRL = 0x9, - CAM_CDM_CMD_DMI_32 = 0xa, - CAM_CDM_CMD_DMI_64 = 0xb, - CAM_CDM_CMD_PRIVATE_BASE = 0xc, - CAM_CDM_CMD_SWD_DMI_32 = (CAM_CDM_CMD_PRIVATE_BASE + 0x64), - CAM_CDM_CMD_SWD_DMI_64 = (CAM_CDM_CMD_PRIVATE_BASE + 0x65), - CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F -}; - -/** - * struct cdm_regrandom_cmd - Definition for CDM random register command. - * @count: Number of register writes - * @reserved: reserved bits - * @cmd: Command ID (CDMCmd) - */ -struct cdm_regrandom_cmd { - unsigned int count : 16; - unsigned int reserved : 8; - unsigned int cmd : 8; -} __attribute__((__packed__)); - -/** - * struct cdm_regcontinuous_cmd - Definition for a CDM register range command. - * @count: Number of register writes - * @reserved0: reserved bits - * @cmd: Command ID (CDMCmd) - * @offset: Start address of the range of registers - * @reserved1: reserved bits - */ -struct cdm_regcontinuous_cmd { - unsigned int count : 16; - unsigned int reserved0 : 8; - unsigned int cmd : 8; - unsigned int offset : 24; - unsigned int reserved1 : 8; -} __attribute__((__packed__)); - -/** - * struct cdm_dmi_cmd - Definition for a CDM DMI command. - * @length: Number of bytes in LUT - 1 - * @reserved: reserved bits - * @cmd: Command ID (CDMCmd) - * @addr: Address of the LUT in memory - * @DMIAddr: Address of the target DMI config register - * @DMISel: DMI identifier - */ -struct cdm_dmi_cmd { - unsigned int length : 16; - unsigned int reserved : 8; - unsigned int cmd : 8; - unsigned int addr; - unsigned int DMIAddr : 24; - unsigned int DMISel : 8; -} __attribute__((__packed__)); diff --git a/system/camerad/cameras/hw.h b/system/camerad/cameras/hw.h deleted file mode 100644 index f20a1b3adecf93..00000000000000 --- a/system/camerad/cameras/hw.h +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include "common/util.h" -#include "cereal/gen/cpp/log.capnp.h" -#include "msgq/visionipc/visionipc_server.h" - -#include "media/cam_isp_ife.h" - - -typedef enum { - ISP_RAW_OUTPUT, // raw frame from sensor - ISP_IFE_PROCESSED, // fully processed image through the IFE - ISP_BPS_PROCESSED, // fully processed image through the BPS -} SpectraOutputType; - -// For the comma 3X three camera platform - -struct CameraConfig { - int camera_num; - VisionStreamType stream_type; - float focal_len; // millimeters - const char *publish_name; - cereal::FrameData::Builder (cereal::Event::Builder::*init_camera_state)(); - bool enabled; - uint32_t phy; - bool vignetting_correction; - SpectraOutputType output_type; -}; - -// NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c -// If you don't do this, the strobe GPIO is an output (even in reset it seems!) -const CameraConfig WIDE_ROAD_CAMERA_CONFIG = { - .camera_num = 0, - .stream_type = VISION_STREAM_WIDE_ROAD, - .focal_len = 1.71, - .publish_name = "wideRoadCameraState", - .init_camera_state = &cereal::Event::Builder::initWideRoadCameraState, - .enabled = !getenv("DISABLE_WIDE_ROAD"), - .phy = CAM_ISP_IFE_IN_RES_PHY_0, - .vignetting_correction = false, - .output_type = ISP_IFE_PROCESSED, -}; - -const CameraConfig ROAD_CAMERA_CONFIG = { - .camera_num = 1, - .stream_type = VISION_STREAM_ROAD, - .focal_len = 8.0, - .publish_name = "roadCameraState", - .init_camera_state = &cereal::Event::Builder::initRoadCameraState, - .enabled = !getenv("DISABLE_ROAD"), - .phy = CAM_ISP_IFE_IN_RES_PHY_1, - .vignetting_correction = true, - .output_type = ISP_IFE_PROCESSED, -}; - -const CameraConfig DRIVER_CAMERA_CONFIG = { - .camera_num = 2, - .stream_type = VISION_STREAM_DRIVER, - .focal_len = 1.71, - .publish_name = "driverCameraState", - .init_camera_state = &cereal::Event::Builder::initDriverCameraState, - .enabled = !getenv("DISABLE_DRIVER"), - .phy = CAM_ISP_IFE_IN_RES_PHY_2, - .vignetting_correction = false, - .output_type = ISP_BPS_PROCESSED, -}; - -const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG}; diff --git a/system/camerad/cameras/ife.h b/system/camerad/cameras/ife.h deleted file mode 100644 index fd87d2baa4a742..00000000000000 --- a/system/camerad/cameras/ife.h +++ /dev/null @@ -1,236 +0,0 @@ -#pragma once - -#include "cdm.h" - -#include "system/camerad/cameras/hw.h" -#include "system/camerad/sensors/sensor.h" - -int build_common_ife_bps(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches, bool ife) { - uint8_t *start = dst; - - /* - Common between IFE and BPS. - */ - - // IFE -> BPS addresses - /* - std::map addrs = { - {0xf30, 0x3468}, - }; - */ - - // YUV - dst += write_cont(dst, ife ? 0xf30 : 0x3468, { - 0x00680208, - 0x00000108, - 0x00400000, - 0x03ff0000, - 0x01c01ed8, - 0x00001f68, - 0x02000000, - 0x03ff0000, - 0x1fb81e88, - 0x000001c0, - 0x02000000, - 0x03ff0000, - }); - - return dst - start; -} - -int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches) { - uint8_t *start = dst; - - // init sequence - dst += write_random(dst, { - 0x2c, 0xffffffff, - 0x30, 0xffffffff, - 0x34, 0xffffffff, - 0x38, 0xffffffff, - 0x3c, 0xffffffff, - }); - - // demux cfg - dst += write_cont(dst, 0x560, { - 0x00000001, - 0x04440444, - 0x04450445, - 0x04440444, - 0x04450445, - 0x000000ca, - 0x0000009c, - }); - - // white balance - dst += write_cont(dst, 0x6fc, { - 0x00800080, - 0x00000080, - 0x00000000, - 0x00000000, - }); - - // module config/enables (e.g. enable debayer, white balance, etc.) - dst += write_cont(dst, 0x40, { - 0x00000c06 | ((uint32_t)(cam.vignetting_correction) << 8), - }); - dst += write_cont(dst, 0x44, { - 0x00000000, - }); - dst += write_cont(dst, 0x48, { - (1 << 3) | (1 << 1), - }); - dst += write_cont(dst, 0x4c, { - 0x00000019, - }); - dst += write_cont(dst, 0xf00, { - 0x00000000, - }); - - // cropping - dst += write_cont(dst, 0xe0c, { - 0x00000e00, - }); - dst += write_cont(dst, 0xe2c, { - 0x00000e00, - }); - - // black level scale + offset - dst += write_cont(dst, 0x6b0, { - ((uint32_t)(1 << 11) << 0xf) | (s->black_level << (14 - s->bits_per_pixel)), - 0x0, - 0x0, - }); - - return dst - start; -} - - -int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches, uint32_t out_width, uint32_t out_height) { - uint8_t *start = dst; - - // start with the every frame config - dst += build_update(dst, cam, s, patches); - - uint64_t addr; - - // setup - dst += write_cont(dst, 0x478, { - 0x00000004, - 0x004000c0, - }); - dst += write_cont(dst, 0x488, { - 0x00000000, - 0x00000000, - 0x00000f0f, - }); - dst += write_cont(dst, 0x49c, { - 0x00000001, - }); - dst += write_cont(dst, 0xce4, { - 0x00000000, - 0x00000000, - }); - - // linearization - dst += write_cont(dst, 0x4dc, { - 0x00000000, - }); - dst += write_cont(dst, 0x4e0, s->linearization_pts); - dst += write_cont(dst, 0x4f0, s->linearization_pts); - dst += write_cont(dst, 0x500, s->linearization_pts); - dst += write_cont(dst, 0x510, s->linearization_pts); - // TODO: this is DMI64 in the dump, does that matter? - dst += write_dmi(dst, &addr, s->linearization_lut.size()*sizeof(uint32_t), 0xc24, 9); - patches.push_back(addr - (uint64_t)start); - - // vignetting correction - dst += write_cont(dst, 0x6bc, { - 0x0b3c0000, - 0x00670067, - 0xd3b1300c, - 0x13b1300c, - }); - dst += write_cont(dst, 0x6d8, { - 0xec4e4000, - 0x0100c003, - }); - dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 14); // GRR - patches.push_back(addr - (uint64_t)start); - dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 15); // GBB - patches.push_back(addr - (uint64_t)start); - - // debayer - dst += write_cont(dst, 0x6f8, { - 0x00000100, - }); - dst += write_cont(dst, 0x71c, { - 0x00008000, - 0x08000066, - }); - - // color correction - dst += write_cont(dst, 0x760, s->color_correct_matrix); - - // gamma - dst += write_cont(dst, 0x798, { - 0x00000000, - }); - dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 26); // G - patches.push_back(addr - (uint64_t)start); - dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 28); // B - patches.push_back(addr - (uint64_t)start); - dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30); // R - patches.push_back(addr - (uint64_t)start); - - // output size/scaling - dst += write_cont(dst, 0xa3c, { - 0x00000003, - ((out_width - 1) << 16) | (s->frame_width - 1), - 0x30036666, - 0x00000000, - 0x00000000, - s->frame_width - 1, - ((out_height - 1) << 16) | (s->frame_height - 1), - 0x30036666, - 0x00000000, - 0x00000000, - s->frame_height - 1, - }); - dst += write_cont(dst, 0xa68, { - 0x00000003, - ((out_width / 2 - 1) << 16) | (s->frame_width - 1), - 0x3006cccc, - 0x00000000, - 0x00000000, - s->frame_width - 1, - ((out_height / 2 - 1) << 16) | (s->frame_height - 1), - 0x3006cccc, - 0x00000000, - 0x00000000, - s->frame_height - 1, - }); - - // cropping - dst += write_cont(dst, 0xe10, { - out_height - 1, - out_width - 1, - }); - dst += write_cont(dst, 0xe30, { - out_height / 2 - 1, - out_width - 1, - }); - dst += write_cont(dst, 0xe18, { - 0x0ff00000, - 0x00000016, - }); - dst += write_cont(dst, 0xe38, { - 0x0ff00000, - 0x00000017, - }); - - dst += build_common_ife_bps(dst, cam, s, patches, true); - - return dst - start; -} - - diff --git a/system/camerad/cameras/nv12_info.h b/system/camerad/cameras/nv12_info.h deleted file mode 100644 index e8eb1174062286..00000000000000 --- a/system/camerad/cameras/nv12_info.h +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "third_party/linux/include/msm_media_info.h" - -// Returns NV12 aligned (stride, y_height, uv_height, buffer_size) for the given frame dimensions. -inline std::tuple get_nv12_info(int width, int height) { - const uint32_t stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width); - const uint32_t y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height); - const uint32_t uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, height); - const uint32_t size = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height); - - // Sanity checks for NV12 format assumptions - assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, width)); - assert(y_height / 2 == uv_height); - assert((stride * y_height) % 0x1000 == 0); // uv_offset must be page-aligned - - return {stride, y_height, uv_height, size}; -} diff --git a/system/camerad/cameras/nv12_info.py b/system/camerad/cameras/nv12_info.py deleted file mode 100644 index bcb6312d2bcf81..00000000000000 --- a/system/camerad/cameras/nv12_info.py +++ /dev/null @@ -1,21 +0,0 @@ -# Python version of system/camerad/cameras/nv12_info.h -# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE) - -def align(val: int, alignment: int) -> int: - return ((val + alignment - 1) // alignment) * alignment - -def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]: - """Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions.""" - stride = align(width, 128) - y_height = align(height, 32) - uv_height = align(height // 2, 16) - - # VENUS_BUFFER_SIZE for NV12 - y_plane = stride * y_height - uv_plane = stride * uv_height + 4096 - size = y_plane + uv_plane + max(16 * 1024, 8 * stride) - size = align(size, 4096) - size += align(width, 512) * 512 # kernel padding for non-aligned frames - size = align(size, 4096) - - return stride, y_height, uv_height, size diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/cameras/real_debayer.cl new file mode 100644 index 00000000000000..4a36a03bf51218 --- /dev/null +++ b/system/camerad/cameras/real_debayer.cl @@ -0,0 +1,193 @@ +#define UV_WIDTH RGB_WIDTH / 2 +#define UV_HEIGHT RGB_HEIGHT / 2 + +#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) +#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) +#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) +#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) + +float3 color_correct(float3 rgb) { + // color correction + #if IS_OX + float3 x = rgb.x * (float3)(1.81485125, -0.51650643, -0.06985117); + x += rgb.y * (float3)(-0.51681964, 1.85935946, -0.49871889); + x += rgb.z * (float3)(-0.29803161, -0.34285304, 1.56857006); + #else + float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); + x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); + x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); + #endif + + // tone mapping params + 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 + return (x > mp) ? + ((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) : + ((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b); +} + +float get_vignetting_s(float r) { + if (r < 62500) { + return (1.0f + 0.0000008f*r); + } else if (r < 490000) { + return (0.9625f + 0.0000014f*r); + } else if (r < 1102500) { + return (1.26434f + 0.0000000000016f*r*r); + } else { + return (0.53503625f + 0.0000000000022f*r*r); + } +} + +constant float ox03c10_lut[] = { + 0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05, + 1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05, + 3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05, + 6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04, + 1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04, + 1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04, + 2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04, + 4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04, + 9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03, + 1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03, + 3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03, + 7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02, + 1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02, + 6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01, + 1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01, + 3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00 +}; + +float4 val4_from_12(uchar8 pvs, float gain) { + uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit + ((uint)pvs.s2<<4) + (pvs.s4&0xF), + ((uint)pvs.s3<<4) + (pvs.s4>>4), + ((uint)pvs.s5<<4) + (pvs.s7&0xF)); + #if IS_OX + // PWL + //float4 pv = (convert_float4(parsed) - 64.0) / (4096.0 - 64.0); + float4 pv = {ox03c10_lut[parsed.s0], ox03c10_lut[parsed.s1], ox03c10_lut[parsed.s2], ox03c10_lut[parsed.s3]}; + + // it's a 24 bit signal, center in the middle 8 bits + return pv*256.0; + #else // AR + // normalize and scale + float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0); + return clamp(pv*gain, 0.0, 1.0); + #endif + +} + +float get_k(float a, float b, float c, float d) { + return 2.0 - (fabs(a - b) + fabs(c - d)); +} + +__kernel void debayer10(const __global uchar * in, __global uchar * out) +{ + const int gid_x = get_global_id(0); + const int gid_y = get_global_id(1); + + const int y_top_mod = (gid_y == 0) ? 2: 0; + const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3; + + float3 rgb; + uchar3 rgb_out[4]; + + int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); + + // read in 8x4 chars + uchar8 dat[4]; + dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod); + dat[1] = vload8(0, in + start + FRAME_STRIDE*1); + dat[2] = vload8(0, in + start + FRAME_STRIDE*2); + dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod); + + // correct vignetting + #if VIGNETTING + int gx = (gid_x*2 - RGB_WIDTH/2); + int gy = (gid_y*2 - RGB_HEIGHT/2); + const float gain = get_vignetting_s(gx*gx + gy*gy); + #else + const float gain = 1.0; + #endif + + // process them to floats + float4 va = val4_from_12(dat[0], gain); + float4 vb = val4_from_12(dat[1], gain); + float4 vc = val4_from_12(dat[2], gain); + float4 vd = val4_from_12(dat[3], gain); + + if (gid_x == 0) { + va.s0 = va.s2; + vb.s0 = vb.s2; + vc.s0 = vc.s2; + vd.s0 = vd.s2; + } else if (gid_x == RGB_WIDTH/2 - 1) { + va.s3 = va.s1; + vb.s3 = vb.s1; + vc.s3 = vc.s1; + vd.s3 = vd.s1; + } + + // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf + const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1); + const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1); + const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1); + const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1); + rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1 + rgb.y = vb.s1; // G1(R) + rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1 + rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + + const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3); + const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2); + const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3); + const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1); + rgb.x = vb.s2; // R + rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R + rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R + rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + + const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2); + const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1); + const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2); + const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0); + rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B + rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B + rgb.z = vc.s1; // B + rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + + const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2); + const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2); + const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2); + const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2); + rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2 + rgb.y = vc.s2; // G2(B) + rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2 + rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + + // write ys + uchar2 yy = (uchar2)( + RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), + RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) + ); + vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2)); + yy = (uchar2)( + RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), + RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) + ); + vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2)); + + // write uvs + const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); + const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); + const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); + uchar2 uv = (uchar2)( + RGB_TO_U(ar, ag, ab), + RGB_TO_V(ar, ag, ab) + ); + vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2)); +} diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h new file mode 100644 index 00000000000000..9df99552e18207 --- /dev/null +++ b/system/camerad/cameras/sensor2_i2c.h @@ -0,0 +1,867 @@ +struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; +struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; +struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; + +struct i2c_random_wr_payload init_array_ox03c10[] = { + {0x103, 1}, + {0x107, 1}, + + // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200 + + // TPM + {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf}, + + /*) + // group 4 + {0x3208, 0x04}, + {0x4620, 0x04}, + {0x3208, 0x14}, + + // group 5 + {0x3208, 0x05}, + {0x4620, 0x04}, + {0x3208, 0x15}, + + // group 2 + {0x3208, 0x02}, + {0x3507, 0x00}, + {0x3208, 0x12}, + + // delay launch group 2 + {0x3208, 0xa2},*/ + + // PLL setup + {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix + {0x0303, 0x01}, // pll1_prediv + {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300 + {0x0306, 0x04}, // pll1_divmipi = 4 + {0x0307, 0x01}, // pll1_divm = 1 + {0x0316, 0x00}, + {0x0317, 0x00}, + {0x0318, 0x00}, + {0x0323, 0x05}, // pll2_prediv + {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300 + + // SCLK/PCLK + {0x0400, 0xe0}, {0x0401, 0x80}, + {0x0403, 0xde}, {0x0404, 0x34}, + {0x0405, 0x3b}, {0x0406, 0xde}, + {0x0407, 0x08}, + {0x0408, 0xe0}, {0x0409, 0x7f}, + {0x040a, 0xde}, {0x040b, 0x34}, + {0x040c, 0x47}, {0x040d, 0xd8}, + {0x040e, 0x08}, + + // xchk + {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79}, + + // SC ctrl + {0x3001, 0x03}, // io_pad_oen + {0x3002, 0xf8}, // io_pad_oen + {0x3005, 0x80}, // io_pad_out + {0x3007, 0x01}, // io_pad_sel + {0x3008, 0x80}, // io_pad_sel + + // FSIN first frame + /* + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x3822, 0x20}, + {0x3823, 0x58}, + + {0x3826, 0x0}, {0x3827, 0x8}, + {0x3881, 0x4}, + + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + */ + + // FSIN with external pulses + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x383E, 0x80}, + {0x3881, 0x4}, + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + + {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI + {0x3020, 0x05}, // SC_CTRL_20 + + // this is not in the datasheet, listed as RSVD + // but the camera doesn't work without it + {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23}, + {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d}, + {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96}, + {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73}, + {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36}, + {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21}, + {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28}, + {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c}, + {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13}, + {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02}, + {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00}, + {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e}, + {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02}, + {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00}, + {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00}, + {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00}, + {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19}, + + // also RSVD + {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01}, + {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21}, + {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3}, + {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03}, + {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80}, + {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c}, + {0x3c73, 0x2c}, {0x3c76, 0x12}, + + // PEC checks + {0x3182, 0x12}, + + {0x320e, 0x00}, {0x320f, 0x00}, // RSVD + {0x3211, 0x61}, + {0x3215, 0xcd}, + {0x3219, 0x08}, + + {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure + {0x350a, 0x04}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + + {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure + {0x358a, 0x04}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain + + {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure + {0x354a, 0x04}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain + + {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure + {0x35ca, 0x04}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain + + // also RSVD + {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01}, + {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a}, + {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b}, + {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f}, + {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00}, + {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a}, + {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10}, + {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a}, + {0x3679, 0x19}, + + // Y_ADDR_START = 4 + {0x3802, 0x00}, {0x3803, 0x04}, + // Y_ADDR_END = 0x50b + {0x3806, 0x05}, {0x3807, 0x0b}, + + // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928) + {0x3808, 0x07}, {0x3809, 0x88}, + + // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208) + {0x380a, 0x04}, {0x380b, 0xb8}, + + // horizontal timing 0x447 + {0x380c, 0x04}, {0x380d, 0x47}, + + // rows per frame (was 0x2ae) + // 0x8ae = 53.65 ms + {0x380e, 0x08}, {0x380f, 0x15}, + // this should be triggered by FSIN, not free running + + {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff + {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff + {0x3816, 0x01}, + {0x3817, 0x01}, + {0x381c, 0x18}, + {0x381e, 0x01}, + {0x381f, 0x01}, + + // don't mirror, just flip + {0x3820, 0x04}, + + {0x3821, 0x19}, + {0x3832, 0x00}, + {0x3834, 0x00}, + {0x384c, 0x02}, + {0x384d, 0x0d}, + {0x3850, 0x00}, + {0x3851, 0x42}, + {0x3852, 0x00}, + {0x3853, 0x40}, + {0x3858, 0x04}, + {0x388c, 0x02}, + {0x388d, 0x2b}, + + // APC + {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90}, + {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20}, + {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e}, + {0x3b4c, 0x00}, {0x3b4d, 0x00}, + {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08}, + {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80}, + {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00}, + {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09}, + + // OTP + {0x3d82, 0x73}, + {0x3d85, 0x05}, + {0x3d8a, 0x03}, + {0x3d8b, 0xff}, + {0x3d99, 0x00}, + {0x3d9a, 0x9f}, + {0x3d9b, 0x00}, + {0x3d9c, 0xa0}, + {0x3da4, 0x00}, + {0x3da7, 0x50}, + + // DTR + {0x420e, 0x6b}, + {0x420f, 0x6e}, + {0x4210, 0x06}, + {0x4211, 0xc1}, + {0x421e, 0x02}, + {0x421f, 0x45}, + {0x4220, 0xe1}, + {0x4221, 0x01}, + {0x4301, 0xff}, + {0x4307, 0x03}, + {0x4308, 0x13}, + {0x430a, 0x13}, + {0x430d, 0x93}, + {0x430f, 0x57}, + {0x4310, 0x95}, + {0x4311, 0x16}, + {0x4316, 0x00}, + + {0x4317, 0x38}, // both embedded rows are enabled + + {0x4319, 0x03}, // spd dcg + {0x431a, 0x00}, // 8 bit mipi + {0x431b, 0x00}, + {0x431d, 0x2a}, + {0x431e, 0x11}, + + {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits + //{0x431f, 0x00}, // disable PWL + + {0x4320, 0x19}, + {0x4323, 0x80}, + {0x4324, 0x00}, + {0x4503, 0x4e}, + {0x4505, 0x00}, + {0x4509, 0x00}, + {0x450a, 0x00}, + {0x4580, 0xf8}, + {0x4583, 0x07}, + {0x4584, 0x6a}, + {0x4585, 0x08}, + {0x4586, 0x05}, + {0x4587, 0x04}, + {0x4588, 0x73}, + {0x4589, 0x05}, + {0x458a, 0x1f}, + {0x458b, 0x02}, + {0x458c, 0xdc}, + {0x458d, 0x03}, + {0x458e, 0x02}, + {0x4597, 0x07}, + {0x4598, 0x40}, + {0x4599, 0x0e}, + {0x459a, 0x0e}, + {0x459b, 0xfb}, + {0x459c, 0xf3}, + {0x4602, 0x00}, + {0x4603, 0x13}, + {0x4604, 0x00}, + {0x4609, 0x0a}, + {0x460a, 0x30}, + {0x4610, 0x00}, + {0x4611, 0x70}, + {0x4612, 0x01}, + {0x4613, 0x00}, + {0x4614, 0x00}, + {0x4615, 0x70}, + {0x4616, 0x01}, + {0x4617, 0x00}, + + {0x4800, 0x04}, // invert output PCLK + {0x480a, 0x22}, + {0x4813, 0xe4}, + + // mipi + {0x4814, 0x2a}, + {0x4837, 0x0d}, + {0x484b, 0x47}, + {0x484f, 0x00}, + {0x4887, 0x51}, + {0x4d00, 0x4a}, + {0x4d01, 0x18}, + {0x4d05, 0xff}, + {0x4d06, 0x88}, + {0x4d08, 0x63}, + {0x4d09, 0xdf}, + {0x4d15, 0x7d}, + {0x4d1a, 0x20}, + {0x4d30, 0x0a}, + {0x4d31, 0x00}, + {0x4d34, 0x7d}, + {0x4d3c, 0x7d}, + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x00}, + {0x4f03, 0x20}, + {0x4f04, 0xe0}, + {0x6a00, 0x00}, + {0x6a01, 0x20}, + {0x6a02, 0x00}, + {0x6a03, 0x20}, + {0x6a04, 0x02}, + {0x6a05, 0x80}, + {0x6a06, 0x01}, + {0x6a07, 0xe0}, + {0x6a08, 0xcf}, + {0x6a09, 0x01}, + {0x6a0a, 0x40}, + {0x6a20, 0x00}, + {0x6a21, 0x02}, + {0x6a22, 0x00}, + {0x6a23, 0x00}, + {0x6a24, 0x00}, + {0x6a25, 0x00}, + {0x6a26, 0x00}, + {0x6a27, 0x00}, + {0x6a28, 0x00}, + + // isp + {0x5000, 0x8f}, + {0x5001, 0x75}, + {0x5002, 0x7f}, // PWL0 + //{0x5002, 0x3f}, // PWL disable + {0x5003, 0x7a}, + + {0x5004, 0x3e}, + {0x5005, 0x1e}, + {0x5006, 0x1e}, + {0x5007, 0x1e}, + + {0x5008, 0x00}, + {0x500c, 0x00}, + {0x502c, 0x00}, + {0x502e, 0x00}, + {0x502f, 0x00}, + {0x504b, 0x00}, + {0x5053, 0x00}, + {0x505b, 0x00}, + {0x5063, 0x00}, + {0x5070, 0x00}, + {0x5074, 0x04}, + {0x507a, 0x04}, + {0x507b, 0x09}, + {0x5500, 0x02}, + {0x5700, 0x02}, + {0x5900, 0x02}, + {0x6007, 0x04}, + {0x6008, 0x05}, + {0x6009, 0x02}, + {0x600b, 0x08}, + {0x600c, 0x07}, + {0x600d, 0x88}, + {0x6016, 0x00}, + {0x6027, 0x04}, + {0x6028, 0x05}, + {0x6029, 0x02}, + {0x602b, 0x08}, + {0x602c, 0x07}, + {0x602d, 0x88}, + {0x6047, 0x04}, + {0x6048, 0x05}, + {0x6049, 0x02}, + {0x604b, 0x08}, + {0x604c, 0x07}, + {0x604d, 0x88}, + {0x6067, 0x04}, + {0x6068, 0x05}, + {0x6069, 0x02}, + {0x606b, 0x08}, + {0x606c, 0x07}, + {0x606d, 0x88}, + {0x6087, 0x04}, + {0x6088, 0x05}, + {0x6089, 0x02}, + {0x608b, 0x08}, + {0x608c, 0x07}, + {0x608d, 0x88}, + + // 12-bit PWL0 + {0x5e00, 0x00}, + + // m_ndX_exp[0:32] + // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518 + {0x5e01, 0x09}, + {0x5e02, 0x09}, + {0x5e03, 0x0a}, + {0x5e04, 0x0a}, + {0x5e05, 0x0a}, + {0x5e06, 0x0b}, + {0x5e07, 0x0b}, + {0x5e08, 0x0c}, + {0x5e09, 0x0c}, + {0x5e0a, 0x0d}, + {0x5e0b, 0x0d}, + {0x5e0c, 0x0e}, + {0x5e0d, 0x0e}, + {0x5e0e, 0x0f}, + {0x5e0f, 0x0f}, + {0x5e10, 0x10}, + {0x5e11, 0x10}, + {0x5e12, 0x11}, + {0x5e13, 0x11}, + {0x5e14, 0x12}, + {0x5e15, 0x12}, + {0x5e16, 0x12}, + {0x5e17, 0x12}, + {0x5e18, 0x13}, + {0x5e19, 0x13}, + {0x5e1a, 0x13}, + {0x5e1b, 0x14}, + {0x5e1c, 0x14}, + {0x5e1d, 0x14}, + {0x5e1e, 0x15}, + {0x5e1f, 0x15}, + {0x5e20, 0x15}, + {0x5e21, 0x16}, + + // m_ndY_val[0:32] + // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095 + {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00}, + {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff}, + {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00}, + {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00}, + {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00}, + {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80}, + {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80}, + {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80}, + {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80}, + {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80}, + {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80}, + {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80}, + {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80}, + {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80}, + {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80}, + {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80}, + {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80}, + {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40}, + {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40}, + {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40}, + {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40}, + {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40}, + {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40}, + {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40}, + {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40}, + {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40}, + {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40}, + {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40}, + {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40}, + {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40}, + {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40}, + {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40}, + {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40}, + + // disable PWL + /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00}, + {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00}, + {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00}, + {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00}, + {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00}, + {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00}, + {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00}, + {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00}, + {0x5e21, 0x00}, + + {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/ + + {0x4001, 0x2b}, // BLC_CTRL_1 + {0x4008, 0x02}, {0x4009, 0x03}, + {0x4018, 0x12}, + {0x4022, 0x40}, + {0x4023, 0x20}, + + // all black level targets are 0x40 + {0x4026, 0x00}, {0x4027, 0x40}, + {0x4028, 0x00}, {0x4029, 0x40}, + {0x402a, 0x00}, {0x402b, 0x40}, + {0x402c, 0x00}, {0x402d, 0x40}, + + {0x407e, 0xcc}, + {0x407f, 0x18}, + {0x4080, 0xff}, + {0x4081, 0xff}, + {0x4082, 0x01}, + {0x4083, 0x53}, + {0x4084, 0x01}, + {0x4085, 0x2b}, + {0x4086, 0x00}, + {0x4087, 0xb3}, + + {0x4640, 0x40}, + {0x4641, 0x11}, + {0x4642, 0x0e}, + {0x4643, 0xee}, + {0x4646, 0x0f}, + {0x4648, 0x00}, + {0x4649, 0x03}, + + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x80}, + {0x4f03, 0x2c}, + {0x4f04, 0xf8}, + + {0x4d09, 0xff}, + {0x4d09, 0xdf}, + + {0x5003, 0x7a}, + {0x5b80, 0x08}, + {0x5c00, 0x08}, + {0x5c80, 0x00}, + {0x5bbe, 0x12}, + {0x5c3e, 0x12}, + {0x5cbe, 0x12}, + {0x5b8a, 0x80}, + {0x5b8b, 0x80}, + {0x5b8c, 0x80}, + {0x5b8d, 0x80}, + {0x5b8e, 0x60}, + {0x5b8f, 0x80}, + {0x5b90, 0x80}, + {0x5b91, 0x80}, + {0x5b92, 0x80}, + {0x5b93, 0x20}, + {0x5b94, 0x80}, + {0x5b95, 0x80}, + {0x5b96, 0x80}, + {0x5b97, 0x20}, + {0x5b98, 0x00}, + {0x5b99, 0x80}, + {0x5b9a, 0x40}, + {0x5b9b, 0x20}, + {0x5b9c, 0x00}, + {0x5b9d, 0x00}, + {0x5b9e, 0x80}, + {0x5b9f, 0x00}, + {0x5ba0, 0x00}, + {0x5ba1, 0x00}, + {0x5ba2, 0x00}, + {0x5ba3, 0x00}, + {0x5ba4, 0x00}, + {0x5ba5, 0x00}, + {0x5ba6, 0x00}, + {0x5ba7, 0x00}, + {0x5ba8, 0x02}, + {0x5ba9, 0x00}, + {0x5baa, 0x02}, + {0x5bab, 0x76}, + {0x5bac, 0x03}, + {0x5bad, 0x08}, + {0x5bae, 0x00}, + {0x5baf, 0x80}, + {0x5bb0, 0x00}, + {0x5bb1, 0xc0}, + {0x5bb2, 0x01}, + {0x5bb3, 0x00}, + + // m_nNormCombineWeight + {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60}, + {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20}, + {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00}, + {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00}, + {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00}, + {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00}, + + // m_nCombinThreL + {0x5c28, 0x02}, {0x5c29, 0x00}, + {0x5c2a, 0x02}, {0x5c2b, 0x76}, + {0x5c2c, 0x03}, {0x5c2d, 0x08}, + + // m_nCombinThreS + {0x5c2e, 0x00}, {0x5c2f, 0x80}, + {0x5c30, 0x00}, {0x5c31, 0xc0}, + {0x5c32, 0x01}, {0x5c33, 0x00}, + + // m_nNormCombineWeight + {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80}, + {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60}, + {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40}, + {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00}, + {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00}, + {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00}, + + {0x5ca8, 0x01}, {0x5ca9, 0x00}, + {0x5caa, 0x02}, {0x5cab, 0x00}, + {0x5cac, 0x03}, {0x5cad, 0x08}, + + {0x5cae, 0x01}, {0x5caf, 0x00}, + {0x5cb0, 0x02}, {0x5cb1, 0x00}, + {0x5cb2, 0x03}, {0x5cb3, 0x08}, + + // combine ISP + {0x5be7, 0x80}, + {0x5bc9, 0x80}, + {0x5bca, 0x80}, + {0x5bcb, 0x80}, + {0x5bcc, 0x80}, + {0x5bcd, 0x80}, + {0x5bce, 0x80}, + {0x5bcf, 0x80}, + {0x5bd0, 0x80}, + {0x5bd1, 0x80}, + {0x5bd2, 0x20}, + {0x5bd3, 0x80}, + {0x5bd4, 0x40}, + {0x5bd5, 0x20}, + {0x5bd6, 0x00}, + {0x5bd7, 0x00}, + {0x5bd8, 0x00}, + {0x5bd9, 0x00}, + {0x5bda, 0x00}, + {0x5bdb, 0x00}, + {0x5bdc, 0x00}, + {0x5bdd, 0x00}, + {0x5bde, 0x00}, + {0x5bdf, 0x00}, + {0x5be0, 0x00}, + {0x5be1, 0x00}, + {0x5be2, 0x00}, + {0x5be3, 0x00}, + {0x5be4, 0x00}, + {0x5be5, 0x00}, + {0x5be6, 0x00}, + + // m_nSPDCombineWeight + {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40}, + {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20}, + {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00}, + {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00}, + {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00}, + {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00}, + + // m_nSPDCombineWeight + {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80}, + {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60}, + {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40}, + {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20}, + {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00}, + {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00}, + + {0x5d74, 0x01}, + {0x5d75, 0x00}, + + {0x5d1f, 0x81}, + {0x5d11, 0x00}, + {0x5d12, 0x10}, + {0x5d13, 0x10}, + {0x5d15, 0x05}, + {0x5d16, 0x05}, + {0x5d17, 0x05}, + {0x5d08, 0x03}, + {0x5d09, 0xb6}, + {0x5d0a, 0x03}, + {0x5d0b, 0xb6}, + {0x5d18, 0x03}, + {0x5d19, 0xb6}, + {0x5d62, 0x01}, + {0x5d40, 0x02}, + {0x5d41, 0x01}, + {0x5d63, 0x1f}, + {0x5d64, 0x00}, + {0x5d65, 0x80}, + {0x5d56, 0x00}, + {0x5d57, 0x20}, + {0x5d58, 0x00}, + {0x5d59, 0x20}, + {0x5d5a, 0x00}, + {0x5d5b, 0x0c}, + {0x5d5c, 0x02}, + {0x5d5d, 0x40}, + {0x5d5e, 0x02}, + {0x5d5f, 0x40}, + {0x5d60, 0x03}, + {0x5d61, 0x40}, + {0x5d4a, 0x02}, + {0x5d4b, 0x40}, + {0x5d4c, 0x02}, + {0x5d4d, 0x40}, + {0x5d4e, 0x02}, + {0x5d4f, 0x40}, + {0x5d50, 0x18}, + {0x5d51, 0x80}, + {0x5d52, 0x18}, + {0x5d53, 0x80}, + {0x5d54, 0x18}, + {0x5d55, 0x80}, + {0x5d46, 0x20}, + {0x5d47, 0x00}, + {0x5d48, 0x22}, + {0x5d49, 0x00}, + {0x5d42, 0x20}, + {0x5d43, 0x00}, + {0x5d44, 0x22}, + {0x5d45, 0x00}, + + {0x5004, 0x1e}, + {0x4221, 0x03}, // this is changed from 1 -> 3 + + // DCG exposure coarse + {0x3501, 0x01}, {0x3502, 0xc8}, + // SPD exposure coarse + {0x3541, 0x01}, {0x3542, 0xc8}, + // VS exposure coarse + {0x35c1, 0x00}, {0x35c2, 0x01}, + + // crc reference + {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, + // crc stat check + {0x507a, 0x5f}, {0x507b, 0x46}, + + // watchdog control + {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c}, + + // color balance gains + // blue + {0x5280, 0x06}, {0x5281, 0x4A}, // hcg + {0x5480, 0x06}, {0x5481, 0x4A}, // lcg + {0x5680, 0x07}, {0x5681, 0xDD}, // spd + {0x5880, 0x06}, {0x5881, 0x4A}, // vs + + // green(blue) + {0x5282, 0x04}, {0x5283, 0x00}, + {0x5482, 0x04}, {0x5483, 0x00}, + {0x5682, 0x04}, {0x5683, 0x00}, + {0x5882, 0x04}, {0x5883, 0x00}, + + // green(red) + {0x5284, 0x04}, {0x5285, 0x00}, + {0x5484, 0x04}, {0x5485, 0x00}, + {0x5684, 0x04}, {0x5685, 0x00}, + {0x5884, 0x04}, {0x5885, 0x00}, + + // red + {0x5286, 0x08}, {0x5287, 0x6C}, + {0x5486, 0x08}, {0x5487, 0x6C}, + {0x5686, 0x08}, {0x5687, 0xAA}, + {0x5886, 0x08}, {0x5887, 0x6C}, +}; + +struct i2c_random_wr_payload init_array_ar0231[] = { + {0x301A, 0x0018}, // RESET_REGISTER + + // 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/cameras/spectra.cc b/system/camerad/cameras/spectra.cc deleted file mode 100644 index 5c3e7a9d233b2a..00000000000000 --- a/system/camerad/cameras/spectra.cc +++ /dev/null @@ -1,1493 +0,0 @@ -#include "cdm.h" - -#include -#include -#include -#include -#include - -#include "media/cam_defs.h" -#include "media/cam_isp.h" -#include "media/cam_icp.h" -#include "media/cam_isp_ife.h" -#include "media/cam_sensor_cmn_header.h" -#include "media/cam_sync.h" - -#include "common/util.h" -#include "common/swaglog.h" -#include "system/camerad/cameras/ife.h" -#include "system/camerad/cameras/nv12_info.h" -#include "system/camerad/cameras/spectra.h" -#include "system/camerad/cameras/bps_blobs.h" - - -// ************** low level camera helpers **************** - -int do_cam_control(int fd, int op_code, void *handle, int size) { - struct cam_control camcontrol = {0}; - camcontrol.op_code = op_code; - camcontrol.handle = (uint64_t)handle; - if (size == 0) { - camcontrol.size = 8; - camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; - } else { - camcontrol.size = size; - camcontrol.handle_type = CAM_HANDLE_USER_POINTER; - } - - int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); - if (ret == -1) { - LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); - } - return ret; -} - -int do_sync_control(int fd, uint32_t id, void *handle, uint32_t size) { - struct cam_private_ioctl_arg arg = { - .id = id, - .size = size, - .ioctl_ptr = (uint64_t)handle, - }; - int ret = HANDLE_EINTR(ioctl(fd, CAM_PRIVATE_IOCTL_CMD, &arg)); - - int32_t ioctl_result = static_cast(arg.result); - if (ret < 0) { - LOGE("CAM_SYNC error: id %u - errno %d - ret %d - ioctl_result %d", id, errno, ret, ioctl_result); - return ret; - } - if (ioctl_result != 0) { - LOGE("CAM_SYNC error: id %u - errno %d - ret %d - ioctl_result %d", id, errno, ret, ioctl_result); - return ioctl_result; - } - return ret; -} - -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { - struct cam_acquire_dev_cmd cmd = { - .session_handle = session_handle, - .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), - .resource_hdl = (uint64_t)data, - }; - int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); - return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; -} - -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { - struct cam_config_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = dev_handle, - .packet_handle = packet_handle, - }; - return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); -} - -int device_control(int fd, int op_code, int session_handle, int dev_handle) { - // start stop and release are all the same - struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; - return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); -} - -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { - struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; - mem_mgr_alloc_cmd.len = len; - mem_mgr_alloc_cmd.align = align; - mem_mgr_alloc_cmd.flags = flags; - mem_mgr_alloc_cmd.num_hdl = 0; - if (mmu_hdl != 0) { - mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; - mem_mgr_alloc_cmd.num_hdl++; - } - if (mmu_hdl2 != 0) { - mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; - mem_mgr_alloc_cmd.num_hdl++; - } - - do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); - *handle = mem_mgr_alloc_cmd.out.buf_handle; - - void *ptr = NULL; - if (mem_mgr_alloc_cmd.out.fd > 0) { - ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); - assert(ptr != MAP_FAILED); - } - - // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); - - return ptr; -} - -void release(int video0_fd, uint32_t handle) { - struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; - mem_mgr_release_cmd.buf_handle = handle; - - int ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); - assert(ret == 0); -} - -static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { - cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings))); - unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; - unconditional_wait->delay = delay_ms; - unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; - return (struct cam_cmd_power *)(unconditional_wait + 1); -} - -// *** MemoryManager *** - -void *MemoryManager::alloc_buf(int size, uint32_t *handle) { - void *ptr; - auto &cache = cached_allocations[size]; - if (!cache.empty()) { - ptr = cache.front(); - cache.pop(); - *handle = handle_lookup[ptr]; - } else { - ptr = alloc_w_mmu_hdl(video0_fd, size, handle); - handle_lookup[ptr] = *handle; - size_lookup[ptr] = size; - } - memset(ptr, 0, size); - return ptr; -} - -void MemoryManager::free(void *ptr) { - cached_allocations[size_lookup[ptr]].push(ptr); -} - -MemoryManager::~MemoryManager() { - for (auto& x : cached_allocations) { - while (!x.second.empty()) { - void *ptr = x.second.front(); - x.second.pop(); - LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); - munmap(ptr, size_lookup[ptr]); - - // release fd - close(handle_lookup[ptr] >> 16); - release(video0_fd, handle_lookup[ptr]); - - handle_lookup.erase(ptr); - size_lookup.erase(ptr); - } - } -} - -// *** SpectraMaster *** - -void SpectraMaster::init() { - LOG("-- Opening devices"); - // video0 is req_mgr, the target of many ioctls - video0_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK)); - assert(video0_fd >= 0); - LOGD("opened video0"); - - // video1 is cam_sync, the target of some ioctls - cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); - assert(cam_sync_fd >= 0); - LOGD("opened video1 (cam_sync)"); - - // looks like there's only one of these - isp_fd = open_v4l_by_name_and_index("cam-isp"); - assert(isp_fd >= 0); - LOGD("opened isp %d", (int)isp_fd); - - icp_fd = open_v4l_by_name_and_index("cam-icp"); - assert(icp_fd >= 0); - LOGD("opened icp %d", (int)icp_fd); - - // query ISP for MMU handles - LOG("-- Query for MMU handles"); - struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; - struct cam_query_cap_cmd query_cap_cmd = {0}; - query_cap_cmd.handle_type = 1; - query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; - query_cap_cmd.size = sizeof(isp_query_cap_cmd); - int ret = do_cam_control(isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); - assert(ret == 0); - LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); - LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); - device_iommu = isp_query_cap_cmd.device_iommu.non_secure; - cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; - - // query ICP for MMU handles - struct cam_icp_query_cap_cmd icp_query_cap_cmd = {0}; - query_cap_cmd.caps_handle = (uint64_t)&icp_query_cap_cmd; - query_cap_cmd.size = sizeof(icp_query_cap_cmd); - ret = do_cam_control(icp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); - assert(ret == 0); - LOGD("using ICP MMU handle: %x", icp_query_cap_cmd.dev_iommu_handle.non_secure); - icp_device_iommu = icp_query_cap_cmd.dev_iommu_handle.non_secure; - - // subscribe - LOG("-- Subscribing"); - struct v4l2_event_subscription sub = {0}; - sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; - sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; - ret = HANDLE_EINTR(ioctl(video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); - LOGD("req mgr subscribe: %d", ret); - - mem_mgr.init(video0_fd); -} - -// *** SpectraCamera *** - -SpectraCamera::SpectraCamera(SpectraMaster *master, const CameraConfig &config) - : m(master), - enabled(config.enabled), - cc(config) { - ife_buf_depth = VIPC_BUFFER_COUNT; - assert(ife_buf_depth < MAX_IFE_BUFS); -} - -SpectraCamera::~SpectraCamera() { - if (open) { - camera_close(); - } -} - -int SpectraCamera::clear_req_queue() { - // for "non-realtime" BPS - if (icp_dev_handle > 0) { - struct cam_flush_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = icp_dev_handle, - .flush_type = CAM_FLUSH_TYPE_ALL, - }; - int err = do_cam_control(m->icp_fd, CAM_FLUSH_REQ, &cmd, sizeof(cmd)); - assert(err == 0); - LOGD("flushed bps: %d", err); - } - - // for "realtime" devices - struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; - req_mgr_flush_request.session_hdl = session_handle; - req_mgr_flush_request.link_hdl = link_handle; - req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL; - int ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); - LOGD("flushed all req: %d", ret); // returns a "time until timeout" on clearing the workq - - for (int i = 0; i < MAX_IFE_BUFS; ++i) { - destroySyncObjectAt(i); - } - - return ret; -} - -void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx) { - if (!openSensor()) { - return; - } - - if (!enabled) return; - - buf.out_img_width = sensor->frame_width / sensor->out_scale; - buf.out_img_height = (sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height) / sensor->out_scale; - - // size is driven by all the HW that handles frames, - // the video encoder has certain alignment requirements in this case - std::tie(stride, y_height, uv_height, yuv_size) = get_nv12_info(buf.out_img_width, buf.out_img_height); - uv_offset = stride * y_height; - - open = true; - configISP(); - if (cc.output_type == ISP_BPS_PROCESSED) configICP(); - configCSIPHY(); - linkDevices(); - - LOGD("camera init %d", cc.camera_num); - buf.init(device_id, ctx, this, v, ife_buf_depth, cc.stream_type); - camera_map_bufs(); - clearAndRequeue(1); -} - -void SpectraCamera::sensors_start() { - if (!enabled) return; - LOGD("starting sensor %d", cc.camera_num); - sensors_i2c(sensor->start_reg_array.data(), sensor->start_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, sensor->data_word); -} - -void SpectraCamera::sensors_poke(int request_id) { - uint32_t cam_packet_handle = 0; - int size = sizeof(struct cam_packet); - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - pkt->num_cmd_buf = 0; - pkt->kmd_cmd_buf_index = -1; - pkt->header.size = size; - pkt->header.op_code = CAM_SENSOR_PACKET_OPCODE_SENSOR_NOP; - pkt->header.request_id = request_id; - - int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); - if (ret != 0) { - LOGE("** sensor %d FAILED poke, disabling", cc.camera_num); - enabled = false; - return; - } -} - -void SpectraCamera::sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { - // LOGD("sensors_i2c: %d", len); - uint32_t cam_packet_handle = 0; - int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - pkt->num_cmd_buf = 1; - pkt->kmd_cmd_buf_index = -1; - pkt->header.size = size; - pkt->header.op_code = op_code; - struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - - buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); - buf_desc[0].type = CAM_CMD_BUF_I2C; - - auto i2c_random_wr = m->mem_mgr.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); - i2c_random_wr->header.count = len; - i2c_random_wr->header.op_code = 1; - i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; - i2c_random_wr->header.data_type = data_word ? CAMERA_SENSOR_I2C_TYPE_WORD : CAMERA_SENSOR_I2C_TYPE_BYTE; - i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; - memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload)); - - int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); - if (ret != 0) { - LOGE("** sensor %d FAILED i2c, disabling", cc.camera_num); - enabled = false; - return; - } -} - -int SpectraCamera::sensors_init() { - uint32_t cam_packet_handle = 0; - int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - pkt->num_cmd_buf = 2; - pkt->kmd_cmd_buf_index = -1; - pkt->header.op_code = CSLDeviceTypeImageSensor | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; - pkt->header.size = size; - struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - - buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); - buf_desc[0].type = CAM_CMD_BUF_LEGACY; - auto i2c_info = m->mem_mgr.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); - auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1); - - probe->camera_id = cc.camera_num; - i2c_info->slave_addr = sensor->getSlaveAddress(cc.camera_num); - // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz - //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; - i2c_info->i2c_freq_mode = I2C_FAST_MODE; - i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO; - - probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD; - probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; - probe->op_code = 3; // don't care? - probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; - probe->reg_addr = sensor->probe_reg_addr; - probe->expected_data = sensor->probe_expected_data; - probe->data_mask = 0; - - //buf_desc[1].size = buf_desc[1].length = 148; - buf_desc[1].size = buf_desc[1].length = 196; - buf_desc[1].type = CAM_CMD_BUF_I2C; - auto power_settings = m->mem_mgr.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); - - // power on - struct cam_cmd_power *power = power_settings.get(); - power->count = 4; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; - power->power_settings[0].power_seq_type = 3; // clock?? - power->power_settings[1].power_seq_type = 1; // analog - power->power_settings[2].power_seq_type = 2; // digital - power->power_settings[3].power_seq_type = 8; // reset low - power = power_set_wait(power, 1); - - // set clock - power->count = 1; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; - power->power_settings[0].power_seq_type = 0; - power->power_settings[0].config_val_low = sensor->mclk_frequency; - power = power_set_wait(power, 1); - - // reset high - power->count = 1; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; - power->power_settings[0].power_seq_type = 8; - power->power_settings[0].config_val_low = 1; - // wait 650000 cycles @ 19.2 mhz = 33.8 ms - power = power_set_wait(power, 34); - - // probe happens here - - // disable clock - power->count = 1; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; - power->power_settings[0].power_seq_type = 0; - power->power_settings[0].config_val_low = 0; - power = power_set_wait(power, 1); - - // reset high - power->count = 1; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; - power->power_settings[0].power_seq_type = 8; - power->power_settings[0].config_val_low = 1; - power = power_set_wait(power, 1); - - // reset low - power->count = 1; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; - power->power_settings[0].power_seq_type = 8; - power->power_settings[0].config_val_low = 0; - power = power_set_wait(power, 1); - - // power off - power->count = 3; - power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; - power->power_settings[0].power_seq_type = 2; - power->power_settings[1].power_seq_type = 1; - power->power_settings[2].power_seq_type = 3; - - int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); - LOGD("probing the sensor: %d", ret); - return ret; -} - -void add_patch(struct cam_packet *pkt, int32_t dst_hdl, uint32_t dst_offset, int32_t src_hdl, uint32_t src_offset) { - void *ptr = (char*)&pkt->payload + pkt->patch_offset; - struct cam_patch_desc *p = (struct cam_patch_desc *)((unsigned char*)ptr + sizeof(struct cam_patch_desc)*pkt->num_patches); - p->dst_buf_hdl = dst_hdl; - p->src_buf_hdl = src_hdl; - p->dst_offset = dst_offset; - p->src_offset = src_offset; - pkt->num_patches++; -}; - -void SpectraCamera::config_bps(int idx, int request_id) { - /* - Handles per-frame BPS config. - * BPS = Bayer Processing Segment - */ - - int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*2; - size += sizeof(struct cam_patch_desc)*9; - - uint32_t cam_packet_handle = 0; - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - - pkt->header.op_code = CSLDeviceTypeBPS | CAM_ICP_OPCODE_BPS_UPDATE; - pkt->header.request_id = request_id; - pkt->header.size = size; - - typedef struct { - struct { - uint32_t ptr[2]; - uint32_t unknown[2]; - } frames[9]; - - uint32_t unknown1; - uint32_t unknown2; - uint32_t unknown3; - uint32_t unknown4; - - uint32_t cdm_addr; - uint32_t cdm_size; - uint32_t settings_addr; - uint32_t striping_addr; - uint32_t cdm_addr2; - - uint32_t req_id; - uint64_t handle; - } bps_tmp; - - typedef struct { - uint32_t a; - uint32_t n; - unsigned base : 32; - unsigned unused : 12; - unsigned length : 20; - uint32_t p; - uint32_t u; - uint32_t h; - uint32_t b; - } cdm_tmp; - - // *** cmd buf *** - std::vector patches; - struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - { - pkt->num_cmd_buf = 2; - pkt->kmd_cmd_buf_index = -1; - pkt->kmd_cmd_buf_offset = 0; - - buf_desc[0].meta_data = 0; - buf_desc[0].mem_handle = bps_cmd.handle; - buf_desc[0].type = CAM_CMD_BUF_FW; - buf_desc[0].offset = bps_cmd.aligned_size()*idx; - - buf_desc[0].length = sizeof(bps_tmp) + sizeof(cdm_tmp); - buf_desc[0].size = buf_desc[0].length; - - // rest gets patched in - bps_tmp *fp = (bps_tmp *)((unsigned char *)bps_cmd.ptr + buf_desc[0].offset); - memset(fp, 0, buf_desc[0].length); - fp->handle = (uint64_t)icp_dev_handle; - fp->cdm_size = bps_cdm_striping_bl.size; // this comes from the striping lib create call - fp->req_id = 0; // why always 0? - - cdm_tmp *pa = (cdm_tmp *)((unsigned char *)fp + sizeof(bps_tmp)); - pa->a = 0; - pa->n = 1; - pa->p = 20; // GENERIC - pa->u = 0; - pa->h = 0; - pa->b = 0; - pa->unused = 0; - pa->base = 0; // this gets patched - - int cdm_len = 0; - - if (bps_lin_reg.size() == 0) { - for (int i = 0; i < 4; i++) { - bps_lin_reg.push_back(((sensor->linearization_pts[i] & 0xffff) << 0x10) | (sensor->linearization_pts[i] >> 0x10)); - } - } - - if (bps_ccm_reg.size() == 0) { - for (int i = 0; i < 3; i++) { - bps_ccm_reg.push_back(sensor->color_correct_matrix[i] | (sensor->color_correct_matrix[i+3] << 0x10)); - bps_ccm_reg.push_back(sensor->color_correct_matrix[i+6]); - } - } - - // white balance - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2868, { - 0x04000400, - 0x00000400, - 0x00000000, - 0x00000000, - }); - // debayer - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2878, { - 0x00000080, - 0x00800066, - }); - // linearization, EN=0 - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1868, bps_lin_reg); - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1878, bps_lin_reg); - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1888, bps_lin_reg); - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1898, bps_lin_reg); - /* - uint8_t *start = (unsigned char *)bps_cdm_program_array.ptr + cdm_len; - uint64_t addr; - cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1); - patches.push_back(addr - (uint64_t)start); - */ - // color correction - cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2e68, bps_ccm_reg); - - cdm_len += build_common_ife_bps((unsigned char *)bps_cdm_program_array.ptr + cdm_len, cc, sensor.get(), patches, false); - - pa->length = cdm_len - 1; - - // *** second command *** - // parsed by cam_icp_packet_generic_blob_handler - struct isp_packet { - uint32_t header; - struct cam_icp_clk_bw_request clk; - } __attribute__((packed)) tmp; - tmp.header = CAM_ICP_CMD_GENERIC_BLOB_CLK; - tmp.header |= (sizeof(cam_icp_clk_bw_request)) << 8; - tmp.clk.budget_ns = 0x1fca058; - tmp.clk.frame_cycles = 2329024; // comes from the striping lib - tmp.clk.rt_flag = 0x0; - tmp.clk.uncompressed_bw = 0x38512180; - tmp.clk.compressed_bw = 0x38512180; - - buf_desc[1].size = sizeof(tmp); - buf_desc[1].offset = 0; - buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; - buf_desc[1].type = CAM_CMD_BUF_GENERIC; - buf_desc[1].meta_data = CAM_ICP_CMD_META_GENERIC_BLOB; - auto buf2 = m->mem_mgr.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); - memcpy(buf2.get(), &tmp, sizeof(tmp)); - } - - // *** io config *** - pkt->num_io_configs = 2; - pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; - struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset); - { - // input frame - io_cfg[0].offsets[0] = 0; - io_cfg[0].mem_handle[0] = buf_handle_raw[idx]; - - io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height + sensor->extra_height, - .plane_stride = sensor->frame_stride, - .slice_height = sensor->frame_height + sensor->extra_height, - }; - io_cfg[0].format = sensor->mipi_format; - io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; - io_cfg[0].color_pattern = 0x5; - io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc); - io_cfg[0].resource_type = CAM_ICP_BPS_INPUT_IMAGE; - io_cfg[0].fence = sync_objs_ife[idx]; - io_cfg[0].direction = CAM_BUF_INPUT; - io_cfg[0].subsample_pattern = 0x1; - io_cfg[0].framedrop_pattern = 0x1; - - // output frame - io_cfg[1].mem_handle[0] = buf_handle_yuv[idx]; - io_cfg[1].mem_handle[1] = buf_handle_yuv[idx]; - io_cfg[1].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, - .plane_stride = stride, - .slice_height = y_height, - }; - io_cfg[1].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, - .plane_stride = stride, - .slice_height = uv_height, - }; - io_cfg[1].offsets[1] = ALIGNED_SIZE(io_cfg[1].planes[0].plane_stride*io_cfg[1].planes[0].slice_height, 0x1000); - assert(io_cfg[1].offsets[1] == uv_offset); - - io_cfg[1].format = CAM_FORMAT_NV12; // TODO: why is this 21 in the dump? should be 12 - io_cfg[1].color_space = CAM_COLOR_SPACE_BT601_FULL; - io_cfg[1].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL; - io_cfg[1].fence = sync_objs_bps[idx]; - io_cfg[1].direction = CAM_BUF_OUTPUT; - io_cfg[1].subsample_pattern = 0x1; - io_cfg[1].framedrop_pattern = 0x1; - } - - // *** patches *** - { - assert(patches.size() == 0 | patches.size() == 1); - pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs; - - if (patches.size() > 0) { - add_patch(pkt.get(), bps_cmd.handle, patches[0], bps_linearization_lut.handle, 0); - } - - // input frame - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[0].ptr[0]), buf_handle_raw[idx], 0); - - // output frame - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0); - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]); - - // rest of buffers - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, settings_addr), bps_iq.handle, 0); - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, cdm_addr2), bps_cmd.handle, sizeof(bps_tmp)); - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + 0xc8, bps_cdm_program_array.handle, 0); - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, striping_addr), bps_striping.handle, 0); - add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, cdm_addr), bps_cdm_striping_bl.handle, 0); - } - - int ret = device_config(m->icp_fd, session_handle, icp_dev_handle, cam_packet_handle); - assert(ret == 0); -} - -void SpectraCamera::config_ife(int idx, int request_id, bool init) { - /* - Handles initial + per-frame IFE config. - * IFE = Image Front End - */ - int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2; - size += sizeof(struct cam_patch_desc)*10; - if (!init) { - size += sizeof(struct cam_buf_io_cfg); - } - - uint32_t cam_packet_handle = 0; - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - - if (!init) { - pkt->header.op_code = CSLDeviceTypeIFE | OpcodesIFEUpdate; // 0xf000001 - pkt->header.request_id = request_id; - } else { - pkt->header.op_code = CSLDeviceTypeIFE | OpcodesIFEInitialConfig; // 0xf000000 - pkt->header.request_id = 1; - } - pkt->header.size = size; - - // *** cmd buf *** - std::vector patches; - { - struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - pkt->num_cmd_buf = 2; - - // *** first command *** - buf_desc[0].size = ife_cmd.size; - buf_desc[0].length = 0; - buf_desc[0].type = CAM_CMD_BUF_DIRECT; - buf_desc[0].meta_data = CAM_ISP_PACKET_META_COMMON; - buf_desc[0].mem_handle = ife_cmd.handle; - buf_desc[0].offset = ife_cmd.aligned_size()*idx; - - // stream of IFE register writes - bool is_raw = cc.output_type != ISP_IFE_PROCESSED; - if (!is_raw) { - if (init) { - buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches, buf.out_img_width, buf.out_img_height); - } else { - buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); - } - } - - pkt->kmd_cmd_buf_offset = buf_desc[0].length; - pkt->kmd_cmd_buf_index = 0; - - // *** second command *** - // parsed by cam_isp_packet_generic_blob_handler - struct isp_packet { - uint32_t type_0; - cam_isp_resource_hfr_config resource_hfr; - - uint32_t type_1; - cam_isp_clock_config clock; - uint64_t extra_rdi_hz[3]; - - uint32_t type_2; - cam_isp_bw_config bw; - struct cam_isp_bw_vote extra_rdi_vote[6]; - } __attribute__((packed)) tmp; - memset(&tmp, 0, sizeof(tmp)); - - tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG; - tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8; - static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20); - tmp.resource_hfr = { - .num_ports = 1, - .port_hfr_config[0] = { - .resource_type = static_cast(is_raw ? CAM_ISP_IFE_OUT_RES_RDI_0 : CAM_ISP_IFE_OUT_RES_FULL), - .subsample_pattern = 1, - .subsample_period = 0, - .framedrop_pattern = 1, - .framedrop_period = 0, - } - }; - - tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG; - tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8; - static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38); - tmp.clock = { - .usage_type = 1, // dual mode - .num_rdi = 4, - .left_pix_hz = 404000000, - .right_pix_hz = 404000000, - .rdi_hz[0] = 404000000, - }; - - tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG; - tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8; - static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0); - tmp.bw = { - .usage_type = 1, // dual mode - .num_rdi = 4, - .left_pix_vote = { - .resource_id = 0, - .cam_bw_bps = 450000000, - .ext_bw_bps = 450000000, - }, - .rdi_vote[0] = { - .resource_id = 0, - .cam_bw_bps = 8706200000, - .ext_bw_bps = 8706200000, - }, - }; - - static_assert(offsetof(struct isp_packet, type_2) == 0x60); - - buf_desc[1].size = sizeof(tmp); - buf_desc[1].offset = !init ? 0x60 : 0; - buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; - buf_desc[1].type = CAM_CMD_BUF_GENERIC; - buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; - auto buf2 = m->mem_mgr.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); - memcpy(buf2.get(), &tmp, sizeof(tmp)); - } - - // *** io config *** - if (!init) { - // configure output frame - pkt->num_io_configs = 1; - pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; - - struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset); - if (cc.output_type != ISP_IFE_PROCESSED) { - io_cfg[0].mem_handle[0] = buf_handle_raw[idx]; - io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height, - .plane_stride = sensor->frame_stride, - .slice_height = sensor->frame_height + sensor->extra_height, - }; - io_cfg[0].format = sensor->mipi_format; - io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; - io_cfg[0].color_pattern = 0x5; - io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc); - io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; - io_cfg[0].fence = sync_objs_ife[idx]; - io_cfg[0].direction = CAM_BUF_OUTPUT; - io_cfg[0].subsample_pattern = 0x1; - io_cfg[0].framedrop_pattern = 0x1; - } else { - io_cfg[0].mem_handle[0] = buf_handle_yuv[idx]; - io_cfg[0].mem_handle[1] = buf_handle_yuv[idx]; - io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, - .plane_stride = stride, - .slice_height = y_height, - }; - io_cfg[0].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, - .plane_stride = stride, - .slice_height = uv_height, - }; - io_cfg[0].offsets[1] = uv_offset; - io_cfg[0].format = CAM_FORMAT_NV12; - io_cfg[0].color_space = 0; - io_cfg[0].color_pattern = 0x0; - io_cfg[0].bpp = 0; - io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_FULL; - io_cfg[0].fence = sync_objs_ife[idx]; - io_cfg[0].direction = CAM_BUF_OUTPUT; - io_cfg[0].subsample_pattern = 0x1; - io_cfg[0].framedrop_pattern = 0x1; - } - } - - // *** patches *** - // sets up the kernel driver to do address translation for the IFE - { - // order here corresponds to the one in build_initial_config - assert(patches.size() == 6 || patches.size() == 0); - - pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs; - if (patches.size() > 0) { - // linearization LUT - add_patch(pkt.get(), ife_cmd.handle, patches[0], ife_linearization_lut.handle, 0); - - // vignetting correction LUTs - add_patch(pkt.get(), ife_cmd.handle, patches[1], ife_vignetting_lut.handle, 0); - add_patch(pkt.get(), ife_cmd.handle, patches[2], ife_vignetting_lut.handle, ife_vignetting_lut.size); - - // gamma LUTs - for (int i = 0; i < 3; i++) { - add_patch(pkt.get(), ife_cmd.handle, patches[i+3], ife_gamma_lut.handle, ife_gamma_lut.size*i); - } - } - } - - int ret = device_config(m->isp_fd, session_handle, isp_dev_handle, cam_packet_handle); - assert(ret == 0); -} - -void SpectraCamera::enqueue_frame(uint64_t request_id) { - int i = request_id % ife_buf_depth; - assert(sync_objs_ife[i] == 0); - - // create output fences - struct cam_sync_info sync_create = {0}; - strcpy(sync_create.name, "NodeOutputPortFence"); - int ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); - if (ret != 0) { - LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj); - } else { - sync_objs_ife[i] = sync_create.sync_obj; - } - - if (icp_dev_handle > 0) { - ret = do_cam_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); - if (ret != 0) { - LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj); - } else { - sync_objs_bps[i] = sync_create.sync_obj; - } - } - - // schedule request with camera request manager - struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; - req_mgr_sched_request.session_hdl = session_handle; - req_mgr_sched_request.link_hdl = link_handle; - req_mgr_sched_request.req_id = request_id; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); - if (ret != 0) { - LOGE("failed to schedule cam mgr request: %d %lu", ret, request_id); - } - - // poke sensor, must happen after schedule - sensors_poke(request_id); - - // submit request to IFE and BPS - config_ife(i, request_id); - if (cc.output_type == ISP_BPS_PROCESSED) config_bps(i, request_id); -} - -void SpectraCamera::destroySyncObjectAt(int index) { - auto destroy_sync_obj = [](int cam_sync_fd, int32_t &sync_obj) { - if (sync_obj == 0) return; - - struct cam_sync_info sync_destroy = {.sync_obj = sync_obj}; - int ret = do_sync_control(cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); - if (ret != 0) { - LOGE("Failed to destroy sync object: %d, sync_obj: %d", ret, sync_destroy.sync_obj); - } - - sync_obj = 0; // Reset the sync object to 0 - }; - - destroy_sync_obj(m->cam_sync_fd, sync_objs_ife[index]); - destroy_sync_obj(m->cam_sync_fd, sync_objs_bps[index]); -} - -void SpectraCamera::camera_map_bufs() { - int ret; - for (int i = 0; i < ife_buf_depth; i++) { - // map our VisionIPC bufs into ISP memory - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.mmu_hdls[0] = m->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - if (icp_dev_handle > 0) { - mem_mgr_map_cmd.num_hdl = 2; - mem_mgr_map_cmd.mmu_hdls[1] = m->icp_device_iommu; - } - - if (cc.output_type != ISP_IFE_PROCESSED) { - // RAW bayer images - mem_mgr_map_cmd.fd = buf.camera_bufs_raw[i].fd; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - assert(ret == 0); - LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs_raw[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle_raw[i] = mem_mgr_map_cmd.out.buf_handle; - } - - if (cc.output_type != ISP_RAW_OUTPUT) { - // final processed images - VisionBuf *vb = buf.vipc_server->get_buffer(buf.stream_type, i); - mem_mgr_map_cmd.fd = vb->fd; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - LOGD("map buf req: (fd: %d) 0x%x %d", vb->fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle_yuv[i] = mem_mgr_map_cmd.out.buf_handle; - } - } -} - -bool SpectraCamera::openSensor() { - sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", cc.camera_num); - assert(sensor_fd >= 0); - LOGD("opened sensor for %d", cc.camera_num); - - LOGD("-- Probing sensor %d", cc.camera_num); - - auto init_sensor_lambda = [this](SensorInfo *s) { - if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) { - ((OS04C10*)s)->ife_downscale_configure(); - } - sensor.reset(s); - return (sensors_init() == 0); - }; - - // Figure out which sensor we have - if (!init_sensor_lambda(new OS04C10) && - !init_sensor_lambda(new OX03C10)) { - LOGE("** sensor %d FAILED bringup, disabling", cc.camera_num); - enabled = false; - return false; - } - LOGD("-- Probing sensor %d success", cc.camera_num); - - // create session - struct cam_req_mgr_session_info session_info = {}; - int ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); - LOGD("get session: %d 0x%X", ret, session_info.session_hdl); - session_handle = session_info.session_hdl; - - // access the sensor - LOGD("-- Accessing sensor"); - auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr); - assert(sensor_dev_handle_); - sensor_dev_handle = *sensor_dev_handle_; - LOGD("acquire sensor dev"); - - LOG("-- Configuring sensor"); - sensors_i2c(sensor->init_reg_array.data(), sensor->init_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, sensor->data_word); - return true; -} - -void SpectraCamera::configISP() { - if (!enabled) return; - - struct cam_isp_in_port_info in_port_info = { - // ISP input to the CSID - .res_type = cc.phy, - .lane_type = CAM_ISP_LANE_TYPE_DPHY, - .lane_num = 4, - .lane_cfg = 0x3210, - - .vc = 0x0, - .dt = sensor->frame_data_type, - .format = sensor->mipi_format, - - .test_pattern = sensor->bayer_pattern, - .usage_type = 0x0, - - .left_start = 0, - .left_stop = sensor->frame_width - 1, - .left_width = sensor->frame_width, - - .right_start = 0, - .right_stop = sensor->frame_width - 1, - .right_width = sensor->frame_width, - - .line_start = sensor->frame_offset, - .line_stop = sensor->frame_height + sensor->frame_offset - 1, - .height = sensor->frame_height + sensor->frame_offset, - - .pixel_clk = 0x0, - .batch_size = 0x0, - .dsp_mode = CAM_ISP_DSP_MODE_NONE, - .hbi_cnt = 0x0, - .custom_csid = 0x0, - - // ISP outputs - .num_out_res = 0x1, - .data[0] = (struct cam_isp_out_port_info){ - .res_type = CAM_ISP_IFE_OUT_RES_FULL, - .format = CAM_FORMAT_NV12, - .width = buf.out_img_width, - .height = buf.out_img_height + sensor->extra_height, - .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, - }, - }; - - if (cc.output_type != ISP_IFE_PROCESSED) { - in_port_info.line_start = 0; - in_port_info.line_stop = sensor->frame_height + sensor->extra_height - 1; - in_port_info.height = sensor->frame_height + sensor->extra_height; - - in_port_info.data[0].res_type = CAM_ISP_IFE_OUT_RES_RDI_0; - in_port_info.data[0].format = sensor->mipi_format; - } - - struct cam_isp_resource isp_resource = { - .resource_id = CAM_ISP_RES_ID_PORT, - .handle_type = CAM_HANDLE_USER_POINTER, - .res_hdl = (uint64_t)&in_port_info, - .length = sizeof(in_port_info), - }; - - auto isp_dev_handle_ = device_acquire(m->isp_fd, session_handle, &isp_resource); - assert(isp_dev_handle_); - isp_dev_handle = *isp_dev_handle_; - LOGD("acquire isp dev"); - - // allocate IFE memory, then configure it - ife_cmd.init(m, 67984, 0x20, false, m->device_iommu, m->cdm_iommu, ife_buf_depth); - if (cc.output_type == ISP_IFE_PROCESSED) { - assert(sensor->gamma_lut_rgb.size() == 64); - ife_gamma_lut.init(m, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x20, false, m->device_iommu, m->cdm_iommu, 3); // 3 for RGB - for (int i = 0; i < 3; i++) { - memcpy(ife_gamma_lut.ptr + ife_gamma_lut.size*i, sensor->gamma_lut_rgb.data(), ife_gamma_lut.size); - } - assert(sensor->linearization_lut.size() == 36); - ife_linearization_lut.init(m, sensor->linearization_lut.size()*sizeof(uint32_t), 0x20, false, m->device_iommu, m->cdm_iommu); - memcpy(ife_linearization_lut.ptr, sensor->linearization_lut.data(), ife_linearization_lut.size); - assert(sensor->vignetting_lut.size() == 221); - ife_vignetting_lut.init(m, sensor->vignetting_lut.size()*sizeof(uint32_t), 0x20, false, m->device_iommu, m->cdm_iommu, 2); - for (int i = 0; i < 2; i++) { - memcpy(ife_vignetting_lut.ptr + ife_vignetting_lut.size*i, sensor->vignetting_lut.data(), ife_vignetting_lut.size); - } - } - - config_ife(0, 1, true); -} - -void SpectraCamera::configICP() { - /* - Configures both the ICP and BPS. - */ - - int cfg_handle; - - uint32_t cfg_size = sizeof(bps_cfg[0]) / sizeof(bps_cfg[0][0]); - void *cfg = alloc_w_mmu_hdl(m->video0_fd, cfg_size, (uint32_t*)&cfg_handle, 0x1, - CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_HW_SHARED_ACCESS, - m->icp_device_iommu); - memcpy(cfg, bps_cfg[sensor->num()], cfg_size); - - struct cam_icp_acquire_dev_info icp_info = { - .scratch_mem_size = 0x0, - .dev_type = CAM_ICP_RES_TYPE_BPS, - .io_config_cmd_size = cfg_size, - .io_config_cmd_handle = cfg_handle, - .secure_mode = 0, - .num_out_res = 1, - .in_res = (struct cam_icp_res_info){ - .format = 0x9, // RAW MIPI - .width = sensor->frame_width, - .height = sensor->frame_height, - .fps = 20, - }, - .out_res[0] = (struct cam_icp_res_info){ - .format = 0x3, // YUV420NV12 - .width = buf.out_img_width, - .height = buf.out_img_height, - .fps = 20, - }, - }; - auto h = device_acquire(m->icp_fd, session_handle, &icp_info); - assert(h); - icp_dev_handle = *h; - LOGD("acquire icp dev"); - - release(m->video0_fd, cfg_handle); - - // BPS has a lot of buffers to init - bps_cmd.init(m, 464, 0x20, true, m->icp_device_iommu, 0, ife_buf_depth); - - // BPSIQSettings struct - uint32_t settings_size = sizeof(bps_settings[0]) / sizeof(bps_settings[0][0]); - bps_iq.init(m, settings_size, 0x20, true, m->icp_device_iommu); - memcpy(bps_iq.ptr, bps_settings[sensor->num()], settings_size); - - // for cdm register writes, just make it bigger than you need - bps_cdm_program_array.init(m, 0x1000, 0x20, true, m->icp_device_iommu); - - // striping lib output - uint32_t striping_size = sizeof(bps_striping_output[0]) / sizeof(bps_striping_output[0][0]); - bps_striping.init(m, striping_size, 0x20, true, m->icp_device_iommu); - memcpy(bps_striping.ptr, bps_striping_output[sensor->num()], striping_size); - - // used internally by the BPS, we just allocate it. - // size comes from the BPSStripingLib - bps_cdm_striping_bl.init(m, 0xa100, 0x20, true, m->icp_device_iommu); - - // LUTs - /* - bps_linearization_lut.init(m, sensor->linearization_lut.size()*sizeof(uint32_t), 0x20, true, m->icp_device_iommu); - memcpy(bps_linearization_lut.ptr, sensor->linearization_lut.data(), bps_linearization_lut.size); - */ -} - -void SpectraCamera::configCSIPHY() { - csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", cc.camera_num); - assert(csiphy_fd >= 0); - LOGD("opened csiphy for %d", cc.camera_num); - - struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0}; - auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info); - assert(csiphy_dev_handle_); - csiphy_dev_handle = *csiphy_dev_handle_; - LOGD("acquire csiphy dev"); - - // config csiphy - LOG("-- Config CSI PHY"); - { - uint32_t cam_packet_handle = 0; - int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - auto pkt = m->mem_mgr.alloc(size, &cam_packet_handle); - pkt->num_cmd_buf = 1; - pkt->kmd_cmd_buf_index = -1; - pkt->header.size = size; - struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; - - buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); - buf_desc[0].type = CAM_CMD_BUF_GENERIC; - - auto csiphy_info = m->mem_mgr.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); - csiphy_info->lane_mask = 0x1f; - csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? - csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane - csiphy_info->combo_mode = 0x0; - csiphy_info->lane_cnt = 0x4; - csiphy_info->secure_mode = 0x0; - csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL; - csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py - - int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); - assert(ret_ == 0); - } -} - -void SpectraCamera::linkDevices() { - LOG("-- Link devices"); - struct cam_req_mgr_link_info req_mgr_link_info = {0}; - req_mgr_link_info.session_hdl = session_handle; - req_mgr_link_info.num_devices = 2; - req_mgr_link_info.dev_hdls[0] = isp_dev_handle; - req_mgr_link_info.dev_hdls[1] = sensor_dev_handle; - int ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); - assert(ret == 0); - link_handle = req_mgr_link_info.link_hdl; - LOGD("link: %d session: 0x%X isp: 0x%X sensors: 0x%X link: 0x%X", ret, session_handle, isp_dev_handle, sensor_dev_handle, link_handle); - - struct cam_req_mgr_link_control req_mgr_link_control = {0}; - req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE; - req_mgr_link_control.session_hdl = session_handle; - req_mgr_link_control.num_links = 1; - req_mgr_link_control.link_hdls[0] = link_handle; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); - LOGD("link control: %d", ret); - - ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle); - LOGD("start csiphy: %d", ret); - assert(ret == 0); - ret = device_control(m->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); - LOGD("start isp: %d", ret); - assert(ret == 0); - if (cc.output_type == ISP_BPS_PROCESSED) { - ret = device_control(m->icp_fd, CAM_START_DEV, session_handle, icp_dev_handle); - LOGD("start icp: %d", ret); - assert(ret == 0); - } -} - -void SpectraCamera::camera_close() { - LOG("-- Stop devices %d", cc.camera_num); - - if (enabled) { - clear_req_queue(); - - // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle); - // LOGD("stop sensor: %d", ret); - int ret = device_control(m->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle); - LOGD("stop isp: %d", ret); - if (cc.output_type == ISP_BPS_PROCESSED) { - ret = device_control(m->icp_fd, CAM_STOP_DEV, session_handle, icp_dev_handle); - LOGD("stop icp: %d", ret); - } - ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle); - LOGD("stop csiphy: %d", ret); - - // link control stop - LOG("-- Stop link control"); - struct cam_req_mgr_link_control req_mgr_link_control = {0}; - req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE; - req_mgr_link_control.session_hdl = session_handle; - req_mgr_link_control.num_links = 1; - req_mgr_link_control.link_hdls[0] = link_handle; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); - LOGD("link control stop: %d", ret); - - // unlink - LOG("-- Unlink"); - struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; - req_mgr_unlink_info.session_hdl = session_handle; - req_mgr_unlink_info.link_hdl = link_handle; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); - LOGD("unlink: %d", ret); - - // release devices - LOGD("-- Release devices"); - ret = device_control(m->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle); - LOGD("release isp: %d", ret); - if (cc.output_type == ISP_BPS_PROCESSED) { - ret = device_control(m->icp_fd, CAM_RELEASE_DEV, session_handle, icp_dev_handle); - LOGD("release icp: %d", ret); - } - ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); - LOGD("release csiphy: %d", ret); - - for (int i = 0; i < ife_buf_depth; i++) { - if (buf_handle_raw[i]) { - release(m->video0_fd, buf_handle_raw[i]); - } - if (buf_handle_yuv[i]) { - release(m->video0_fd, buf_handle_yuv[i]); - } - } - LOGD("released buffers"); - } - - int ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); - LOGD("release sensor: %d", ret); - - // destroyed session - struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle}; - ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info)); - LOGD("destroyed session %d: %d", cc.camera_num, ret); -} - -bool SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { - /* - Handles camera SOF event. Returns true if the frame is valid for publishing. - */ - - uint64_t request_id = event_data->u.frame_msg.request_id; // ID from the camera request manager - uint64_t frame_id_raw = event_data->u.frame_msg.frame_id; // raw as opposed to our re-indexed frame ID - uint64_t timestamp = event_data->u.frame_msg.timestamp; // timestamped in the kernel's SOF IRQ callback - //LOGD("handle cam %d ts %lu req id %lu frame id %lu", cc.camera_num, timestamp, request_id, frame_id_raw); - - // if there's a lag, some more frames could have already come in before - // we cleared the queue, so we'll still get them with valid (> 0) request IDs. - if (timestamp < last_requeue_ts) { - LOGD("skipping frame: ts before requeue / cam %d ts %lu req id %lu frame id %lu", cc.camera_num, timestamp, request_id, frame_id_raw); - return false; - } - - if (stress_test("skipping SOF event")) { - return false; - } - - if (!validateEvent(request_id, frame_id_raw)) { - return false; - } - - // Update tracking variables - if (request_id == request_id_last + 1) { - skip_expected = false; - } - frame_id_raw_last = frame_id_raw; - request_id_last = request_id; - - // Wait until frame's fully read out and processed - if (!waitForFrameReady(request_id)) { - // Reset queue on sync failure to prevent frame tearing - LOGE("camera %d sync failure %ld %ld ", cc.camera_num, request_id, frame_id_raw); - clearAndRequeue(request_id + 1); - return false; - } - - int buf_idx = request_id % ife_buf_depth; - bool ret = processFrame(buf_idx, request_id, frame_id_raw, timestamp); - destroySyncObjectAt(buf_idx); - enqueue_frame(request_id + ife_buf_depth); // request next frame for this slot - return ret; -} - -bool SpectraCamera::validateEvent(uint64_t request_id, uint64_t frame_id_raw) { - // check if the request ID is even valid. this happens after queued - // requests are cleared. unclear if it happens any other time. - if (request_id == 0) { - if (invalid_request_count++ > ife_buf_depth+2) { - LOGE("camera %d reset after half second of invalid requests", cc.camera_num); - clearAndRequeue(request_id_last + 1); - invalid_request_count = 0; - } - return false; - } - invalid_request_count = 0; - - // check for skips in frame_id or request_id - if (!skip_expected) { - if (frame_id_raw != frame_id_raw_last + 1) { - LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, frame_id_raw_last, frame_id_raw); - clearAndRequeue(request_id + 1); - return false; - } - - if (request_id != request_id_last + 1) { - LOGE("camera %d requests skipped %ld -> %ld", cc.camera_num, request_id_last, request_id); - clearAndRequeue(request_id + 1); - return false; - } - } - return true; -} - -void SpectraCamera::clearAndRequeue(uint64_t from_request_id) { - // clear everything, then queue up a fresh set of frames - LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, from_request_id); - clear_req_queue(); - last_requeue_ts = nanos_since_boot(); - for (uint64_t id = from_request_id; id < from_request_id + ife_buf_depth; ++id) { - enqueue_frame(id); - } - skip_expected = true; -} - -bool SpectraCamera::waitForFrameReady(uint64_t request_id) { - int buf_idx = request_id % ife_buf_depth; - assert(sync_objs_ife[buf_idx]); - - if (stress_test("sync sleep time")) { - util::sleep_for(350); - return false; - } - - auto waitForSync = [&](uint32_t sync_obj, int timeout_ms, const char *sync_type) { - double st = millis_since_boot(); - struct cam_sync_wait sync_wait = {}; - sync_wait.sync_obj = sync_obj; - sync_wait.timeout_ms = stress_test(sync_type) ? 1 : timeout_ms; - bool ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)) == 0; - double et = millis_since_boot(); - if (!ret) LOGE("camera %d %s failed after %.2fms", cc.camera_num, sync_type, et-st); - return ret; - }; - - // wait for frame from IFE - // - in RAW_OUTPUT mode, this time is just the frame readout from the sensor - // - in IFE_PROCESSED mode, this time also includes image processing (~1ms) - bool success = waitForSync(sync_objs_ife[buf_idx], 100, "IFE sync"); - if (success && sync_objs_bps[buf_idx]) { - // BPS is typically 7ms - success = waitForSync(sync_objs_bps[buf_idx], 50, "BPS sync"); - } - - return success; -} - -bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp) { - if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) { - return false; - } - - // in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time - uint64_t timestamp_eof = timestamp + sensor->readout_time_ns; - - // Update buffer and frame data - buf.cur_buf_idx = buf_idx; - buf.cur_frame_data = { - .frame_id = (uint32_t)(frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset), - .request_id = (uint32_t)request_id, - .timestamp_sof = timestamp, - .timestamp_eof = timestamp_eof, - .processing_time = float((nanos_since_boot() - timestamp_eof) * 1e-9) - }; - return true; -} - -bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) { - if (first_frame_synced) return true; - - // Store the frame data for this camera - camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1}; - - // Ensure all cameras are up - int enabled_camera_count = std::count_if(std::begin(ALL_CAMERA_CONFIGS), std::end(ALL_CAMERA_CONFIGS), - [](const auto &config) { return config.enabled; }); - bool all_cams_up = camera_sync_data.size() == enabled_camera_count; - - // Wait until the timestamps line up - bool all_cams_synced = true; - for (const auto &[_, sync_data] : camera_sync_data) { - uint64_t diff = std::max(timestamp, sync_data.timestamp) - - std::min(timestamp, sync_data.timestamp); - if (diff > 0.2*1e6) { // milliseconds - all_cams_synced = false; - } - } - - if (all_cams_up && all_cams_synced) { - first_frame_synced = true; - for (const auto&[cam, sync_data] : camera_sync_data) { - LOGW("camera %d synced on frame_id_offset %ld timestamp %lu", cam, sync_data.frame_id_offset, sync_data.timestamp); - } - } - - // Timeout in case the timestamps never line up - if (raw_id > 40) { - LOGE("camera first frame sync timed out"); - first_frame_synced = true; - } - - return false; -} diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h deleted file mode 100644 index 13cb13f98f6627..00000000000000 --- a/system/camerad/cameras/spectra.h +++ /dev/null @@ -1,219 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "media/cam_req_mgr.h" - -#include "common/util.h" -#include "common/swaglog.h" -#include "system/camerad/cameras/hw.h" -#include "system/camerad/cameras/camera_common.h" -#include "system/camerad/sensors/sensor.h" - -#define MAX_IFE_BUFS 20 - -const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py - -// For use with the Titan 170 ISP in the SDM845 -// https://github.com/commaai/agnos-kernel-sdm845 - -// CSLDeviceType/CSLPacketOpcodesIFE from camx -// cam_packet_header.op_code = (device << 24) | (opcode); -#define CSLDeviceTypeImageSensor (0x01 << 24) -#define CSLDeviceTypeIFE (0x0F << 24) -#define CSLDeviceTypeBPS (0x10 << 24) -#define OpcodesIFEInitialConfig 0x0 -#define OpcodesIFEUpdate 0x1 - -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); -int device_control(int fd, int op_code, int session_handle, int dev_handle); -int do_cam_control(int fd, int op_code, void *handle, int size); -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, - int mmu_hdl = 0, int mmu_hdl2 = 0); -void release(int video0_fd, uint32_t handle); - -class MemoryManager { -public: - void init(int _video0_fd) { video0_fd = _video0_fd; } - ~MemoryManager(); - - template - auto alloc(int len, uint32_t *handle) { - return std::unique_ptr>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); }); - } - -private: - void *alloc_buf(int len, uint32_t *handle); - void free(void *ptr); - - std::map handle_lookup; - std::map size_lookup; - std::map > cached_allocations; - int video0_fd; -}; - -class SpectraMaster { -public: - void init(); - - unique_fd video0_fd; - unique_fd cam_sync_fd; - unique_fd isp_fd; - unique_fd icp_fd; - int device_iommu = -1; - int cdm_iommu = -1; - int icp_device_iommu = -1; - MemoryManager mem_mgr; -}; - -class SpectraBuf { -public: - SpectraBuf() = default; - - ~SpectraBuf() { - if (video_fd >= 0 && ptr) { - munmap(ptr, mmap_size); - release(video_fd, handle); - } - } - - void init(SpectraMaster *m, int s, int a, bool shared_access, int mmu_hdl = 0, int mmu_hdl2 = 0, int count = 1) { - video_fd = m->video0_fd; - size = s; - alignment = a; - mmap_size = aligned_size() * count; - - uint32_t flags = CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE; - if (shared_access) { - flags |= CAM_MEM_FLAG_HW_SHARED_ACCESS; - } - - void *p = alloc_w_mmu_hdl(video_fd, mmap_size, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2); - ptr = (unsigned char*)p; - assert(ptr != NULL); - }; - - uint32_t aligned_size() { - return ALIGNED_SIZE(size, alignment); - }; - - int video_fd = -1; - unsigned char *ptr = nullptr; - int size = 0, alignment = 0, handle = 0, mmap_size = 0; -}; - -class SpectraCamera { -public: - SpectraCamera(SpectraMaster *master, const CameraConfig &config); - ~SpectraCamera(); - - void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx); - bool handle_camera_event(const cam_req_mgr_message *event_data); - void camera_close(); - void camera_map_bufs(); - void config_bps(int idx, int request_id); - void config_ife(int idx, int request_id, bool init=false); - - int clear_req_queue(); - void enqueue_frame(uint64_t request_id); - - int sensors_init(); - void sensors_start(); - void sensors_poke(int request_id); - void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); - - bool openSensor(); - void configISP(); - void configICP(); - void configCSIPHY(); - void linkDevices(); - void destroySyncObjectAt(int index); - - // *** state *** - - int ife_buf_depth = -1; - bool open = false; - bool enabled = true; - CameraConfig cc; - std::unique_ptr sensor; - - // YUV image size - uint32_t stride; - uint32_t y_height; - uint32_t uv_height; - uint32_t uv_offset; - uint32_t yuv_size; - - unique_fd sensor_fd; - unique_fd csiphy_fd; - - int32_t session_handle = -1; - int32_t sensor_dev_handle = -1; - int32_t isp_dev_handle = -1; - int32_t icp_dev_handle = -1; - int32_t csiphy_dev_handle = -1; - - int32_t link_handle = -1; - - SpectraBuf ife_cmd; - SpectraBuf ife_gamma_lut; - SpectraBuf ife_linearization_lut; - SpectraBuf ife_vignetting_lut; - - SpectraBuf bps_cmd; - SpectraBuf bps_cdm_buffer; - SpectraBuf bps_cdm_program_array; - SpectraBuf bps_cdm_striping_bl; - SpectraBuf bps_iq; - SpectraBuf bps_striping; - SpectraBuf bps_linearization_lut; - std::vector bps_lin_reg; - std::vector bps_ccm_reg; - - int buf_handle_yuv[MAX_IFE_BUFS] = {}; - int buf_handle_raw[MAX_IFE_BUFS] = {}; - int sync_objs_ife[MAX_IFE_BUFS] = {}; - int sync_objs_bps[MAX_IFE_BUFS] = {}; - uint64_t request_id_last = 0; - uint64_t last_requeue_ts = 0; - uint64_t frame_id_raw_last = 0; - int invalid_request_count = 0; - bool skip_expected = true; - - CameraBuf buf; - SpectraMaster *m; - -private: - void clearAndRequeue(uint64_t from_request_id); - bool validateEvent(uint64_t request_id, uint64_t frame_id_raw); - bool waitForFrameReady(uint64_t request_id); - bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp); - static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp); - struct SyncData { - uint64_t timestamp; - uint64_t frame_id_offset = 0; - }; - inline static std::map camera_sync_data; - inline static bool first_frame_synced = false; - - // a mode for stressing edge cases: realignment, sync failures, etc. - inline bool stress_test(std::string log) { - static double last_trigger = 0; - static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1")); - static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1")); - bool triggered = (prob > 0) && \ - ((static_cast(rand()) / RAND_MAX) < prob) && \ - (millis_since_boot() - last_trigger) > dt; - if (triggered) { - last_trigger = millis_since_boot(); - LOGE("stress test (cam %d): %s", cc.camera_num, log.c_str()); - } - return triggered; - } -}; diff --git a/system/camerad/imgproc/conv.cl b/system/camerad/imgproc/conv.cl new file mode 100644 index 00000000000000..a7115ae76cad7a --- /dev/null +++ b/system/camerad/imgproc/conv.cl @@ -0,0 +1,110 @@ +// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights +// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights + +// convert input rgb image to single channel then conv +__kernel void rgb2gray_conv2d( + const __global uchar * input, + __global short * output, + __constant short * filter, + __local uchar3 * cached +) +{ + const int rowOffset = get_global_id(1) * IMAGE_W; + const int my = get_global_id(0) + rowOffset; + + const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0); + const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen; + const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE; + + // cache local pixels + cached[ myLocal ].x = input[ my * 3 ]; // r + cached[ myLocal ].y = input[ my * 3 + 1]; // g + cached[ myLocal ].z = input[ my * 3 + 2]; // b + + // pad + if ( + get_global_id(0) < HALF_FILTER_SIZE || + get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 || + get_global_id(1) < HALF_FILTER_SIZE || + get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1 + ) + { + barrier(CLK_LOCAL_MEM_FENCE); + return; + } + else + { + int localColOffset = -1; + int globalColOffset = -1; + + // cache extra + if ( get_local_id(0) < HALF_FILTER_SIZE ) + { + localColOffset = get_local_id(0); + globalColOffset = -HALF_FILTER_SIZE; + + cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ]; + cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1]; + cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2]; + } + else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE ) + { + localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE; + globalColOffset = HALF_FILTER_SIZE; + + cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ]; + cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1]; + cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2]; + } + + + if ( get_local_id(1) < HALF_FILTER_SIZE ) + { + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ]; + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; + if (localColOffset > 0) + { + cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; + cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; + cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; + } + } + else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE ) + { + int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ]; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; + if (localColOffset > 0) + { + cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; + cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; + cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; + } + } + + // sync + barrier(CLK_LOCAL_MEM_FENCE); + + // perform convolution + int fIndex = 0; + short sum = 0; + + for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++) + { + int curRow = r * localRowLen; + for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++) + { + if (!FLIP_RB){ + // sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; + sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ]; + } else { + // sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; + sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ]; + } + } + } + output[my] = sum; + } +} \ No newline at end of file diff --git a/system/camerad/imgproc/pool.cl b/system/camerad/imgproc/pool.cl new file mode 100644 index 00000000000000..d674b5f363eda7 --- /dev/null +++ b/system/camerad/imgproc/pool.cl @@ -0,0 +1,34 @@ +// calculate variance in each subregion +__kernel void var_pool( + const __global char * input, + __global ushort * output // should not be larger than 128*128 so uint16 +) +{ + const int xidx = get_global_id(0) + ROI_X_MIN; + const int yidx = get_global_id(1) + ROI_Y_MIN; + + const int size = X_PITCH * Y_PITCH; + + float fsum = 0; + char mean, max; + + for (int i = 0; i < size; i++) { + int x_offset = i % X_PITCH; + int y_offset = i / X_PITCH; + fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]; + max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max; + } + + mean = convert_char_rte(fsum / size); + + float fvar = 0; + for (int i = 0; i < size; i++) { + int x_offset = i % X_PITCH; + int y_offset = i / X_PITCH; + fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean); + } + + fvar = fvar / size; + + output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max)); +} \ No newline at end of file diff --git a/system/camerad/imgproc/utils.cc b/system/camerad/imgproc/utils.cc new file mode 100644 index 00000000000000..a7bbeb9e8673c4 --- /dev/null +++ b/system/camerad/imgproc/utils.cc @@ -0,0 +1,106 @@ +#include "system/camerad/imgproc/utils.h" + +#include +#include +#include +#include +#include + +const int16_t lapl_conv_krnl[9] = {0, 1, 0, + 1, -4, 1, + 0, 1, 0}; + +// calculate score based on laplacians in one area +uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) { + const int size = x_pitch * y_pitch; + // avg and max of roi + int16_t max = 0; + int sum = 0; + for (int i = 0; i < size; ++i) { + const int16_t v = lap[i]; + sum += v; + if (v > max) max = v; + } + + const int16_t mean = sum / size; + + // var of roi + int var = 0; + for (int i = 0; i < size; ++i) { + var += std::pow(lap[i] - mean, 2); + } + + const float fvar = (float)var / size; + return std::min(5 * fvar + max, (float)65535); +} + +bool is_blur(const uint16_t *lapmap, const size_t size) { + float bad_sum = 0; + for (int i = 0; i < size; i++) { + if (lapmap[i] < LM_THRESH) { + bad_sum += 1 / (float)size; + } + } + return (bad_sum > LM_PREC_THRESH); +} + +static cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) { + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d " + "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d", + image_w, image_h, 1, + filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w); + return cl_program_from_file(context, device_id, "imgproc/conv.cl", args); +} + +LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size) + : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride), + roi_buf(width * height * 3), result_buf(width * height) { + + prg = build_conv_program(device_id, ctx, width, height, filter_size); + krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb2gray_conv2d", &err)); + // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter + roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, roi_buf.size() * sizeof(roi_buf[0]), NULL, &err)); + result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, result_buf.size() * sizeof(result_buf[0]), NULL, &err)); + filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + 9 * sizeof(int16_t), (void *)&lapl_conv_krnl, &err)); +} + +LapConv::~LapConv() { + CL_CHECK(clReleaseMemObject(roi_cl)); + CL_CHECK(clReleaseMemObject(result_cl)); + CL_CHECK(clReleaseMemObject(filter_cl)); + CL_CHECK(clReleaseKernel(krnl)); + CL_CHECK(clReleaseProgram(prg)); +} + +uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id) { + // sharpness scores + const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1); + const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1); + + const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3; + for (int i = 0; i < height; ++i) { + memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3); + } + + constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t)); + const size_t global_work_size[] = {(size_t)width, (size_t)height}; + const size_t local_work_size[] = {CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE}; + + CL_CHECK(clEnqueueWriteBuffer(q, roi_cl, CL_TRUE, 0, roi_buf.size() * sizeof(roi_buf[0]), roi_buf.data(), 0, 0, 0)); + CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), (void *)&roi_cl)); + CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), (void *)&result_cl)); + CL_CHECK(clSetKernelArg(krnl, 2, sizeof(cl_mem), (void *)&filter_cl)); + CL_CHECK(clSetKernelArg(krnl, 3, local_mem_size, 0)); + cl_event conv_event; + CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, global_work_size, local_work_size, 0, 0, &conv_event)); + CL_CHECK(clWaitForEvents(1, &conv_event)); + CL_CHECK(clReleaseEvent(conv_event)); + CL_CHECK(clEnqueueReadBuffer(q, result_cl, CL_TRUE, 0, + result_buf.size() * sizeof(result_buf[0]), result_buf.data(), 0, 0, 0)); + + return get_lapmap_one(result_buf.data(), width, height); +} diff --git a/system/camerad/imgproc/utils.h b/system/camerad/imgproc/utils.h new file mode 100644 index 00000000000000..94323b15c5da1e --- /dev/null +++ b/system/camerad/imgproc/utils.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +#include "common/clutil.h" + +#define NUM_SEGMENTS_X 8 +#define NUM_SEGMENTS_Y 6 + +#define ROI_X_MIN 1 +#define ROI_X_MAX 6 +#define ROI_Y_MIN 2 +#define ROI_Y_MAX 3 + +#define LM_THRESH 120 +#define LM_PREC_THRESH 0.9 // 90 perc is blur +#define CONV_LOCAL_WORKSIZE 16 + +class LapConv { +public: + LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size); + ~LapConv(); + uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id); + +private: + cl_mem roi_cl, result_cl, filter_cl; + cl_program prg; + cl_kernel krnl; + const int width, height; + const int rgb_stride; + std::vector roi_buf; + std::vector result_buf; +}; + +bool is_blur(const uint16_t *lapmap, const size_t size); diff --git a/third_party/linux/include/media/cam_cpas.h b/system/camerad/include/media/cam_cpas.h similarity index 100% rename from third_party/linux/include/media/cam_cpas.h rename to system/camerad/include/media/cam_cpas.h diff --git a/third_party/linux/include/media/cam_defs.h b/system/camerad/include/media/cam_defs.h similarity index 100% rename from third_party/linux/include/media/cam_defs.h rename to system/camerad/include/media/cam_defs.h diff --git a/third_party/linux/include/media/cam_fd.h b/system/camerad/include/media/cam_fd.h similarity index 100% rename from third_party/linux/include/media/cam_fd.h rename to system/camerad/include/media/cam_fd.h diff --git a/third_party/linux/include/media/cam_icp.h b/system/camerad/include/media/cam_icp.h similarity index 100% rename from third_party/linux/include/media/cam_icp.h rename to system/camerad/include/media/cam_icp.h diff --git a/third_party/linux/include/media/cam_isp.h b/system/camerad/include/media/cam_isp.h similarity index 100% rename from third_party/linux/include/media/cam_isp.h rename to system/camerad/include/media/cam_isp.h diff --git a/third_party/linux/include/media/cam_isp_ife.h b/system/camerad/include/media/cam_isp_ife.h similarity index 100% rename from third_party/linux/include/media/cam_isp_ife.h rename to system/camerad/include/media/cam_isp_ife.h diff --git a/third_party/linux/include/media/cam_isp_vfe.h b/system/camerad/include/media/cam_isp_vfe.h similarity index 100% rename from third_party/linux/include/media/cam_isp_vfe.h rename to system/camerad/include/media/cam_isp_vfe.h diff --git a/third_party/linux/include/media/cam_jpeg.h b/system/camerad/include/media/cam_jpeg.h similarity index 100% rename from third_party/linux/include/media/cam_jpeg.h rename to system/camerad/include/media/cam_jpeg.h diff --git a/third_party/linux/include/media/cam_lrme.h b/system/camerad/include/media/cam_lrme.h similarity index 100% rename from third_party/linux/include/media/cam_lrme.h rename to system/camerad/include/media/cam_lrme.h diff --git a/third_party/linux/include/media/cam_req_mgr.h b/system/camerad/include/media/cam_req_mgr.h similarity index 100% rename from third_party/linux/include/media/cam_req_mgr.h rename to system/camerad/include/media/cam_req_mgr.h diff --git a/third_party/linux/include/media/cam_sensor.h b/system/camerad/include/media/cam_sensor.h similarity index 100% rename from third_party/linux/include/media/cam_sensor.h rename to system/camerad/include/media/cam_sensor.h diff --git a/third_party/linux/include/media/cam_sensor_cmn_header.h b/system/camerad/include/media/cam_sensor_cmn_header.h similarity index 100% rename from third_party/linux/include/media/cam_sensor_cmn_header.h rename to system/camerad/include/media/cam_sensor_cmn_header.h diff --git a/third_party/linux/include/media/cam_sync.h b/system/camerad/include/media/cam_sync.h similarity index 100% rename from third_party/linux/include/media/cam_sync.h rename to system/camerad/include/media/cam_sync.h diff --git a/third_party/linux/include/msm_cam_sensor.h b/system/camerad/include/msm_cam_sensor.h similarity index 100% rename from third_party/linux/include/msm_cam_sensor.h rename to system/camerad/include/msm_cam_sensor.h diff --git a/third_party/linux/include/msm_camsensor_sdk.h b/system/camerad/include/msm_camsensor_sdk.h similarity index 100% rename from third_party/linux/include/msm_camsensor_sdk.h rename to system/camerad/include/msm_camsensor_sdk.h diff --git a/third_party/linux/include/msmb_camera.h b/system/camerad/include/msmb_camera.h similarity index 100% rename from third_party/linux/include/msmb_camera.h rename to system/camerad/include/msmb_camera.h diff --git a/third_party/linux/include/msmb_isp.h b/system/camerad/include/msmb_isp.h similarity index 100% rename from third_party/linux/include/msmb_isp.h rename to system/camerad/include/msmb_isp.h diff --git a/third_party/linux/include/msmb_ispif.h b/system/camerad/include/msmb_ispif.h similarity index 100% rename from third_party/linux/include/msmb_ispif.h rename to system/camerad/include/msmb_ispif.h diff --git a/system/camerad/main.cc b/system/camerad/main.cc index 05fdef1a65c16a..35a3329f300cdd 100644 --- a/system/camerad/main.cc +++ b/system/camerad/main.cc @@ -4,10 +4,18 @@ #include "common/params.h" #include "common/util.h" +#include "system/hardware/hw.h" int main(int argc, char *argv[]) { - // doesn't need RT priority since we're using isolcpus - int ret = util::set_core_affinity({6}); + if (Hardware::PC()) { + printf("camerad is not meant to run on PC\n"); + return 0; + } + + int ret; + ret = util::set_realtime_priority(53); + assert(ret == 0); + ret = util::set_core_affinity({6}); assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores camerad_thread(); diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc deleted file mode 100644 index 62c26ca80973e0..00000000000000 --- a/system/camerad/sensors/os04c10.cc +++ /dev/null @@ -1,146 +0,0 @@ -#include - -#include "system/camerad/sensors/sensor.h" -#include "third_party/linux/include/msm_camsensor_sdk.h" - -namespace { - -const float sensor_analog_gains_OS04C10[] = { - 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, - 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, - 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, - 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, - 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; - -const uint32_t os04c10_analog_gains_reg[] = { - 0x080, 0x088, 0x090, 0x098, 0x0A0, 0x0A8, 0x0B0, 0x0B8, 0x0C0, 0x0C8, 0x0D8, - 0x0E8, 0x0F8, 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, - 0x190, 0x1B0, 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, - 0x2E0, 0x300, 0x320, 0x340, 0x380, 0x3C0, 0x400, 0x440, 0x480, 0x4C0, 0x500, - 0x540, 0x580, 0x5C0, 0x600, 0x640, 0x680, 0x6C0, 0x700, 0x740, 0x780, 0x7C0}; - -} // namespace - -void OS04C10::ife_downscale_configure() { - out_scale = 2; - - pixel_size_mm = 0.002; - frame_width = 2688; - frame_height = 1520; - exposure_time_max = 2352; - - init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10)); -} - -OS04C10::OS04C10() { - image_sensor = cereal::FrameData::ImageSensor::OS04C10; - bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG; - pixel_size_mm = 0.004; - data_word = false; - - // hdr_offset = 64 * 2 + 8; // stagger - frame_width = 1344; - frame_height = 760; //760 * 2 + hdr_offset; - frame_stride = (frame_width * 12 / 8); // no alignment - - extra_height = 0; - frame_offset = 0; - - start_reg_array.assign(std::begin(start_reg_array_os04c10), std::end(start_reg_array_os04c10)); - init_reg_array.assign(std::begin(init_array_os04c10), std::end(init_array_os04c10)); - probe_reg_addr = 0x300a; - probe_expected_data = 0x5304; - bits_per_pixel = 12; - mipi_format = CAM_FORMAT_MIPI_RAW_12; - frame_data_type = CSI_RAW12; - mclk_frequency = 24000000; // Hz - - // TODO: this was set from logs. actually calculate it out - readout_time_ns = 11000000; - - ev_scale = 150.0; - dc_gain_factor = 1; - dc_gain_min_weight = 1; // always on is fine - dc_gain_max_weight = 1; - dc_gain_on_grey = 0.9; - dc_gain_off_grey = 1.0; - exposure_time_min = 2; - exposure_time_max = 1684; - analog_gain_min_idx = 0x0; - analog_gain_rec_idx = 0x0; // 1x - analog_gain_max_idx = 0x28; - analog_gain_cost_delta = -1; - analog_gain_cost_low = 0.4; - analog_gain_cost_high = 6.4; - for (int i = 0; i <= analog_gain_max_idx; i++) { - sensor_analog_gains[i] = sensor_analog_gains_OS04C10[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 = 0.01; - - black_level = 48; - color_correct_matrix = { - 0x000000c2, 0x00000fe0, 0x00000fde, - 0x00000fa7, 0x000000d9, 0x00001000, - 0x00000fca, 0x00000fef, 0x000000c7, - }; - for (int i = 0; i < 65; i++) { - float fx = i / 64.0; - gamma_lut_rgb.push_back((uint32_t)((10*fx)/(1+9*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 = { - 0x01064832, 0x00da26d1, 0x00bb25d9, 0x00aac556, 0x00a06503, 0x009a64d3, 0x009744ba, 0x009744ba, 0x009a24d1, 0x00a00500, 0x00aa2551, 0x00ba45d2, 0x00d826c1, 0x01040820, 0x013729b9, 0x0171ab8d, 0x01b36d9b, - 0x00eee777, 0x00c2c616, 0x00ae2571, 0x009fe4ff, 0x0096e4b7, 0x0090e487, 0x008d446a, 0x008d2469, 0x0090a485, 0x009684b4, 0x009f64fb, 0x00ad456a, 0x00c1a60d, 0x00eca765, 0x011fc8fe, 0x015a4ad2, 0x019c0ce0, - 0x00dee6f7, 0x00b9c5ce, 0x00a5652b, 0x009964cb, 0x00904482, 0x00892449, 0x0085842c, 0x0085642b, 0x0088e447, 0x008fe47f, 0x0098e4c7, 0x00a4c526, 0x00b8a5c5, 0x00dc86e4, 0x010fc87e, 0x014a2a51, 0x018c0c60, - 0x00d626b1, 0x00b4e5a7, 0x00a1e50f, 0x0095e4af, 0x008c2461, 0x00850428, 0x0081640b, 0x0081440a, 0x0084a425, 0x008ba45d, 0x009564ab, 0x00a1450a, 0x00b3c59e, 0x00d3e69f, 0x01070838, 0x01418a0c, 0x01834c1a, - 0x00d4c6a6, 0x00b425a1, 0x00a1450a, 0x009544aa, 0x008b645b, 0x00844422, 0x0080a405, 0x0080a405, 0x00840420, 0x008b0458, 0x0094c4a6, 0x00a0a505, 0x00b30598, 0x00d26693, 0x0105a82d, 0x01402a01, 0x0181ec0f, - 0x00daa6d5, 0x00b765bb, 0x00a3c51e, 0x0097a4bd, 0x008e4472, 0x00872439, 0x0083841c, 0x0083641b, 0x0086e437, 0x008de46f, 0x009724b9, 0x00a30518, 0x00b665b3, 0x00d866c3, 0x010b885c, 0x01460a30, 0x0187ec3f, - 0x00e80740, 0x00bec5f6, 0x00aa6553, 0x009d24e9, 0x009404a0, 0x008d846c, 0x0089e44f, 0x0089e44f, 0x008d446a, 0x0093c49e, 0x009ca4e5, 0x00a9854c, 0x00bdc5ee, 0x00e5a72d, 0x0118c8c6, 0x01534a9a, 0x01952ca9, - 0x00fca7e5, 0x00d06683, 0x00b5c5ae, 0x00a5852c, 0x009c84e4, 0x009664b3, 0x0093649b, 0x0093449a, 0x009624b1, 0x009c24e1, 0x00a50528, 0x00b4e5a7, 0x00ce8674, 0x00fa47d2, 0x012d696b, 0x0167eb3f, 0x01a9cd4e, - 0x011888c4, 0x00ec6763, 0x00c7863c, 0x00b4e5a7, 0x00a8a545, 0x00a1c50e, 0x009ec4f6, 0x009ea4f5, 0x00a1a50d, 0x00a82541, 0x00b445a2, 0x00c5e62f, 0x00ea6753, 0x011648b2, 0x01496a4b, 0x0183ec1f, 0x01c5ae2d, - 0x013bc9de, 0x010fa87d, 0x00eac756, 0x00cd466a, 0x00bc25e1, 0x00b405a0, 0x00afc57e, 0x00afa57d, 0x00b3a59d, 0x00bbc5de, 0x00cc0660, 0x00e92749, 0x010da86d, 0x013989cc, 0x016cab65, 0x01a72d39, 0x01e8ef47, - 0x01666b33, 0x013a49d2, 0x011568ab, 0x00f7e7bf, 0x00e1c70e, 0x00d2e697, 0x00cb665b, 0x00cb2659, 0x00d26693, 0x00e0c706, 0x00f6a7b5, 0x0113c89e, 0x013849c2, 0x01642b21, 0x01974cba, 0x01d1ce8e, 0x0213909c, - 0x01986cc3, 0x016c2b61, 0x01476a3b, 0x0129e94f, 0x0113a89d, 0x0104c826, 0x00fd47ea, 0x00fd27e9, 0x01044822, 0x0112c896, 0x0128a945, 0x0145ca2e, 0x016a4b52, 0x01960cb0, 0x01c92e49, 0x0203b01d, 0x0245922c, - 0x01d1ae8d, 0x01a58d2c, 0x0180ac05, 0x01632b19, 0x014cea67, 0x013e29f1, 0x013689b4, 0x013669b3, 0x013d89ec, 0x014c0a60, 0x0161eb0f, 0x017f0bf8, 0x01a38d1c, 0x01cf4e7a, 0x02029014, 0x023d11e8, 0x027ed3f6, - }; -} - -std::vector OS04C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { - uint32_t long_time = exposure_time; - uint32_t real_gain = os04c10_analog_gains_reg[new_exp_g]; - - return { - {0x3501, long_time>>8}, {0x3502, long_time&0xFF}, - {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, - {0x350c, real_gain>>8}, {0x350d, real_gain&0xFF}, - }; -} - -int OS04C10::getSlaveAddress(int port) const { - assert(port >= 0 && port <= 2); - return (int[]){0x6C, 0x20, 0x6C}[port]; -} - -float OS04C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { - float score = std::abs(desired_ev - (exp_t * exp_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; - score += ((1 - analog_gain_cost_delta) + - analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * - std::abs(exp_g_idx - gain_idx) * 3.0; - return score; -} diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h deleted file mode 100644 index 28d6b3310c59d6..00000000000000 --- a/system/camerad/sensors/os04c10_registers.h +++ /dev/null @@ -1,352 +0,0 @@ -#pragma once - -const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}}; -const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}}; - -const struct i2c_random_wr_payload init_array_os04c10[] = { - // baseed on DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE - {0x0103, 0x01}, // software reset - - // PLL + clocks - {0x0301, 0xe4}, - {0x0303, 0x01}, - {0x0305, 0xb6}, - {0x0306, 0x01}, - {0x0307, 0x17}, - {0x0323, 0x04}, - {0x0324, 0x01}, - {0x0325, 0x62}, - - {0x3012, 0x06}, - {0x3013, 0x02}, - {0x3016, 0x72}, - {0x3021, 0x03}, - {0x3106, 0x21}, - {0x3107, 0xa1}, - - // Analog/timing fine-tuning block - {0x3624, 0x00}, - {0x3625, 0x4c}, - {0x3660, 0x04}, - {0x3666, 0xa5}, - {0x3667, 0xa5}, - {0x366a, 0x50}, - {0x3673, 0x0d}, - {0x3672, 0x0d}, - {0x3671, 0x0d}, - {0x3670, 0x0d}, - {0x3685, 0x00}, - {0x3694, 0x0d}, - {0x3693, 0x0d}, - {0x3692, 0x0d}, - {0x3691, 0x0d}, - {0x3696, 0x4c}, - {0x3697, 0x4c}, - {0x3698, 0x00}, - {0x3699, 0x80}, - {0x369a, 0x80}, - {0x369b, 0x1f}, - {0x369c, 0x1f}, - {0x369d, 0x80}, - {0x369e, 0x40}, - {0x369f, 0x21}, - {0x36a0, 0x12}, - {0x36a1, 0xdd}, - {0x36a2, 0x66}, - {0x370a, 0x02}, - {0x370e, 0x00}, - {0x3710, 0x00}, - {0x3713, 0x04}, - {0x3725, 0x02}, - {0x372a, 0x03}, - {0x3738, 0xce}, - {0x3748, 0x02}, - {0x374a, 0x02}, - {0x374c, 0x02}, - {0x374e, 0x02}, - {0x3756, 0x00}, - {0x3757, 0x00}, - {0x3767, 0x00}, - {0x3771, 0x00}, - {0x377b, 0x28}, - {0x377c, 0x00}, - {0x377d, 0x0c}, - {0x3781, 0x03}, - {0x3782, 0x00}, - {0x3789, 0x14}, - {0x3795, 0x02}, - {0x379c, 0x00}, - {0x379d, 0x00}, - {0x37b8, 0x04}, - {0x37ba, 0x03}, - {0x37bb, 0x00}, - {0x37bc, 0x04}, - {0x37be, 0x26}, - {0x37c4, 0x11}, - {0x37c5, 0x80}, - {0x37c6, 0x14}, - {0x37c7, 0xa8}, - {0x37da, 0x11}, - {0x381f, 0x08}, - {0x3881, 0x00}, - {0x3888, 0x04}, - {0x388b, 0x00}, - {0x3c80, 0x10}, - {0x3c86, 0x00}, - {0x3c8c, 0x20}, - {0x3c9f, 0x01}, - {0x3d85, 0x1b}, - {0x3d8c, 0x71}, - {0x3d8d, 0xe2}, - {0x3f00, 0x0b}, - {0x3f06, 0x04}, - - // BLC - black level correction - {0x400a, 0x01}, - {0x400b, 0x50}, - {0x400e, 0x08}, - {0x4043, 0x7e}, - {0x4045, 0x7e}, - {0x4047, 0x7e}, - {0x4049, 0x7e}, - {0x4090, 0x04}, - {0x40b0, 0x00}, - {0x40b1, 0x00}, - {0x40b2, 0x00}, - {0x40b3, 0x00}, - {0x40b4, 0x00}, - {0x40b5, 0x00}, - {0x40b7, 0x00}, - {0x40b8, 0x00}, - {0x40b9, 0x00}, - {0x40ba, 0x01}, - - {0x4301, 0x00}, - {0x4303, 0x00}, - {0x4502, 0x04}, - {0x4503, 0x00}, - {0x4504, 0x06}, - {0x4506, 0x00}, - {0x4507, 0x47}, - {0x4803, 0x00}, - {0x480c, 0x32}, - {0x480e, 0x04}, - {0x4813, 0xe4}, - {0x4819, 0x70}, - {0x481f, 0x30}, - {0x4823, 0x3f}, - {0x4825, 0x30}, - {0x4833, 0x10}, - {0x484b, 0x27}, - {0x488b, 0x00}, - {0x4d00, 0x04}, - {0x4d01, 0xad}, - {0x4d02, 0xbc}, - {0x4d03, 0xa1}, - {0x4d04, 0x1f}, - {0x4d05, 0x4c}, - {0x4d0b, 0x01}, - {0x4e00, 0x2a}, - {0x4e0d, 0x00}, - - // ISP - {0x5001, 0x09}, - {0x5004, 0x00}, - {0x5080, 0x04}, - {0x5036, 0x80}, - {0x5180, 0x70}, - {0x5181, 0x10}, - - // DPC - defective pixel correction - {0x520a, 0x03}, - {0x520b, 0x06}, - {0x520c, 0x0c}, - - {0x580b, 0x0f}, - {0x580d, 0x00}, - {0x580f, 0x00}, - {0x5820, 0x00}, - {0x5821, 0x00}, - - {0x301c, 0xf8}, - {0x301e, 0xb4}, - {0x301f, 0xf0}, - {0x3022, 0x61}, - {0x3109, 0xe7}, - {0x3600, 0x00}, - {0x3610, 0x65}, - {0x3611, 0x85}, - {0x3613, 0x3a}, - {0x3615, 0x60}, - {0x3621, 0xb0}, - {0x3620, 0x0c}, - {0x3629, 0x00}, - {0x3661, 0x04}, - {0x3664, 0x70}, - {0x3665, 0x00}, - {0x3681, 0x80}, - {0x3682, 0x40}, - {0x3683, 0x21}, - {0x3684, 0x12}, - {0x3700, 0x2a}, - {0x3701, 0x12}, - {0x3703, 0x28}, - {0x3704, 0x0e}, - {0x3706, 0x9d}, - {0x3709, 0x4a}, - {0x370b, 0x48}, - {0x370c, 0x01}, - {0x370f, 0x00}, - {0x3714, 0x28}, - {0x3716, 0x04}, - {0x3719, 0x11}, - {0x371a, 0x1e}, - {0x3720, 0x00}, - {0x3724, 0x13}, - {0x373f, 0xb0}, - {0x3741, 0x9d}, - {0x3743, 0x9d}, - {0x3745, 0x9d}, - {0x3747, 0x9d}, - {0x3749, 0x48}, - {0x374b, 0x48}, - {0x374d, 0x48}, - {0x374f, 0x48}, - {0x3755, 0x10}, - {0x376c, 0x00}, - {0x378d, 0x3c}, - {0x3790, 0x01}, - {0x3791, 0x01}, - {0x3798, 0x40}, - {0x379e, 0x00}, - {0x379f, 0x04}, - {0x37a1, 0x10}, - {0x37a2, 0x1e}, - {0x37a8, 0x10}, - {0x37a9, 0x1e}, - {0x37ac, 0xa0}, - {0x37b9, 0x01}, - {0x37bd, 0x01}, - {0x37bf, 0x26}, - {0x37c0, 0x11}, - {0x37c2, 0x14}, - {0x37cd, 0x19}, - {0x37e0, 0x08}, - {0x37e6, 0x04}, - {0x37e5, 0x02}, - {0x37e1, 0x0c}, - {0x3737, 0x04}, - {0x37d8, 0x02}, - {0x37e2, 0x10}, - {0x3739, 0x10}, - {0x3662, 0x08}, - {0x37e4, 0x20}, - {0x37e3, 0x08}, - {0x37d9, 0x04}, - {0x4040, 0x00}, - {0x4041, 0x03}, - {0x4008, 0x01}, - {0x4009, 0x06}, - - // FSIN - frame sync - {0x3002, 0x22}, - {0x3663, 0x22}, - {0x368a, 0x04}, - {0x3822, 0x44}, - {0x3823, 0x00}, - {0x3829, 0x03}, - {0x3832, 0xf8}, - {0x382c, 0x00}, - {0x3844, 0x06}, - {0x3843, 0x00}, - {0x382a, 0x00}, - {0x382b, 0x0c}, - - // 2704x1536 -> 2688x1520 out - {0x3800, 0x00}, {0x3801, 0x00}, - {0x3802, 0x00}, {0x3803, 0x00}, - {0x3804, 0x0a}, {0x3805, 0x8f}, - {0x3806, 0x05}, {0x3807, 0xff}, - {0x3808, 0x05}, {0x3809, 0x40}, - {0x380a, 0x02}, {0x380b, 0xf8}, - {0x3811, 0x08}, - {0x3813, 0x08}, - {0x3814, 0x03}, - {0x3815, 0x01}, - {0x3816, 0x03}, - {0x3817, 0x01}, - - {0x380c, 0x0b}, {0x380d, 0xac}, // HTS (line length) - {0x380e, 0x06}, {0x380f, 0x9c}, // VTS (frame length) - - {0x3820, 0xb3}, - {0x3821, 0x01}, - {0x3880, 0x00}, - {0x3882, 0x20}, - {0x3c91, 0x0b}, - {0x3c94, 0x45}, - {0x3cad, 0x00}, - {0x3cae, 0x00}, - {0x4000, 0xf3}, - {0x4001, 0x60}, - {0x4003, 0x40}, - {0x4300, 0xff}, - {0x4302, 0x0f}, - {0x4305, 0x83}, - {0x4505, 0x84}, - {0x4809, 0x0e}, - {0x480a, 0x04}, - {0x4837, 0x15}, - {0x4c00, 0x08}, - {0x4c01, 0x08}, - {0x4c04, 0x00}, - {0x4c05, 0x00}, - {0x5000, 0xf9}, - // {0x0100, 0x01}, - // {0x320d, 0x00}, - // {0x3208, 0xa0}, - - // initialize exposure - {0x3503, 0x88}, - - // long exposure - {0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x10}, - {0x3508, 0x00}, {0x3509, 0x80}, - {0x350a, 0x04}, {0x350b, 0x00}, - - // short exposure - {0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x40}, - {0x350c, 0x00}, {0x350d, 0x80}, - {0x350e, 0x04}, {0x350f, 0x00}, - - // white balance - // b - {0x5100, 0x06}, {0x5101, 0x7e}, - {0x5140, 0x06}, {0x5141, 0x7e}, - // g - {0x5102, 0x04}, {0x5103, 0x00}, - {0x5142, 0x04}, {0x5143, 0x00}, - // r - {0x5104, 0x08}, {0x5105, 0xd6}, - {0x5144, 0x08}, {0x5145, 0xd6}, -}; - -const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = { - // based on OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz - {0x3c8c, 0x40}, - {0x3714, 0x24}, - {0x37c2, 0x04}, - {0x3662, 0x10}, - {0x37d9, 0x08}, - {0x4041, 0x07}, - {0x4008, 0x02}, - {0x4009, 0x0d}, - {0x3808, 0x0a}, {0x3809, 0x80}, - {0x380a, 0x05}, {0x380b, 0xf0}, - {0x3814, 0x01}, - {0x3816, 0x01}, - {0x380c, 0x08}, {0x380d, 0x5c}, // HTS - {0x380e, 0x09}, {0x380f, 0x38}, // VTS - {0x3820, 0xb0}, - {0x3821, 0x00}, -}; diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc deleted file mode 100644 index 05d58f03c60209..00000000000000 --- a/system/camerad/sensors/ox03c10.cc +++ /dev/null @@ -1,142 +0,0 @@ -#include - -#include "system/camerad/sensors/sensor.h" -#include "third_party/linux/include/msm_camsensor_sdk.h" - -namespace { - -const float sensor_analog_gains_OX03C10[] = { - 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, - 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, - 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, - 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, - 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; - -const uint32_t ox03c10_analog_gains_reg[] = { - 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, - 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, - 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, - 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, - 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; - -const uint32_t VS_TIME_MIN_OX03C10 = 1; -const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 - -} // namespace - -OX03C10::OX03C10() { - image_sensor = cereal::FrameData::ImageSensor::OX03C10; - bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR; - pixel_size_mm = 0.003; - data_word = false; - frame_width = 1928; - frame_height = 1208; - frame_stride = (frame_width * 12 / 8) + 4; - extra_height = 16; // top 2 + bot 14 - frame_offset = 2; - - start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10)); - init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10)); - probe_reg_addr = 0x300a; - probe_expected_data = 0x5803; - bits_per_pixel = 12; - mipi_format = CAM_FORMAT_MIPI_RAW_12; - frame_data_type = CSI_RAW12; - mclk_frequency = 24000000; // Hz - - readout_time_ns = 14697000; - - dc_gain_factor = 7.32; - dc_gain_min_weight = 1; // always on is fine - dc_gain_max_weight = 1; - dc_gain_on_grey = 0.9; - dc_gain_off_grey = 1.0; - exposure_time_min = 2; // 1x - exposure_time_max = 2016; - analog_gain_min_idx = 0x0; - analog_gain_rec_idx = 0x0; // 1x - analog_gain_max_idx = 0x36; - analog_gain_cost_delta = -1; - analog_gain_cost_low = 0.4; - analog_gain_cost_high = 6.4; - for (int i = 0; i <= analog_gain_max_idx; i++) { - sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; - } - min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * 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 = 0.01; - - black_level = 0; - color_correct_matrix = { - 0x000000b6, 0x00000ff1, 0x00000fda, - 0x00000fcc, 0x000000b9, 0x00000ffb, - 0x00000fc2, 0x00000ff6, 0x000000c9, - }; - for (int i = 0; i < 65; i++) { - float fx = i / 64.0; - fx = -0.507089*exp(-12.54124638*fx) + 0.9655*pow(fx, 0.5) - 0.472597*fx + 0.507089; - gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5)); - } - prepare_gamma_lut(); - linearization_lut = { - 0x00200000, 0x00200000, 0x00200000, 0x00200000, - 0x00404080, 0x00404080, 0x00404080, 0x00404080, - 0x00804100, 0x00804100, 0x00804100, 0x00804100, - 0x02014402, 0x02014402, 0x02014402, 0x02014402, - 0x0402c804, 0x0402c804, 0x0402c804, 0x0402c804, - 0x0805d00a, 0x0805d00a, 0x0805d00a, 0x0805d00a, - 0x100ba015, 0x100ba015, 0x100ba015, 0x100ba015, - 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, - 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, - }; - linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x1fff23ff, 0x27ff3fff}; - 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 OX03C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { - // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD - uint32_t hcg_time = exposure_time; - uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); - uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); - - uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; - - return { - {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, - {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, - {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, - {0x35c2, vs_time&0xFF}, - - {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, - }; -} - -int OX03C10::getSlaveAddress(int port) const { - assert(port >= 0 && port <= 2); - return (int[]){0x6C, 0x20, 0x6C}[port]; -} - -float OX03C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { - float score = std::abs(desired_ev - (exp_t * exp_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; - score += ((1 - analog_gain_cost_delta) + - analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * - std::abs(exp_g_idx - gain_idx) * 5.0; - return score; -} diff --git a/system/camerad/sensors/ox03c10_registers.h b/system/camerad/sensors/ox03c10_registers.h deleted file mode 100644 index bb7a1c5dd63f6e..00000000000000 --- a/system/camerad/sensors/ox03c10_registers.h +++ /dev/null @@ -1,751 +0,0 @@ -#pragma once - -const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; -const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; - -const struct i2c_random_wr_payload init_array_ox03c10[] = { - {0x103, 1}, - {0x107, 1}, - - // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200 - - // TPM - {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf}, - - /*) - // group 4 - {0x3208, 0x04}, - {0x4620, 0x04}, - {0x3208, 0x14}, - - // group 5 - {0x3208, 0x05}, - {0x4620, 0x04}, - {0x3208, 0x15}, - - // group 2 - {0x3208, 0x02}, - {0x3507, 0x00}, - {0x3208, 0x12}, - - // delay launch group 2 - {0x3208, 0xa2},*/ - - // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config - // PLL setup - {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix - {0x0303, 0x01}, // pll1_prediv - {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300 - {0x0306, 0x04}, // pll1_divmipi = 4 - {0x0307, 0x01}, // pll1_divm = 1 - {0x0316, 0x00}, - {0x0317, 0x00}, - {0x0318, 0x00}, - {0x0323, 0x05}, // pll2_prediv - {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300 - - // SCLK/PCLK - {0x0400, 0xe0}, {0x0401, 0x80}, - {0x0403, 0xde}, {0x0404, 0x34}, - {0x0405, 0x3b}, {0x0406, 0xde}, - {0x0407, 0x08}, - {0x0408, 0xe0}, {0x0409, 0x7f}, - {0x040a, 0xde}, {0x040b, 0x34}, - {0x040c, 0x47}, {0x040d, 0xd8}, - {0x040e, 0x08}, - - // xchk - {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79}, - - // SC ctrl - {0x3001, 0x03}, // io_pad_oen - {0x3002, 0xfc}, // io_pad_oen - {0x3005, 0x80}, // io_pad_out - {0x3007, 0x01}, // io_pad_sel - {0x3008, 0x80}, // io_pad_sel - - // FSIN (frame sync) with external pulses - {0x3009, 0x2}, - {0x3015, 0x2}, - {0x383E, 0x80}, - {0x3881, 0x4}, - {0x3882, 0x8}, {0x3883, 0x0D}, - {0x3836, 0x1F}, {0x3837, 0x40}, - - // causes issues on some devices - //{0x3822, 0x33}, // wait for pulse before first frame - - {0x3892, 0x44}, - {0x3823, 0x41}, - - {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI - {0x3020, 0x05}, // SC_CTRL_20 - - // this is not in the datasheet, listed as RSVD - // but the camera doesn't work without it - {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23}, - {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d}, - {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96}, - {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73}, - {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36}, - {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21}, - {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28}, - {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c}, - {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13}, - {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02}, - {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00}, - {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e}, - {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02}, - {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00}, - {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00}, - {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00}, - {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19}, - - // also RSVD - {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01}, - {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21}, - {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3}, - {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03}, - {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80}, - {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c}, - {0x3c73, 0x2c}, {0x3c76, 0x12}, - - // PEC checks - {0x3182, 0x12}, - - {0x320e, 0x00}, {0x320f, 0x00}, // RSVD - {0x3211, 0x61}, - {0x3215, 0xcd}, - {0x3219, 0x08}, - - {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure - {0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain - - {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure - {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain - - {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure - {0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain - - {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure - {0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain - - // also RSVD - {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01}, - {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a}, - {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b}, - {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f}, - {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00}, - {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a}, - {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10}, - {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a}, - {0x3679, 0x19}, - - // Y_ADDR_START = 4 - {0x3802, 0x00}, {0x3803, 0x04}, - // Y_ADDR_END = 0x50b - {0x3806, 0x05}, {0x3807, 0x0b}, - - // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928) - {0x3808, 0x07}, {0x3809, 0x88}, - - // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208) - {0x380a, 0x04}, {0x380b, 0xb8}, - - // horizontal timing 0x447 - {0x380c, 0x04}, {0x380d, 0x47}, - - // rows per frame (was 0x2ae) - // 0x8ae = 53.65 ms - {0x380e, 0x08}, {0x380f, 0x15}, - // this should be triggered by FSIN, not free running - - {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff - {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff - {0x3816, 0x01}, - {0x3817, 0x01}, - {0x381c, 0x18}, - {0x381e, 0x01}, - {0x381f, 0x01}, - - // don't mirror, just flip - {0x3820, 0x04}, - - {0x3821, 0x19}, - {0x3832, 0xF0}, - {0x3834, 0xF0}, - {0x384c, 0x02}, - {0x384d, 0x0d}, - {0x3850, 0x00}, - {0x3851, 0x42}, - {0x3852, 0x00}, - {0x3853, 0x40}, - {0x3858, 0x04}, - {0x388c, 0x02}, - {0x388d, 0x2b}, - - // APC - {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90}, - {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20}, - {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e}, - {0x3b4c, 0x00}, {0x3b4d, 0x00}, - {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08}, - {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80}, - {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00}, - {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09}, - - // OTP - {0x3d82, 0x73}, - {0x3d85, 0x05}, - {0x3d8a, 0x03}, - {0x3d8b, 0xff}, - {0x3d99, 0x00}, - {0x3d9a, 0x9f}, - {0x3d9b, 0x00}, - {0x3d9c, 0xa0}, - {0x3da4, 0x00}, - {0x3da7, 0x50}, - - // DTR - {0x420e, 0x6b}, - {0x420f, 0x6e}, - {0x4210, 0x06}, - {0x4211, 0xc1}, - {0x421e, 0x02}, - {0x421f, 0x45}, - {0x4220, 0xe1}, - {0x4221, 0x01}, - {0x4301, 0xff}, - {0x4307, 0x03}, - {0x4308, 0x13}, - {0x430a, 0x13}, - {0x430d, 0x93}, - {0x430f, 0x57}, - {0x4310, 0x95}, - {0x4311, 0x16}, - {0x4316, 0x00}, - - {0x4317, 0x38}, // both embedded rows are enabled - - {0x4319, 0x03}, // spd dcg - {0x431a, 0x00}, // 8 bit mipi - {0x431b, 0x00}, - {0x431d, 0x2a}, - {0x431e, 0x11}, - - {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits - //{0x431f, 0x00}, // disable PWL - - {0x4320, 0x19}, - {0x4323, 0x80}, - {0x4324, 0x00}, - {0x4503, 0x4e}, - {0x4505, 0x00}, - {0x4509, 0x00}, - {0x450a, 0x00}, - {0x4580, 0xf8}, - {0x4583, 0x07}, - {0x4584, 0x6a}, - {0x4585, 0x08}, - {0x4586, 0x05}, - {0x4587, 0x04}, - {0x4588, 0x73}, - {0x4589, 0x05}, - {0x458a, 0x1f}, - {0x458b, 0x02}, - {0x458c, 0xdc}, - {0x458d, 0x03}, - {0x458e, 0x02}, - {0x4597, 0x07}, - {0x4598, 0x40}, - {0x4599, 0x0e}, - {0x459a, 0x0e}, - {0x459b, 0xfb}, - {0x459c, 0xf3}, - {0x4602, 0x00}, - {0x4603, 0x13}, - {0x4604, 0x00}, - {0x4609, 0x0a}, - {0x460a, 0x30}, - {0x4610, 0x00}, - {0x4611, 0x70}, - {0x4612, 0x01}, - {0x4613, 0x00}, - {0x4614, 0x00}, - {0x4615, 0x70}, - {0x4616, 0x01}, - {0x4617, 0x00}, - - {0x4800, 0x04}, // invert output PCLK - {0x480a, 0x22}, - {0x4813, 0xe4}, - - // mipi - {0x4814, 0x2a}, - {0x4837, 0x0d}, - {0x484b, 0x47}, - {0x484f, 0x00}, - {0x4887, 0x51}, - {0x4d00, 0x4a}, - {0x4d01, 0x18}, - {0x4d05, 0xff}, - {0x4d06, 0x88}, - {0x4d08, 0x63}, - {0x4d09, 0xdf}, - {0x4d15, 0x7d}, - {0x4d1a, 0x20}, - {0x4d30, 0x0a}, - {0x4d31, 0x00}, - {0x4d34, 0x7d}, - {0x4d3c, 0x7d}, - {0x4f00, 0x00}, - {0x4f01, 0x00}, - {0x4f02, 0x00}, - {0x4f03, 0x20}, - {0x4f04, 0xe0}, - {0x6a00, 0x00}, - {0x6a01, 0x20}, - {0x6a02, 0x00}, - {0x6a03, 0x20}, - {0x6a04, 0x02}, - {0x6a05, 0x80}, - {0x6a06, 0x01}, - {0x6a07, 0xe0}, - {0x6a08, 0xcf}, - {0x6a09, 0x01}, - {0x6a0a, 0x40}, - {0x6a20, 0x00}, - {0x6a21, 0x02}, - {0x6a22, 0x00}, - {0x6a23, 0x00}, - {0x6a24, 0x00}, - {0x6a25, 0x00}, - {0x6a26, 0x00}, - {0x6a27, 0x00}, - {0x6a28, 0x00}, - - // isp - {0x5000, 0x8f}, - {0x5001, 0x75}, - {0x5002, 0x7f}, // PWL0 - //{0x5002, 0x3f}, // PWL disable - {0x5003, 0x7a}, - - {0x5004, 0x3e}, - {0x5005, 0x1e}, - {0x5006, 0x1e}, - {0x5007, 0x1e}, - - {0x5008, 0x00}, - {0x500c, 0x00}, - {0x502c, 0x00}, - {0x502e, 0x00}, - {0x502f, 0x00}, - {0x504b, 0x00}, - {0x5053, 0x00}, - {0x505b, 0x00}, - {0x5063, 0x00}, - {0x5070, 0x00}, - {0x5074, 0x04}, - {0x507a, 0x04}, - {0x507b, 0x09}, - {0x5500, 0x02}, - {0x5700, 0x02}, - {0x5900, 0x02}, - {0x6007, 0x04}, - {0x6008, 0x05}, - {0x6009, 0x02}, - {0x600b, 0x08}, - {0x600c, 0x07}, - {0x600d, 0x88}, - {0x6016, 0x00}, - {0x6027, 0x04}, - {0x6028, 0x05}, - {0x6029, 0x02}, - {0x602b, 0x08}, - {0x602c, 0x07}, - {0x602d, 0x88}, - {0x6047, 0x04}, - {0x6048, 0x05}, - {0x6049, 0x02}, - {0x604b, 0x08}, - {0x604c, 0x07}, - {0x604d, 0x88}, - {0x6067, 0x04}, - {0x6068, 0x05}, - {0x6069, 0x02}, - {0x606b, 0x08}, - {0x606c, 0x07}, - {0x606d, 0x88}, - {0x6087, 0x04}, - {0x6088, 0x05}, - {0x6089, 0x02}, - {0x608b, 0x08}, - {0x608c, 0x07}, - {0x608d, 0x88}, - - // 12-bit PWL0 - {0x5e00, 0x00}, - - // m_ndX_exp[0:32] - // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518 - {0x5e01, 0x09}, - {0x5e02, 0x09}, - {0x5e03, 0x0a}, - {0x5e04, 0x0a}, - {0x5e05, 0x0a}, - {0x5e06, 0x0b}, - {0x5e07, 0x0b}, - {0x5e08, 0x0c}, - {0x5e09, 0x0c}, - {0x5e0a, 0x0d}, - {0x5e0b, 0x0d}, - {0x5e0c, 0x0e}, - {0x5e0d, 0x0e}, - {0x5e0e, 0x0f}, - {0x5e0f, 0x0f}, - {0x5e10, 0x10}, - {0x5e11, 0x10}, - {0x5e12, 0x11}, - {0x5e13, 0x11}, - {0x5e14, 0x12}, - {0x5e15, 0x12}, - {0x5e16, 0x12}, - {0x5e17, 0x12}, - {0x5e18, 0x13}, - {0x5e19, 0x13}, - {0x5e1a, 0x13}, - {0x5e1b, 0x14}, - {0x5e1c, 0x14}, - {0x5e1d, 0x14}, - {0x5e1e, 0x15}, - {0x5e1f, 0x15}, - {0x5e20, 0x15}, - {0x5e21, 0x16}, - - // m_ndY_val[0:32] - // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095 - {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00}, - {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff}, - {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00}, - {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00}, - {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00}, - {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80}, - {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80}, - {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80}, - {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80}, - {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80}, - {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80}, - {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80}, - {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80}, - {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80}, - {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80}, - {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80}, - {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80}, - {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40}, - {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40}, - {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40}, - {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40}, - {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40}, - {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40}, - {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40}, - {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40}, - {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40}, - {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40}, - {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40}, - {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40}, - {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40}, - {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40}, - {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40}, - {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40}, - - // disable PWL - /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00}, - {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00}, - {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00}, - {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00}, - {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00}, - {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00}, - {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00}, - {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00}, - {0x5e21, 0x00}, - - {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/ - - {0x4001, 0x2b}, // BLC_CTRL_1 - {0x4008, 0x02}, {0x4009, 0x03}, - {0x4018, 0x12}, - {0x4022, 0x40}, - {0x4023, 0x20}, - - // all black level targets are 0x40 - {0x4026, 0x00}, {0x4027, 0x40}, - {0x4028, 0x00}, {0x4029, 0x40}, - {0x402a, 0x00}, {0x402b, 0x40}, - {0x402c, 0x00}, {0x402d, 0x40}, - - {0x407e, 0xcc}, - {0x407f, 0x18}, - {0x4080, 0xff}, - {0x4081, 0xff}, - {0x4082, 0x01}, - {0x4083, 0x53}, - {0x4084, 0x01}, - {0x4085, 0x2b}, - {0x4086, 0x00}, - {0x4087, 0xb3}, - - {0x4640, 0x40}, - {0x4641, 0x11}, - {0x4642, 0x0e}, - {0x4643, 0xee}, - {0x4646, 0x0f}, - {0x4648, 0x00}, - {0x4649, 0x03}, - - {0x4f00, 0x00}, - {0x4f01, 0x00}, - {0x4f02, 0x80}, - {0x4f03, 0x2c}, - {0x4f04, 0xf8}, - - {0x4d09, 0xff}, - {0x4d09, 0xdf}, - - {0x5003, 0x7a}, - {0x5b80, 0x08}, - {0x5c00, 0x08}, - {0x5c80, 0x00}, - {0x5bbe, 0x12}, - {0x5c3e, 0x12}, - {0x5cbe, 0x12}, - {0x5b8a, 0x80}, - {0x5b8b, 0x80}, - {0x5b8c, 0x80}, - {0x5b8d, 0x80}, - {0x5b8e, 0x60}, - {0x5b8f, 0x80}, - {0x5b90, 0x80}, - {0x5b91, 0x80}, - {0x5b92, 0x80}, - {0x5b93, 0x20}, - {0x5b94, 0x80}, - {0x5b95, 0x80}, - {0x5b96, 0x80}, - {0x5b97, 0x20}, - {0x5b98, 0x00}, - {0x5b99, 0x80}, - {0x5b9a, 0x40}, - {0x5b9b, 0x20}, - {0x5b9c, 0x00}, - {0x5b9d, 0x00}, - {0x5b9e, 0x80}, - {0x5b9f, 0x00}, - {0x5ba0, 0x00}, - {0x5ba1, 0x00}, - {0x5ba2, 0x00}, - {0x5ba3, 0x00}, - {0x5ba4, 0x00}, - {0x5ba5, 0x00}, - {0x5ba6, 0x00}, - {0x5ba7, 0x00}, - {0x5ba8, 0x02}, - {0x5ba9, 0x00}, - {0x5baa, 0x02}, - {0x5bab, 0x76}, - {0x5bac, 0x03}, - {0x5bad, 0x08}, - {0x5bae, 0x00}, - {0x5baf, 0x80}, - {0x5bb0, 0x00}, - {0x5bb1, 0xc0}, - {0x5bb2, 0x01}, - {0x5bb3, 0x00}, - - // m_nNormCombineWeight - {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60}, - {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20}, - {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00}, - {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00}, - {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00}, - {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00}, - - // m_nCombinThreL - {0x5c28, 0x02}, {0x5c29, 0x00}, - {0x5c2a, 0x02}, {0x5c2b, 0x76}, - {0x5c2c, 0x03}, {0x5c2d, 0x08}, - - // m_nCombinThreS - {0x5c2e, 0x00}, {0x5c2f, 0x80}, - {0x5c30, 0x00}, {0x5c31, 0xc0}, - {0x5c32, 0x01}, {0x5c33, 0x00}, - - // m_nNormCombineWeight - {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80}, - {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60}, - {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40}, - {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00}, - {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00}, - {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00}, - - {0x5ca8, 0x01}, {0x5ca9, 0x00}, - {0x5caa, 0x02}, {0x5cab, 0x00}, - {0x5cac, 0x03}, {0x5cad, 0x08}, - - {0x5cae, 0x01}, {0x5caf, 0x00}, - {0x5cb0, 0x02}, {0x5cb1, 0x00}, - {0x5cb2, 0x03}, {0x5cb3, 0x08}, - - // combine ISP - {0x5be7, 0x80}, - {0x5bc9, 0x80}, - {0x5bca, 0x80}, - {0x5bcb, 0x80}, - {0x5bcc, 0x80}, - {0x5bcd, 0x80}, - {0x5bce, 0x80}, - {0x5bcf, 0x80}, - {0x5bd0, 0x80}, - {0x5bd1, 0x80}, - {0x5bd2, 0x20}, - {0x5bd3, 0x80}, - {0x5bd4, 0x40}, - {0x5bd5, 0x20}, - {0x5bd6, 0x00}, - {0x5bd7, 0x00}, - {0x5bd8, 0x00}, - {0x5bd9, 0x00}, - {0x5bda, 0x00}, - {0x5bdb, 0x00}, - {0x5bdc, 0x00}, - {0x5bdd, 0x00}, - {0x5bde, 0x00}, - {0x5bdf, 0x00}, - {0x5be0, 0x00}, - {0x5be1, 0x00}, - {0x5be2, 0x00}, - {0x5be3, 0x00}, - {0x5be4, 0x00}, - {0x5be5, 0x00}, - {0x5be6, 0x00}, - - // m_nSPDCombineWeight - {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40}, - {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20}, - {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00}, - {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00}, - {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00}, - {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00}, - - // m_nSPDCombineWeight - {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80}, - {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60}, - {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40}, - {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20}, - {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00}, - {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00}, - - {0x5d74, 0x01}, - {0x5d75, 0x00}, - - {0x5d1f, 0x81}, - {0x5d11, 0x00}, - {0x5d12, 0x10}, - {0x5d13, 0x10}, - {0x5d15, 0x05}, - {0x5d16, 0x05}, - {0x5d17, 0x05}, - {0x5d08, 0x03}, - {0x5d09, 0xb6}, - {0x5d0a, 0x03}, - {0x5d0b, 0xb6}, - {0x5d18, 0x03}, - {0x5d19, 0xb6}, - {0x5d62, 0x01}, - {0x5d40, 0x02}, - {0x5d41, 0x01}, - {0x5d63, 0x1f}, - {0x5d64, 0x00}, - {0x5d65, 0x80}, - {0x5d56, 0x00}, - {0x5d57, 0x20}, - {0x5d58, 0x00}, - {0x5d59, 0x20}, - {0x5d5a, 0x00}, - {0x5d5b, 0x0c}, - {0x5d5c, 0x02}, - {0x5d5d, 0x40}, - {0x5d5e, 0x02}, - {0x5d5f, 0x40}, - {0x5d60, 0x03}, - {0x5d61, 0x40}, - {0x5d4a, 0x02}, - {0x5d4b, 0x40}, - {0x5d4c, 0x02}, - {0x5d4d, 0x40}, - {0x5d4e, 0x02}, - {0x5d4f, 0x40}, - {0x5d50, 0x18}, - {0x5d51, 0x80}, - {0x5d52, 0x18}, - {0x5d53, 0x80}, - {0x5d54, 0x18}, - {0x5d55, 0x80}, - {0x5d46, 0x20}, - {0x5d47, 0x00}, - {0x5d48, 0x22}, - {0x5d49, 0x00}, - {0x5d42, 0x20}, - {0x5d43, 0x00}, - {0x5d44, 0x22}, - {0x5d45, 0x00}, - - {0x5004, 0x1e}, - {0x4221, 0x03}, // this is changed from 1 -> 3 - - // DCG exposure coarse - // {0x3501, 0x01}, {0x3502, 0xc8}, - // SPD exposure coarse - // {0x3541, 0x01}, {0x3542, 0xc8}, - // VS exposure coarse - // {0x35c1, 0x00}, {0x35c2, 0x01}, - - // crc reference - {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, - // crc stat check - {0x507a, 0x5f}, {0x507b, 0x46}, - - // watchdog control - {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c}, - - // color balance gains - // blue - {0x5280, 0x06}, {0x5281, 0xCB}, // hcg - {0x5480, 0x06}, {0x5481, 0xCB}, // lcg - {0x5680, 0x06}, {0x5681, 0xCB}, // spd - {0x5880, 0x06}, {0x5881, 0xCB}, // vs - - // green(blue) - {0x5282, 0x04}, {0x5283, 0x00}, - {0x5482, 0x04}, {0x5483, 0x00}, - {0x5682, 0x04}, {0x5683, 0x00}, - {0x5882, 0x04}, {0x5883, 0x00}, - - // green(red) - {0x5284, 0x04}, {0x5285, 0x00}, - {0x5484, 0x04}, {0x5485, 0x00}, - {0x5684, 0x04}, {0x5685, 0x00}, - {0x5884, 0x04}, {0x5885, 0x00}, - - // red - {0x5286, 0x08}, {0x5287, 0xDE}, - {0x5486, 0x08}, {0x5487, 0xDE}, - {0x5686, 0x08}, {0x5687, 0xDE}, - {0x5886, 0x08}, {0x5887, 0xDE}, - - // fixed gains - {0x3588, 0x01}, {0x3589, 0x00}, - {0x35c8, 0x01}, {0x35c9, 0x00}, - {0x3548, 0x0F}, {0x3549, 0x00}, - {0x35c1, 0x00}, -}; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h deleted file mode 100644 index 96aa8b604f323e..00000000000000 --- a/system/camerad/sensors/sensor.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include "media/cam_isp.h" -#include "media/cam_sensor.h" - -#include "cereal/gen/cpp/log.capnp.h" -#include "system/camerad/sensors/ox03c10_registers.h" -#include "system/camerad/sensors/os04c10_registers.h" - -#define ANALOG_GAIN_MAX_CNT 55 - -class SensorInfo { -public: - SensorInfo() = default; - virtual std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; } - virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; } - virtual int getSlaveAddress(int port) const { assert(0); } - - cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN; - float pixel_size_mm; - uint32_t frame_width, frame_height; - uint32_t frame_stride; - uint32_t frame_offset = 0; - uint32_t extra_height = 0; - int out_scale = 1; - int registers_offset = -1; - int stats_offset = -1; - int hdr_offset = -1; - - int exposure_time_min; - int exposure_time_max; - - float dc_gain_factor; - int dc_gain_min_weight; - int dc_gain_max_weight; - float dc_gain_on_grey; - float dc_gain_off_grey; - - float ev_scale = 1.0; - float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; - int analog_gain_min_idx; - int analog_gain_max_idx; - int analog_gain_rec_idx; - int analog_gain_cost_delta; - float analog_gain_cost_low; - float analog_gain_cost_high; - float target_grey_factor; - float min_ev; - float max_ev; - - bool data_word; - uint32_t probe_reg_addr; - uint32_t probe_expected_data; - std::vector start_reg_array; - std::vector init_reg_array; - - uint32_t bits_per_pixel; - uint32_t bayer_pattern; - uint32_t mipi_format; - uint32_t mclk_frequency; - uint32_t frame_data_type; - - uint32_t readout_time_ns; // used to recover EOF from SOF - - // ISP image processing params - uint32_t black_level; - std::vector color_correct_matrix; // 3x3 - std::vector gamma_lut_rgb; // gamma LUTs are length 64 * sizeof(uint32_t); same for r/g/b here - void prepare_gamma_lut() { - for (int i = 0; i < 64; i++) { - gamma_lut_rgb[i] |= ((uint32_t)(gamma_lut_rgb[i+1] - gamma_lut_rgb[i]) << 10); - } - gamma_lut_rgb.pop_back(); - } - std::vector linearization_lut; // length 36 - std::vector linearization_pts; // length 4 - std::vector vignetting_lut; // length 221 - - const int num() const { - return static_cast(image_sensor); - }; -}; - -class OX03C10 : public SensorInfo { -public: - OX03C10(); - 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; -}; - -class OS04C10 : public SensorInfo { -public: - OS04C10(); - void ife_downscale_configure(); - 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; -}; diff --git a/system/camerad/snapshot.py b/system/camerad/snapshot.py deleted file mode 100755 index 035a4acdcf4c09..00000000000000 --- a/system/camerad/snapshot.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python3 -import subprocess -import time - -import numpy as np -from PIL import Image - -import cereal.messaging as messaging -from msgq.visionipc import VisionIpcClient, VisionStreamType -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.system.hardware import PC -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.manager.process_config import managed_processes - - -VISION_STREAMS = { - "roadCameraState": VisionStreamType.VISION_STREAM_ROAD, - "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, - "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD, -} - - -def jpeg_write(fn, dat): - img = Image.fromarray(dat) - img.save(fn, "JPEG") - - -def yuv_to_rgb(y, u, v): - ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape) - vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape) - - yuv = np.dstack((y, ul, vl)).astype(np.int16) - yuv[:, :, 1:] -= 128 - - m = np.array([ - [1.00000, 1.00000, 1.00000], - [0.00000, -0.39465, 2.03211], - [1.13983, -0.58060, 0.00000], - ]) - rgb = np.dot(yuv, m).clip(0, 255) - return rgb.astype(np.uint8) - - -def extract_image(buf): - # NV12 format: Y plane followed by interleaved UV plane - # UV plane size is stride * uv_height, where uv_height = align(height/2, 16) - uv_height = ((buf.height // 2) + 15) // 16 * 16 - uv_plane_size = buf.stride * uv_height - - y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width] - uv_data = buf.data[buf.uv_offset:buf.uv_offset + uv_plane_size] - u = np.array(uv_data[::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2] - v = np.array(uv_data[1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2] - - return yuv_to_rgb(y, u, v) - - -def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): - sockets = [s for s in (frame, front_frame) if s is not None] - sm = messaging.SubMaster(sockets) - vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets} - - # wait 4 sec from camerad startup for focus and exposure - while sm[sockets[0]].frameId < int(4. / DT_MDL): - sm.update() - - for client in vipc_clients.values(): - client.connect(True) - - # grab images - rear, front = None, None - if frame is not None: - c = vipc_clients[frame] - rear = extract_image(c.recv()) - if front_frame is not None: - c = vipc_clients[front_frame] - front = extract_image(c.recv()) - return rear, front - - -def snapshot(): - params = Params() - - if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"): - print("Already taking snapshot") - return None, None - - front_camera_allowed = params.get_bool("RecordFront") - params.put_bool("IsTakingSnapshot", True) - set_offroad_alert("Offroad_IsTakingSnapshot", True) - time.sleep(2.0) # Give hardwared time to read the param, or if just started give camerad time to start - - # Check if camerad is already started - try: - subprocess.check_call(["pgrep", "camerad"]) - print("Camerad already running") - params.put_bool("IsTakingSnapshot", False) - params.remove("Offroad_IsTakingSnapshot") - return None, None - except subprocess.CalledProcessError: - pass - - try: - # Allow testing on replay on PC - if not PC: - managed_processes['camerad'].start() - - frame = "wideRoadCameraState" - front_frame = "driverCameraState" if front_camera_allowed else None - rear, front = get_snapshots(frame, front_frame) - finally: - managed_processes['camerad'].stop() - params.put_bool("IsTakingSnapshot", False) - set_offroad_alert("Offroad_IsTakingSnapshot", False) - - if not front_camera_allowed: - front = None - - return rear, front - - -if __name__ == "__main__": - pic, fpic = snapshot() - if pic is not None: - print(pic.shape) - jpeg_write("/tmp/back.jpg", pic) - if fpic is not None: - jpeg_write("/tmp/front.jpg", fpic) - else: - print("Error taking snapshot") diff --git a/system/camerad/snapshot/__init__.py b/system/camerad/snapshot/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py new file mode 100755 index 00000000000000..48dfc9e02d510d --- /dev/null +++ b/system/camerad/snapshot/snapshot.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +import subprocess +import time + +import numpy as np +from PIL import Image + +import cereal.messaging as messaging +from cereal.visionipc import VisionIpcClient, VisionStreamType +from common.params import Params +from common.realtime import DT_MDL +from system.hardware import PC +from selfdrive.controls.lib.alertmanager import set_offroad_alert +from selfdrive.manager.process_config import managed_processes + +LM_THRESH = 120 # defined in system/camerad/imgproc/utils.h + +VISION_STREAMS = { + "roadCameraState": VisionStreamType.VISION_STREAM_ROAD, + "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, + "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD, +} + + +def jpeg_write(fn, dat): + img = Image.fromarray(dat) + img.save(fn, "JPEG") + + +def yuv_to_rgb(y, u, v): + ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape) + vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape) + + yuv = np.dstack((y, ul, vl)).astype(np.int16) + yuv[:, :, 1:] -= 128 + + m = np.array([ + [1.00000, 1.00000, 1.00000], + [0.00000, -0.39465, 2.03211], + [1.13983, -0.58060, 0.00000], + ]) + rgb = np.dot(yuv, m).clip(0, 255) + return rgb.astype(np.uint8) + + +def extract_image(buf, w, h, stride, uv_offset): + y = np.array(buf[:uv_offset], dtype=np.uint8).reshape((-1, stride))[:h, :w] + u = np.array(buf[uv_offset::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] + v = np.array(buf[uv_offset+1::2], dtype=np.uint8).reshape((-1, stride//2))[:h//2, :w//2] + + return yuv_to_rgb(y, u, v) + + +def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): + sockets = [s for s in (frame, front_frame) if s is not None] + sm = messaging.SubMaster(sockets) + vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets} + + # wait 4 sec from camerad startup for focus and exposure + while sm[sockets[0]].frameId < int(4. / DT_MDL): + sm.update() + + for client in vipc_clients.values(): + client.connect(True) + + # grab images + rear, front = None, None + if frame is not None: + c = vipc_clients[frame] + rear = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) + if front_frame is not None: + c = vipc_clients[front_frame] + front = extract_image(c.recv(), c.width, c.height, c.stride, c.uv_offset) + return rear, front + + +def snapshot(): + params = Params() + + if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"): + print("Already taking snapshot") + return None, None + + front_camera_allowed = params.get_bool("RecordFront") + params.put_bool("IsTakingSnapshot", True) + set_offroad_alert("Offroad_IsTakingSnapshot", True) + time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start + + # Check if camerad is already started + try: + subprocess.check_call(["pgrep", "camerad"]) + print("Camerad already running") + params.put_bool("IsTakingSnapshot", False) + params.remove("Offroad_IsTakingSnapshot") + return None, None + except subprocess.CalledProcessError: + pass + + try: + # Allow testing on replay on PC + if not PC: + managed_processes['camerad'].start() + + frame = "wideRoadCameraState" + front_frame = "driverCameraState" if front_camera_allowed else None + rear, front = get_snapshots(frame, front_frame) + finally: + managed_processes['camerad'].stop() + params.put_bool("IsTakingSnapshot", False) + set_offroad_alert("Offroad_IsTakingSnapshot", False) + + if not front_camera_allowed: + front = None + + return rear, front + + +if __name__ == "__main__": + pic, fpic = snapshot() + if pic is not None: + print(pic.shape) + jpeg_write("/tmp/back.jpg", pic) + if fpic is not None: + jpeg_write("/tmp/front.jpg", fpic) + else: + print("Error taking snapshot") diff --git a/system/camerad/test/.gitignore b/system/camerad/test/.gitignore index d67473ebcdb088..44cd0b2730e262 100644 --- a/system/camerad/test/.gitignore +++ b/system/camerad/test/.gitignore @@ -1,2 +1 @@ jpegs/ -test_ae_gray diff --git a/system/camerad/test/ae_gray_test.cc b/system/camerad/test/ae_gray_test.cc new file mode 100644 index 00000000000000..aabd7534ee44c6 --- /dev/null +++ b/system/camerad/test/ae_gray_test.cc @@ -0,0 +1,67 @@ +// unittest for set_exposure_target + +#include "ae_gray_test.h" + +#include + +#include +#include + +#include "common/util.h" +#include "system/camerad/cameras/camera_common.h" + +int main() { + // set up fake camerabuf + CameraBuf cb = {}; + VisionBuf vb = {}; + uint8_t * fb_y = new uint8_t[W*H]; + vb.y = fb_y; + cb.cur_yuv_buf = &vb; + cb.rgb_width = W; + cb.rgb_height = H; + + printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height); + + // mix of 5 tones + uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max + + bool passed = true; + float rtol = 0.05; + // generate pattern and calculate EV + int cnt = 0; + for (int i_0=0; i_0 rtol*evgt) { + passed = false; + } + + // report + printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); + cnt++; + } + } + } + } + assert(passed); + + delete[] fb_y; + return 0; +} diff --git a/system/camerad/test/ae_gray_test.h b/system/camerad/test/ae_gray_test.h new file mode 100644 index 00000000000000..fb54cd958437f9 --- /dev/null +++ b/system/camerad/test/ae_gray_test.h @@ -0,0 +1,18 @@ +#pragma once + +#define W 240 +#define H 160 + +#define TONE_SPLITS 3 + +float gts[TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS] = { + 0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.917969, + 0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750, + 0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000,0.375000, + 0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,0.093750,0.093750, + 0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750, + 0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, + 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, + 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, + 0.000000 +}; diff --git a/system/camerad/test/check_skips.py b/system/camerad/test/check_skips.py new file mode 100755 index 00000000000000..0814ce44ff80ee --- /dev/null +++ b/system/camerad/test/check_skips.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +# type: ignore +import cereal.messaging as messaging + +all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState'] +prev_id = [None,None,None] +this_id = [None,None,None] +dt = [None,None,None] +num_skipped = [0,0,0] + +if __name__ == "__main__": + sm = messaging.SubMaster(all_sockets) + while True: + sm.update() + + for i in range(len(all_sockets)): + if not sm.updated[all_sockets[i]]: + continue + this_id[i] = sm[all_sockets[i]].frameId + if prev_id[i] is None: + prev_id[i] = this_id[i] + continue + dt[i] = this_id[i] - prev_id[i] + if dt[i] != 1: + num_skipped[i] += dt[i] - 1 + print(all_sockets[i] ,dt[i] - 1, num_skipped[i]) + prev_id[i] = this_id[i] diff --git a/system/camerad/test/debug.sh b/system/camerad/test/debug.sh deleted file mode 100755 index a031be69236709..00000000000000 --- a/system/camerad/test/debug.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env bash -set -e - -#echo 4294967295 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl - -# no CCI and UTIL, very spammy -echo 0xfffdbfff | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl -#echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl - -sudo dmesg -C -scons -u -j8 --minimal . -export DEBUG_FRAMES=1 -export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 -#export DISABLE_DRIVER=1 -export LOGPRINT=debug -./camerad diff --git a/system/camerad/test/frame_test.py b/system/camerad/test/frame_test.py new file mode 100755 index 00000000000000..39198e19da58d1 --- /dev/null +++ b/system/camerad/test/frame_test.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import numpy as np +import cereal.messaging as messaging +from PIL import ImageFont, ImageDraw, Image + +font = ImageFont.truetype("arial", size=72) +def get_frame(idx): + img = np.zeros((874, 1164, 3), np.uint8) + img[100:400, 100:100+(idx % 10) * 100] = 255 + + # big number + im2 = Image.new("RGB", (200, 200)) + draw = ImageDraw.Draw(im2) + draw.text((10, 100), "%02d" % idx, font=font) + img[400:600, 400:600] = np.array(im2.getdata()).reshape((200, 200, 3)) + return img.tostring() + +if __name__ == "__main__": + from common.realtime import Ratekeeper + rk = Ratekeeper(20) + + pm = messaging.PubMaster(['roadCameraState']) + frm = [get_frame(x) for x in range(30)] + idx = 0 + while 1: + print("send %d" % idx) + dat = messaging.new_message('roadCameraState') + dat.valid = True + dat.frame = { + "frameId": idx, + "image": frm[idx % len(frm)], + } + pm.send('roadCameraState', dat) + + idx += 1 + rk.keep_time() + #time.sleep(1.0) diff --git a/system/camerad/test/get_thumbnails_for_segment.py b/system/camerad/test/get_thumbnails_for_segment.py new file mode 100755 index 00000000000000..898377b11110c1 --- /dev/null +++ b/system/camerad/test/get_thumbnails_for_segment.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 + +import os + +from tqdm import tqdm + +from common.file_helpers import mkdirs_exists_ok +from tools.lib.logreader import LogReader +from tools.lib.route import Route + +import argparse + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("route", help="The route name") + parser.add_argument("segment", type=int, help="The index of the segment") + args = parser.parse_args() + + out_path = os.path.join("jpegs", f"{args.route.replace('|', '_')}_{args.segment}") + mkdirs_exists_ok(out_path) + + r = Route(args.route) + path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment] + lr = list(LogReader(path)) + + for msg in tqdm(lr): + if msg.which() == 'thumbnail': + with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.thumbnail.thumbnail) + elif msg.which() == 'navThumbnail': + with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.navThumbnail.thumbnail) diff --git a/system/camerad/test/icp_debug.sh b/system/camerad/test/icp_debug.sh deleted file mode 100755 index ebeef9bf8f2d7c..00000000000000 --- a/system/camerad/test/icp_debug.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/usr/bin/env bash -set -e - -cd /sys/kernel/debug/tracing -echo "" > trace -echo 1 > tracing_on -#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q -echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type -echo 1 > /sys/kernel/debug/tracing/events/camera/enable -echo 0xffffffff > /sys/kernel/debug/camera_icp/a5_debug_lvl -echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable - -cat /sys/kernel/debug/tracing/trace_pipe diff --git a/system/camerad/test/intercept.sh b/system/camerad/test/intercept.sh deleted file mode 100755 index e269929afc5feb..00000000000000 --- a/system/camerad/test/intercept.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/usr/bin/env bash -DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad diff --git a/system/camerad/test/test_ae_gray.cc b/system/camerad/test/test_ae_gray.cc deleted file mode 100644 index 39c3d9c4e55785..00000000000000 --- a/system/camerad/test/test_ae_gray.cc +++ /dev/null @@ -1,84 +0,0 @@ -#define CATCH_CONFIG_MAIN -#include "catch2/catch.hpp" - -#include - -#include -#include - -#include "common/util.h" -#include "system/camerad/cameras/camera_common.h" - -#define W 240 -#define H 160 - - -#define TONE_SPLITS 3 - -float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = { - 0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969, - 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, - 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000, - 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750, - 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, - 0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, - 0.000000}; - - -TEST_CASE("camera.test_calculate_exposure_value") { - // set up fake camerabuf - CameraBuf cb = {}; - VisionBuf vb = {}; - uint8_t * fb_y = new uint8_t[W*H]; - vb.y = fb_y; - cb.cur_yuv_buf = &vb; - cb.out_img_width = W; - cb.out_img_height = H; - Rect rect = {0, 0, W-1, H-1}; - - printf("AE test patterns %dx%d\n", cb.out_img_width, cb.out_img_height); - - // mix of 5 tones - uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max - - bool passed = true; - float rtol = 0.05; - // generate pattern and calculate EV - int cnt = 0; - for (int i_0=0; i_0 rtol*evgt) { - passed = false; - } - - // report - printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); - cnt++; - } - } - } - } - assert(passed); - - delete[] fb_y; -} diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py old mode 100644 new mode 100755 index 1f3f97b0820e6d..1a2e365a8f2390 --- a/system/camerad/test/test_camerad.py +++ b/system/camerad/test/test_camerad.py @@ -1,101 +1,63 @@ -import os +#!/usr/bin/env python3 + import time -import pytest -import numpy as np +import unittest import cereal.messaging as messaging -from cereal.services import SERVICE_LIST -from openpilot.system.manager.process_config import managed_processes -from openpilot.tools.lib.log_time_series import msgs_to_time_series - -TEST_TIMESPAN = 10 -CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - - -def run_and_log(procs, services, duration): - logs = [] - - try: - for p in procs: - managed_processes[p].start() - socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services] - - start_time = time.monotonic() - while time.monotonic() - start_time < duration: - for s in socks: - logs.extend(messaging.drain_sock(s)) - for p in procs: - assert managed_processes[p].proc.is_alive() - finally: - for p in procs: - managed_processes[p].stop() - - return logs - -@pytest.fixture(scope="module") -def logs(): - logs = run_and_log(["camerad", ], CAMERAS, TEST_TIMESPAN) - ts = msgs_to_time_series(logs) - - for cam in CAMERAS: - expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN - cnt = len(ts[cam]['t']) - assert expected_frames*0.8 < cnt < expected_frames*1.2, f"unexpected frame count {cam}: {expected_frames=}, got {cnt}" - - dts = np.abs(np.diff([ts[cam]['timestampSof']/1e6]) - 1000/SERVICE_LIST[cam].frequency) - assert (dts < 1.0).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}" - return ts - -@pytest.mark.tici -class TestCamerad: - def test_frame_skips(self, logs): - for c in CAMERAS: - assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips" - - def test_frame_sync(self, logs): - n = range(len(logs['roadCameraState']['t'][:-10])) - - frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n} - assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned" - - frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n} - diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()} - - laggy_frames = {k: v for k, v in diffs.items() if v > 1.1} - assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}" - - def test_sanity_checks(self, logs): - self._sanity_checks(logs) - - def _sanity_checks(self, ts): - for c in CAMERAS: - assert c in ts - assert len(ts[c]['t']) > 20 - - # not a valid request id - assert 0 not in ts[c]['requestId'] - - # should monotonically increase - assert np.all(np.diff(ts[c]['frameId']) >= 1) - assert np.all(np.diff(ts[c]['requestId']) >= 1) - - # EOF > SOF - assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0) - - # logMonoTime > SOF - assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7) - - # logMonoTime > EOF, needs some tolerance since EOF is (SOF + readout time) but there is noise in the SOF timestamping (done via IRQ) - assert np.mean((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7) > 0.7 # should be mostly logMonoTime > EOF - assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > -0.10) # when EOF > logMonoTime, it should never be more than two frames - - def test_stress_test(self): - os.environ['SPECTRA_ERROR_PROB'] = '0.008' - logs = run_and_log(["camerad", ], CAMERAS, 10) - ts = msgs_to_time_series(logs) - - # we should see some jumps from introduced errors - assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1 - assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1 - - self._sanity_checks(ts) +from system.hardware import TICI +from selfdrive.test.helpers import with_processes + +TEST_TIMESPAN = 30 # random.randint(60, 180) # seconds +SKIP_FRAME_TOLERANCE = 0 +LAG_FRAME_TOLERANCE = 2 # ms + +FPS_BASELINE = 20 +CAMERAS = { + "roadCameraState": FPS_BASELINE, + "driverCameraState": FPS_BASELINE // 2, +} + +if TICI: + CAMERAS["driverCameraState"] = FPS_BASELINE + CAMERAS["wideRoadCameraState"] = FPS_BASELINE + +class TestCamerad(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + @with_processes(['camerad']) + def test_frame_packets(self): + print("checking frame pkts continuity") + print(TEST_TIMESPAN) + + sm = messaging.SubMaster([socket_name for socket_name in CAMERAS]) + + last_frame_id = dict.fromkeys(CAMERAS, None) + last_ts = dict.fromkeys(CAMERAS, None) + start_time_sec = time.time() + while time.time()- start_time_sec < TEST_TIMESPAN: + sm.update() + + for camera in CAMERAS: + if sm.updated[camera]: + ct = (sm[camera].timestampEof if not TICI else sm[camera].timestampSof) / 1e6 + if last_frame_id[camera] is None: + last_frame_id[camera] = sm[camera].frameId + last_ts[camera] = ct + continue + + dfid = sm[camera].frameId - last_frame_id[camera] + self.assertTrue(abs(dfid - 1) <= SKIP_FRAME_TOLERANCE, "%s frame id diff is %d" % (camera, dfid)) + + dts = ct - last_ts[camera] + self.assertTrue(abs(dts - (1000/CAMERAS[camera])) < LAG_FRAME_TOLERANCE, f"{camera} frame t(ms) diff is {dts:f}") + + last_frame_id[camera] = sm[camera].frameId + last_ts[camera] = ct + + time.sleep(0.01) + +if __name__ == "__main__": + unittest.main() diff --git a/system/camerad/test/test_exposure.py b/system/camerad/test/test_exposure.py old mode 100644 new mode 100755 index 6f89e048004da3..8cce7e7ffa592b --- a/system/camerad/test/test_exposure.py +++ b/system/camerad/test/test_exposure.py @@ -1,26 +1,27 @@ +#!/usr/bin/env python3 import time +import unittest import numpy as np -import pytest -from openpilot.selfdrive.test.helpers import with_processes -from openpilot.system.camerad.snapshot import get_snapshots +from selfdrive.test.helpers import with_processes +from system.camerad.snapshot.snapshot import get_snapshots + +from system.hardware import TICI TEST_TIME = 45 REPEAT = 5 -@pytest.mark.tici -class TestCamerad: +class TestCamerad(unittest.TestCase): @classmethod - def setup_class(cls): - pass + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest def _numpy_rgb2gray(self, im): ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) return ret - def _is_exposure_okay(self, i, med_mean=None): - if med_mean is None: - med_mean = np.array([[0.18,0.3],[0.18,0.3]]) + def _is_exposure_okay(self, i, med_mean=np.array([[0.2,0.4],[0.2,0.6]])): h, w = i.shape[:2] i = i[h//10:9*h//10,w//10:9*w//10] med_ex, mean_ex = med_mean @@ -33,14 +34,16 @@ def _is_exposure_okay(self, i, med_mean=None): @with_processes(['camerad']) def test_camera_operation(self): passed = 0 - start = time.monotonic() - while time.monotonic() - start < TEST_TIME and passed < REPEAT: + start = time.time() + while time.time() - start < TEST_TIME and passed < REPEAT: rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") - wpic, _ = get_snapshots(frame="wideRoadCameraState") res = self._is_exposure_okay(rpic) res = res and self._is_exposure_okay(dpic) - res = res and self._is_exposure_okay(wpic) + + if TICI: + wpic, _ = get_snapshots(frame="wideRoadCameraState") + res = res and self._is_exposure_okay(wpic) if passed > 0 and not res: passed = -passed # fails test if any failure after first sus @@ -48,4 +51,7 @@ def test_camera_operation(self): passed += int(res) time.sleep(2) - assert passed >= REPEAT + self.assertGreaterEqual(passed, REPEAT) + +if __name__ == "__main__": + unittest.main() diff --git a/system/camerad/transforms/rgb_to_yuv.cl b/system/camerad/transforms/rgb_to_yuv.cl new file mode 100644 index 00000000000000..60dbdb4d5e3849 --- /dev/null +++ b/system/camerad/transforms/rgb_to_yuv.cl @@ -0,0 +1,127 @@ +#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) +#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) +#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) +#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) + +inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) { + uchar2 yy = (uchar2)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3) + ); +#ifdef CL_DEBUG + if(yi >= RGB_SIZE) + printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE); +#endif + vstore2(yy, 0, out_yuv + yi); +} + +inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) { + const uchar4 yy = (uchar4)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3), + RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6), + RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1) + ); +#ifdef CL_DEBUG + if(yi > RGB_SIZE - 4) + printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4); +#endif + vstore4(yy, 0, out_yuv + yi); +} + +inline void convert_uv(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2) { + // U & V: average of 2x2 pixels square + const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); +#ifdef CL_DEBUG + if(ui >= RGB_SIZE + RGB_SIZE / 4) + printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4); + if(vi >= RGB_SIZE + RGB_SIZE / 2) + printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2); +#endif + out_yuv[ui] = RGB_TO_U(ar, ag, ab); + out_yuv[vi] = RGB_TO_V(ar, ag, ab); +} + +inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) { + // U & V: average of 2x2 pixels square + const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); + const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1); + const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2); + const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3); + uchar2 u2 = (uchar2)( + RGB_TO_U(ar1, ag1, ab1), + RGB_TO_U(ar2, ag2, ab2) + ); + uchar2 v2 = (uchar2)( + RGB_TO_V(ar1, ag1, ab1), + RGB_TO_V(ar2, ag2, ab2) + ); +#ifdef CL_DEBUG1 + if(ui > RGB_SIZE + RGB_SIZE / 4 - 2) + printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2); + if(vi > RGB_SIZE + RGB_SIZE / 2 - 2) + printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2); +#endif + vstore2(u2, 0, out_yuv + ui); + vstore2(v2, 0, out_yuv + vi); +} + +__kernel void rgb_to_yuv(__global uchar const * const rgb, + __global uchar * out_yuv) +{ + const int dx = get_global_id(0); + const int dy = get_global_id(1); + const int col = mul24(dx, 4); // Current column in rgb image + const int row = mul24(dy, 4); // Current row in rgb image + const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted + const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer + int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2); + int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2); + int num_col = min(WIDTH - col, 4); + int num_row = min(HEIGHT - row, 4); + if(num_row == 4) { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2); + const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8); + const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3); + const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0); + } + } else { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + } + } +} diff --git a/system/clocksd/.gitignore b/system/clocksd/.gitignore new file mode 100644 index 00000000000000..a6d841d65ea4f0 --- /dev/null +++ b/system/clocksd/.gitignore @@ -0,0 +1 @@ +clocksd diff --git a/system/clocksd/SConscript b/system/clocksd/SConscript new file mode 100644 index 00000000000000..d1cf13e9e8428a --- /dev/null +++ b/system/clocksd/SConscript @@ -0,0 +1,2 @@ +Import('env', 'common', 'cereal', 'messaging') +env.Program('clocksd.cc', LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/system/clocksd/clocksd.cc b/system/clocksd/clocksd.cc new file mode 100644 index 00000000000000..a5912cf51a0e14 --- /dev/null +++ b/system/clocksd/clocksd.cc @@ -0,0 +1,73 @@ +#include +#include +#include + +#include +#include + +// Apple doesn't have timerfd +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/timing.h" +#include "common/util.h" + +ExitHandler do_exit; + +int main() { + setpriority(PRIO_PROCESS, 0, -13); + PubMaster pm({"clocks"}); + +#ifndef __APPLE__ + int timerfd = timerfd_create(CLOCK_BOOTTIME, 0); + assert(timerfd >= 0); + + struct itimerspec spec = {0}; + spec.it_interval.tv_sec = 1; + spec.it_interval.tv_nsec = 0; + spec.it_value.tv_sec = 1; + spec.it_value.tv_nsec = 0; + + int err = timerfd_settime(timerfd, 0, &spec, 0); + assert(err == 0); + + uint64_t expirations = 0; + while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) { + if (err < 0) { + if (errno == EINTR) continue; + break; + } +#else + // Just run at 1Hz on apple + while (!do_exit) { + util::sleep_for(1000); +#endif + + uint64_t boottime = nanos_since_boot(); + uint64_t monotonic = nanos_monotonic(); + uint64_t monotonic_raw = nanos_monotonic_raw(); + uint64_t wall_time = nanos_since_epoch(); + + MessageBuilder msg; + auto clocks = msg.initEvent().initClocks(); + + clocks.setBootTimeNanos(boottime); + clocks.setMonotonicNanos(monotonic); + clocks.setMonotonicRawNanos(monotonic_raw); + clocks.setWallTimeNanos(wall_time); + + pm.send("clocks", msg); + } + +#ifndef __APPLE__ + close(timerfd); +#endif + return 0; +} diff --git a/system/hardware/.gitignore b/system/hardware/.gitignore new file mode 100644 index 00000000000000..980f09abfa7d56 --- /dev/null +++ b/system/hardware/.gitignore @@ -0,0 +1 @@ +eon/rat diff --git a/system/hardware/__init__.py b/system/hardware/__init__.py index 99079b5ef3b145..77bb0e5e2a4731 100644 --- a/system/hardware/__init__.py +++ b/system/hardware/__init__.py @@ -1,9 +1,9 @@ import os from typing import cast -from openpilot.system.hardware.base import HardwareBase -from openpilot.system.hardware.tici.hardware import Tici -from openpilot.system.hardware.pc.hardware import Pc +from system.hardware.base import HardwareBase +from system.hardware.tici.hardware import Tici +from system.hardware.pc.hardware import Pc TICI = os.path.isfile('/TICI') AGNOS = os.path.isfile('/AGNOS') diff --git a/system/hardware/base.h b/system/hardware/base.h index 3eded659acc38c..b70948d4820900 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -2,26 +2,27 @@ #include #include -#include -#include - -#include "cereal/gen/cpp/log.capnp.h" +#include "cereal/messaging/messaging.h" // no-op base hw class class HardwareNone { public: - static std::string get_name() { return ""; } - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; } - static int get_voltage() { return 0; } - static int get_current() { return 0; } + static constexpr float MAX_VOLUME = 0.7; + static constexpr float MIN_VOLUME = 0.2; - static std::string get_serial() { return "cccccc"; } + static std::string get_os_version() { return ""; } + static std::string get_name() { return ""; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }; + static int get_voltage() { return 0; }; + static int get_current() { return 0; }; - static std::map get_init_logs() { - return {}; - } + static void reboot() {} + static void poweroff() {} + static void set_brightness(int percent) {} + static void set_display_power(bool on) {} - static void set_ir_power(int percentage) {} + static bool get_ssh_enabled() { return false; } + static void set_ssh_enabled(bool enabled) {} static bool PC() { return false; } static bool TICI() { return false; } diff --git a/system/hardware/base.py b/system/hardware/base.py index c12c0758f5069e..31df1babe0f21b 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -1,101 +1,16 @@ -import os from abc import abstractmethod, ABC -from dataclasses import dataclass, fields +from collections import namedtuple +from typing import Dict from cereal import log +ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient', 'pmic']) NetworkType = log.DeviceState.NetworkType -NetworkStrength = log.DeviceState.NetworkStrength - -class LPAError(RuntimeError): - pass - -class LPAProfileNotFoundError(LPAError): - pass - -@dataclass -class Profile: - iccid: str - nickname: str - enabled: bool - provider: str - -@dataclass -class ThermalZone: - # a zone from /sys/class/thermal/thermal_zone* - name: str # a.k.a type - scale: float = 1000. # scale to get degrees in C - zone_number = -1 - - def read(self) -> float: - if self.zone_number < 0: - for n in os.listdir("/sys/devices/virtual/thermal"): - if not n.startswith("thermal_zone"): - continue - with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f: - if f.read().strip() == self.name: - self.zone_number = int(n.removeprefix("thermal_zone")) - break - try: - with open(f"/sys/devices/virtual/thermal/thermal_zone{self.zone_number}/temp") as f: - return int(f.read()) / self.scale - except FileNotFoundError: - return 0 - -@dataclass -class ThermalConfig: - cpu: list[ThermalZone] | None = None - gpu: list[ThermalZone] | None = None - dsp: ThermalZone | None = None - pmic: list[ThermalZone] | None = None - memory: ThermalZone | None = None - intake: ThermalZone | None = None - exhaust: ThermalZone | None = None - case: ThermalZone | None = None - - def get_msg(self): - ret = {} - for f in fields(ThermalConfig): - v = getattr(self, f.name) - if v is not None: - if isinstance(v, list): - ret[f.name + "TempC"] = [x.read() for x in v] - else: - ret[f.name + "TempC"] = v.read() - return ret - -class LPABase(ABC): - @abstractmethod - def list_profiles(self) -> list[Profile]: - pass - - @abstractmethod - def get_active_profile(self) -> Profile | None: - pass - - @abstractmethod - def delete_profile(self, iccid: str) -> None: - pass - - @abstractmethod - def download_profile(self, qr: str, nickname: str | None = None) -> None: - pass - - @abstractmethod - def nickname_profile(self, iccid: str, nickname: str) -> None: - pass - - @abstractmethod - 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]: + def get_cmdline() -> Dict[str, str]: with open('/proc/cmdline') as f: cmdline = f.read() return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2} @@ -108,99 +23,117 @@ def read_param_file(path, parser, default=0): except Exception: return default - def booted(self) -> bool: - return True - + @abstractmethod def reboot(self, reason=None): - print("REBOOT!") + pass + @abstractmethod def uninstall(self): - print("uninstall") + pass + @abstractmethod def get_os_version(self): - return None + pass @abstractmethod def get_device_type(self): pass + @abstractmethod + def get_sound_card_online(self): + pass + + @abstractmethod def get_imei(self, slot) -> str: - return "" + pass + @abstractmethod def get_serial(self): - return "" + pass + + @abstractmethod + def get_subscriber_info(self): + pass + @abstractmethod def get_network_info(self): - return None + pass + @abstractmethod def get_network_type(self): - return NetworkType.none + pass + @abstractmethod def get_sim_info(self): - return { - 'sim_id': '', - 'mcc_mnc': None, - 'network_type': ["Unknown"], - 'sim_state': ["ABSENT"], - 'data_connected': False - } - - def get_sim_lpa(self) -> LPABase: - raise NotImplementedError("SIM LPA not available") + pass + @abstractmethod def get_network_strength(self, network_type): - return NetworkStrength.unknown + pass def get_network_metered(self, network_type) -> bool: return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet) + @staticmethod + def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None: + pass + + @abstractmethod def get_current_power_draw(self): - return 0 + pass + @abstractmethod def get_som_power_draw(self): - return 0 + pass + @abstractmethod def shutdown(self): - print("SHUTDOWN!") + pass + @abstractmethod def get_thermal_config(self): - return ThermalConfig() - - def set_display_power(self, on: bool): pass + @abstractmethod def set_screen_brightness(self, percentage): pass + @abstractmethod def get_screen_brightness(self): - return 0 + pass + @abstractmethod def set_power_save(self, powersave_enabled): pass + @abstractmethod def get_gpu_usage_percent(self): - return 0 + pass def get_modem_version(self): return None + def get_modem_nv(self): + return None + + @abstractmethod def get_modem_temperatures(self): - return [] + pass - def initialize_hardware(self): + @abstractmethod + def get_nvme_temperatures(self): pass - def configure_modem(self): + @abstractmethod + def initialize_hardware(self): pass - def reboot_modem(self): + def configure_modem(self): pass + @abstractmethod def get_networks(self): - return None - - def has_internal_panda(self) -> bool: - return False + pass def reset_internal_panda(self): pass @@ -210,12 +143,3 @@ def recover_internal_panda(self): def get_modem_data_usage(self): return -1, -1 - - def get_voltage(self) -> float: - return 0. - - def get_current(self) -> float: - return 0. - - def set_ir_power(self, percent: int): - pass diff --git a/system/hardware/esim.py b/system/hardware/esim.py deleted file mode 100755 index 58ead6593f67b9..00000000000000 --- a/system/hardware/esim.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -import time -from openpilot.system.hardware import HARDWARE - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(prog='esim.py', description='manage eSIM profiles on your comma device', epilog='comma.ai') - 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)') - parser.add_argument('--download', nargs=2, metavar=('qr', 'name'), help='download a profile using QR code (format: LPA:1$rsp.truphone.com$QRF-SPEEDTEST)') - parser.add_argument('--nickname', nargs=2, metavar=('iccid', 'name'), help='update the nickname for a profile') - args = parser.parse_args() - - mutated = False - lpa = HARDWARE.get_sim_lpa() - if args.switch: - lpa.switch_profile(args.switch) - mutated = True - elif args.delete: - confirm = input('are you sure you want to delete this profile? (y/N) ') - if confirm == 'y': - lpa.delete_profile(args.delete) - mutated = True - else: - print('cancelled') - exit(0) - elif args.download: - lpa.download_profile(args.download[0], args.download[1]) - elif args.nickname: - lpa.nickname_profile(args.nickname[0], args.nickname[1]) - else: - parser.print_help() - - if mutated: - HARDWARE.reboot_modem() - # eUICC needs a small delay post-reboot before querying profiles - time.sleep(.5) - - profiles = lpa.list_profiles() - print(f'\n{len(profiles)} profile{"s" if len(profiles) > 1 else ""}:') - for p in profiles: - print(f'- {p.iccid} (nickname: {p.nickname or ""}) (provider: {p.provider}) - {"enabled" if p.enabled else "disabled"}') diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py deleted file mode 100755 index b2140d33d41ed6..00000000000000 --- a/system/hardware/fan_controller.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np - -from openpilot.common.pid import PIDController - - -class FanController: - def __init__(self, rate: int) -> None: - self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, rate=rate) - - def update(self, cur_temp: float, ignition: bool) -> int: - self.controller.pos_limit = 100 if ignition else 30 - self.controller.neg_limit = 30 if ignition else 0 - - if ignition != self.last_ignition: - self.controller.reset() - self.last_ignition = ignition - - return int(self.controller.update( - error=(cur_temp - 75), # temperature setpoint in C - feedforward=np.interp(cur_temp, [60.0, 100.0], [0, 100]) - )) diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py deleted file mode 100755 index aad30f77b2b32c..00000000000000 --- a/system/hardware/hardwared.py +++ /dev/null @@ -1,488 +0,0 @@ -#!/usr/bin/env python3 -import fcntl -import os -import queue -import struct -import threading -import time -from collections import OrderedDict, namedtuple - -import psutil - -import cereal.messaging as messaging -from cereal import log -from cereal.services import SERVICE_LIST -from openpilot.common.utils import strip_deprecated_keys -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.common.realtime import DT_HW -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.hardware import HARDWARE, TICI, AGNOS, PC -from openpilot.system.loggerd.config import get_available_percent -from openpilot.system.statsd import statlog -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware.power_monitoring import PowerMonitoring -from openpilot.system.hardware.fan_controller import FanController -from openpilot.system.version import terms_version, training_version -from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID - -ThermalStatus = log.DeviceState.ThermalStatus -NetworkType = log.DeviceState.NetworkType -NetworkStrength = log.DeviceState.NetworkStrength -CURRENT_TAU = 15. # 15s time constant -TEMP_TAU = 5. # 5s time constant -DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert -PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5) # 1.5x the expected pandaState frequency -ONROAD_CYCLE_TIME = 1 # seconds to wait offroad after requesting an onroad cycle - -ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) -HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', - 'network_metered', 'modem_temps']) - -# List of thermal bands. We will stay within this region as long as we are within the bounds. -# When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict. -THERMAL_BANDS = OrderedDict({ - ThermalStatus.green: ThermalBand(None, 80.0), - ThermalStatus.yellow: ThermalBand(75.0, 96.0), - ThermalStatus.red: ThermalBand(88.0, 107.), - ThermalStatus.danger: ThermalBand(94.0, None), -}) - -# Override to highest thermal band when offroad and above this temp -OFFROAD_DANGER_TEMP = 75 - -prev_offroad_states: dict[str, tuple[bool, str | None]] = {} - - - -def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None): - if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): - return - prev_offroad_states[offroad_alert] = (show_alert, extra_text) - set_offroad_alert(offroad_alert, show_alert, extra_text) - -def touch_thread(end_event): - count = 0 - - pm = messaging.PubMaster(["touch"]) - - event_format = "llHHi" - event_size = struct.calcsize(event_format) - event_frame = [] - - with open("/dev/input/by-path/platform-894000.i2c-event", "rb") as event_file: - fcntl.fcntl(event_file, fcntl.F_SETFL, os.O_NONBLOCK) - while not end_event.is_set(): - if (count % int(1. / DT_HW)) == 0: - event = event_file.read(event_size) - if event: - (sec, usec, etype, code, value) = struct.unpack(event_format, event) - if etype != 0 or code != 0 or value != 0: - touch = log.Touch.new_message() - touch.sec = sec - touch.usec = usec - touch.type = etype - touch.code = code - touch.value = value - event_frame.append(touch) - else: # end of frame, push new log - msg = messaging.new_message('touch', len(event_frame), valid=True) - msg.touch = event_frame - pm.send('touch', msg) - event_frame = [] - continue - - count += 1 - time.sleep(DT_HW) - - -def hw_state_thread(end_event, hw_queue): - """Handles non critical hardware state, and sends over queue""" - count = 0 - prev_hw_state = None - - modem_version = None - modem_configured = False - modem_missing_count = 0 - modem_restart_count = 0 - - while not end_event.is_set(): - # these are expensive calls. update every 10s - if (count % int(10. / DT_HW)) == 0: - try: - network_type = HARDWARE.get_network_type() - modem_temps = HARDWARE.get_modem_temperatures() - if len(modem_temps) == 0 and prev_hw_state is not None: - modem_temps = prev_hw_state.modem_temps - - # Log modem version once - if AGNOS and (modem_version is None): - modem_version = HARDWARE.get_modem_version() - - if modem_version is not None: - cloudlog.event("modem version", version=modem_version) - - if AGNOS and modem_restart_count < 3 and HARDWARE.get_modem_version() is None: - # TODO: we may be able to remove this with a MM update - # ModemManager's probing on startup can fail - # rarely, restart the service to probe again. - # Also, AT commands sometimes timeout resulting in ModemManager not - # trying to use this modem anymore. - modem_missing_count += 1 - if (modem_missing_count % 4) == 0: - modem_restart_count += 1 - cloudlog.event("restarting ModemManager") - os.system("sudo systemctl restart --no-block ModemManager") - - tx, rx = HARDWARE.get_modem_data_usage() - - hw_state = HardwareState( - network_type=network_type, - network_info=HARDWARE.get_network_info(), - network_strength=HARDWARE.get_network_strength(network_type), - network_stats={'wwanTx': tx, 'wwanRx': rx}, - network_metered=HARDWARE.get_network_metered(network_type), - modem_temps=modem_temps, - ) - - try: - hw_queue.put_nowait(hw_state) - except queue.Full: - pass - - if not modem_configured and HARDWARE.get_modem_version() is not None: - cloudlog.warning("configuring modem") - HARDWARE.configure_modem() - modem_configured = True - - prev_hw_state = hw_state - except Exception: - cloudlog.exception("Error getting hardware state") - - count += 1 - time.sleep(DT_HW) - - -def hardware_thread(end_event, hw_queue) -> None: - pm = messaging.PubMaster(['deviceState']) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates") - - count = 0 - - onroad_conditions: dict[str, bool] = { - "ignition": False, - "not_onroad_cycle": True, - "device_temp_good": True, - } - startup_conditions: dict[str, bool] = {} - startup_conditions_prev: dict[str, bool] = {} - - off_ts: float | None = None - started_ts: float | None = None - started_seen = False - startup_blocked_ts: float | None = None - thermal_status = ThermalStatus.yellow - - last_hw_state = HardwareState( - network_type=NetworkType.none, - network_info=None, - network_metered=False, - network_strength=NetworkStrength.unknown, - network_stats={'wwanTx': -1, 'wwanRx': -1}, - modem_temps=[], - ) - - all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False) - offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False) - should_start_prev = False - in_car = False - engaged_prev = False - pwrsave = False - offroad_cycle_count = 0 - - params = Params() - power_monitor = PowerMonitoring() - - uptime_offroad: float = params.get("UptimeOffroad", return_default=True) - uptime_onroad: float = params.get("UptimeOnroad", return_default=True) - last_uptime_ts: float = time.monotonic() - - HARDWARE.initialize_hardware() - thermal_config = HARDWARE.get_thermal_config() - - fan_controller = FanController(int(1./DT_HW)) - - while not end_event.is_set(): - sm.update(PANDA_STATES_TIMEOUT) - - pandaStates = sm['pandaStates'] - peripheralState = sm['peripheralState'] - - # handle requests to cycle system started state - if params.get_bool("OnroadCycleRequested"): - params.put_bool("OnroadCycleRequested", False) - offroad_cycle_count = sm.frame - onroad_conditions["not_onroad_cycle"] = (sm.frame - offroad_cycle_count) >= ONROAD_CYCLE_TIME * SERVICE_LIST['pandaStates'].frequency - - if sm.updated['pandaStates'] and len(pandaStates) > 0: - - # Set ignition based on any panda connected - onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown) - - pandaState = pandaStates[0] - - in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected - - elif (time.monotonic() - sm.recv_time['pandaStates']) > DISCONNECT_TIMEOUT: - if onroad_conditions["ignition"]: - onroad_conditions["ignition"] = False - cloudlog.error("panda timed out onroad") - - # Run at 2Hz, plus either edge of ignition - ign_edge = (started_ts is not None) != all(onroad_conditions.values()) - if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge: - continue - - msg = messaging.new_message('deviceState', valid=True) - msg.deviceState = thermal_config.get_msg() - msg.deviceState.deviceType = HARDWARE.get_device_type() - - try: - last_hw_state = hw_queue.get_nowait() - except queue.Empty: - pass - - msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) - msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) - msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) - online_cpu_usage = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] - offline_cpu_usage = [0., ] * (len(msg.deviceState.cpuTempC) - len(online_cpu_usage)) - msg.deviceState.cpuUsagePercent = online_cpu_usage + offline_cpu_usage - - msg.deviceState.networkType = last_hw_state.network_type - msg.deviceState.networkMetered = last_hw_state.network_metered - msg.deviceState.networkStrength = last_hw_state.network_strength - msg.deviceState.networkStats = last_hw_state.network_stats - if last_hw_state.network_info is not None: - msg.deviceState.networkInfo = last_hw_state.network_info - - msg.deviceState.modemTempC = last_hw_state.modem_temps - - msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() - - # this subset is only used for offroad - temp_sources = [ - msg.deviceState.memoryTempC, - max(msg.deviceState.cpuTempC, default=0.), - max(msg.deviceState.gpuTempC, default=0.), - ] - offroad_comp_temp = offroad_temp_filter.update(max(temp_sources)) - - # this drives the thermal status while onroad - temp_sources.append(max(msg.deviceState.pmicTempC, default=0.)) - all_comp_temp = all_temp_filter.update(max(temp_sources)) - msg.deviceState.maxTempC = all_comp_temp - - msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"]) - - is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5)) - if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP: - # if device is offroad and already hot without the extra onroad load, - # we want to cool down first before increasing load - thermal_status = ThermalStatus.danger - else: - current_band = THERMAL_BANDS[thermal_status] - band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) - if current_band.min_temp is not None and all_comp_temp < current_band.min_temp: - thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] - elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp: - thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] - - # **** starting logic **** - - startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") - startup_conditions["no_excessive_actuation"] = params.get("Offroad_ExcessiveActuation") is None - startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") - startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version - - # with 2% left, we killall, otherwise the phone will take a long time to boot - startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 - startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version - startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled") - startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot") - - # must be at an engageable thermal band to go onroad - startup_conditions["device_temp_engageable"] = thermal_status < ThermalStatus.red - - # ensure device is fully booted - startup_conditions["device_booted"] = startup_conditions.get("device_booted", False) or HARDWARE.booted() - - # if the temperature enters the danger zone, go offroad to cool down - onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger - extra_text = f"{offroad_comp_temp:.1f}C" - 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) - - # *** registration check *** - if not PC: - # we enforce this for our software, but you are welcome - # to make a different decision in your software - startup_conditions["registered_device"] = PC or (params.get("DongleId") != UNREGISTERED_DONGLE_ID) - - # Handle offroad/onroad transition - should_start = all(onroad_conditions.values()) - if started_ts is None: - should_start = should_start and all(startup_conditions.values()) - - if should_start != should_start_prev or (count == 0): - params.put_bool("IsEngaged", False) - engaged_prev = False - - if sm.updated['selfdriveState']: - engaged = sm['selfdriveState'].enabled - if engaged != engaged_prev: - params.put_bool("IsEngaged", engaged) - engaged_prev = engaged - - try: - with open('/dev/kmsg', 'w') as kmsg: - kmsg.write(f"<3>[hardware] engaged: {engaged}\n") - except Exception: - pass - - should_pwrsave = not onroad_conditions["ignition"] and msg.deviceState.screenBrightnessPercent < 1e-3 - if should_pwrsave != pwrsave or (count == 0): - HARDWARE.set_power_save(should_pwrsave) - pwrsave = should_pwrsave - - if should_start: - off_ts = None - if started_ts is None: - started_ts = time.monotonic() - started_seen = True - if startup_blocked_ts is not None: - cloudlog.event("Startup after block", block_duration=(time.monotonic() - startup_blocked_ts), - startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, - startup_conditions_prev=startup_conditions_prev, error=True) - startup_blocked_ts = None - else: - if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): - cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True) - startup_conditions_prev = startup_conditions.copy() - startup_blocked_ts = time.monotonic() - - started_ts = None - if off_ts is None: - off_ts = time.monotonic() - - # Offroad power monitoring - voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage - power_monitor.calculate(voltage, onroad_conditions["ignition"]) - msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() - msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) - current_power_draw = HARDWARE.get_current_power_draw() - statlog.sample("power_draw", current_power_draw) - msg.deviceState.powerDrawW = current_power_draw - - som_power_draw = HARDWARE.get_som_power_draw() - statlog.sample("som_power_draw", som_power_draw) - msg.deviceState.somPowerDrawW = som_power_draw - - # Check if we need to shut down - if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen): - cloudlog.warning(f"shutting device down, offroad since {off_ts}") - params.put_bool("DoShutdown", True) - - msg.deviceState.started = started_ts is not None - msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) - - last_ping = params.get("LastAthenaPingTime") - if last_ping is not None: - msg.deviceState.lastAthenaPingTime = last_ping - - msg.deviceState.thermalStatus = thermal_status - pm.send("deviceState", msg) - - # Log to statsd - statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) - statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) - statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent) - for i, usage in enumerate(msg.deviceState.cpuUsagePercent): - statlog.gauge(f"cpu{i}_usage_percent", usage) - for i, temp in enumerate(msg.deviceState.cpuTempC): - statlog.gauge(f"cpu{i}_temperature", temp) - for i, temp in enumerate(msg.deviceState.gpuTempC): - statlog.gauge(f"gpu{i}_temperature", temp) - statlog.gauge("memory_temperature", msg.deviceState.memoryTempC) - for i, temp in enumerate(msg.deviceState.pmicTempC): - statlog.gauge(f"pmic{i}_temperature", temp) - for i, temp in enumerate(last_hw_state.modem_temps): - statlog.gauge(f"modem_temperature{i}", temp) - statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired) - statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent) - - # report to server once every 10 minutes - rising_edge_started = should_start and not should_start_prev - if rising_edge_started or (count % int(600. / DT_HW)) == 0: - dat = { - 'count': count, - 'pandaStates': [strip_deprecated_keys(p.to_dict()) for p in pandaStates], - 'peripheralState': strip_deprecated_keys(peripheralState.to_dict()), - 'location': (strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), - 'deviceState': strip_deprecated_keys(msg.to_dict()) - } - cloudlog.event("STATUS_PACKET", **dat) - - # save last one before going onroad - if rising_edge_started: - try: - params.put("LastOffroadStatusPacket", dat) - except Exception: - cloudlog.exception("failed to save offroad status") - - params.put_bool_nonblocking("NetworkMetered", msg.deviceState.networkMetered) - - now_ts = time.monotonic() - if off_ts: - uptime_offroad += now_ts - max(last_uptime_ts, off_ts) - elif started_ts: - uptime_onroad += now_ts - max(last_uptime_ts, started_ts) - last_uptime_ts = now_ts - - if (count % int(60. / DT_HW)) == 0: - params.put("UptimeOffroad", uptime_offroad) - params.put("UptimeOnroad", uptime_onroad) - - count += 1 - should_start_prev = should_start - - -def main(): - hw_queue = queue.Queue(maxsize=1) - end_event = threading.Event() - - threads = [ - threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), - threading.Thread(target=hardware_thread, args=(end_event, hw_queue)), - ] - - if TICI: - threads.append(threading.Thread(target=touch_thread, args=(end_event,))) - - for t in threads: - t.start() - - try: - while True: - time.sleep(1) - if not all(t.is_alive() for t in threads): - break - finally: - end_event.set() - - for t in threads: - t.join() - - -if __name__ == "__main__": - main() diff --git a/system/hardware/hw.h b/system/hardware/hw.h index a9058401ceb9cd..f50e94abe1013e 100644 --- a/system/hardware/hw.h +++ b/system/hardware/hw.h @@ -1,58 +1,35 @@ #pragma once -#include - #include "system/hardware/base.h" #include "common/util.h" -#if __TICI__ +#if QCOM2 #include "system/hardware/tici/hardware.h" #define Hardware HardwareTici #else -#include "system/hardware/pc/hardware.h" +class HardwarePC : public HardwareNone { +public: + static std::string get_os_version() { return "openpilot for PC"; } + static std::string get_name() { return "pc"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; + static bool PC() { return true; } + static bool TICI() { return util::getenv("TICI", 0) == 1; } + static bool AGNOS() { return util::getenv("TICI", 0) == 1; } +}; #define Hardware HardwarePC #endif namespace Path { - inline std::string openpilot_prefix() { - return util::getenv("OPENPILOT_PREFIX", ""); - } - - inline std::string comma_home() { - return util::getenv("HOME") + "/.comma" + Path::openpilot_prefix(); - } - - inline std::string log_root() { - if (const char *env = getenv("LOG_ROOT")) { - return env; - } - return Hardware::PC() ? Path::comma_home() + "/media/0/realdata" : "/data/media/0/realdata"; - } - - inline std::string params() { - return util::getenv("PARAMS_ROOT", Hardware::PC() ? (Path::comma_home() + "/params") : "/data/params"); +inline std::string log_root() { + if (const char *env = getenv("LOG_ROOT")) { + return env; } - - inline std::string rsa_file() { - return Hardware::PC() ? Path::comma_home() + "/persist/comma/id_rsa" : "/persist/comma/id_rsa"; - } - - inline std::string swaglog_ipc() { - return "ipc:///tmp/logmessage" + Path::openpilot_prefix(); - } - - inline std::string download_cache_root() { - if (const char *env = getenv("COMMA_CACHE")) { - return env; - } - return "/tmp/comma_download_cache" + Path::openpilot_prefix() + "/"; - } - - inline std::string shm_path() { - #ifdef __APPLE__ - return"/tmp"; - #else - return "/dev/shm"; - #endif - } + return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata"; +} +inline std::string params() { + return Hardware::PC() ? util::getenv("HOME") + "/.comma/params" : "/data/params"; +} +inline std::string rsa_file() { + return Hardware::PC() ? util::getenv("HOME") + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa"; +} } // namespace Path diff --git a/system/hardware/hw.py b/system/hardware/hw.py deleted file mode 100644 index dc36dc047498a7..00000000000000 --- a/system/hardware/hw.py +++ /dev/null @@ -1,65 +0,0 @@ -import os -import platform -from pathlib import Path - -from openpilot.system.hardware import PC - -DEFAULT_DOWNLOAD_CACHE_ROOT = "/tmp/comma_download_cache" - -class Paths: - @staticmethod - def comma_home() -> str: - return os.path.join(str(Path.home()), ".comma" + os.environ.get("OPENPILOT_PREFIX", "")) - - @staticmethod - def log_root() -> str: - if os.environ.get('LOG_ROOT', False): - return os.environ['LOG_ROOT'] - elif PC: - return str(Path(Paths.comma_home()) / "media" / "0" / "realdata") - else: - return '/data/media/0/realdata/' - - @staticmethod - def swaglog_root() -> str: - if PC: - return os.path.join(Paths.comma_home(), "log") - else: - return "/data/log/" - - @staticmethod - def swaglog_ipc() -> str: - return "ipc:///tmp/logmessage" + os.environ.get("OPENPILOT_PREFIX", "") - - @staticmethod - def download_cache_root() -> str: - if os.environ.get('COMMA_CACHE', False): - return os.environ['COMMA_CACHE'] + "/" - return DEFAULT_DOWNLOAD_CACHE_ROOT + os.environ.get("OPENPILOT_PREFIX", "") + "/" - - @staticmethod - def persist_root() -> str: - if PC: - return os.path.join(Paths.comma_home(), "persist") - else: - return "/persist/" - - @staticmethod - def stats_root() -> str: - if PC: - return str(Path(Paths.comma_home()) / "stats") - else: - return "/data/stats/" - - @staticmethod - def config_root() -> str: - if PC: - return Paths.comma_home() - else: - return "/tmp/.comma" - - @staticmethod - def shm_path() -> str: - if PC and platform.system() == "Darwin": - return "/tmp" # This is not really shared memory on macOS, but it's the closest we can get - return "/dev/shm" diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h deleted file mode 100644 index 71f58b188bed01..00000000000000 --- a/system/hardware/pc/hardware.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include - -#include "system/hardware/base.h" - -class HardwarePC : public HardwareNone { -public: - static std::string get_name() { return "pc"; } - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; } - static bool PC() { return true; } - static bool TICI() { return util::getenv("TICI", 0) == 1; } - static bool AGNOS() { return util::getenv("TICI", 0) == 1; } -}; diff --git a/system/hardware/pc/hardware.py b/system/hardware/pc/hardware.py index f3d527429ce4a6..564f9e483add9d 100644 --- a/system/hardware/pc/hardware.py +++ b/system/hardware/pc/hardware.py @@ -1,12 +1,87 @@ +import random + from cereal import log -from openpilot.system.hardware.base import HardwareBase +from system.hardware.base import HardwareBase, ThermalConfig NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength class Pc(HardwareBase): + def get_os_version(self): + return None + def get_device_type(self): return "pc" + def get_sound_card_online(self): + return True + + def reboot(self, reason=None): + print("REBOOT!") + + def uninstall(self): + print("uninstall") + + def get_imei(self, slot): + return "%015d" % random.randint(0, 1 << 32) + + def get_serial(self): + return "cccccccc" + + def get_subscriber_info(self): + return "" + + def get_network_info(self): + return None + def get_network_type(self): return NetworkType.wifi + + def get_sim_info(self): + return { + 'sim_id': '', + 'mcc_mnc': None, + 'network_type': ["Unknown"], + 'sim_state': ["ABSENT"], + 'data_connected': False + } + + def get_network_strength(self, network_type): + return NetworkStrength.unknown + + def get_current_power_draw(self): + return 0 + + def get_som_power_draw(self): + return 0 + + def shutdown(self): + print("SHUTDOWN!") + + def get_thermal_config(self): + return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1), pmic=((None,), 1)) + + def set_screen_brightness(self, percentage): + pass + + def get_screen_brightness(self): + return 0 + + def set_power_save(self, powersave_enabled): + pass + + def get_gpu_usage_percent(self): + return 0 + + def get_modem_temperatures(self): + return [] + + def get_nvme_temperatures(self): + return [] + + def initialize_hardware(self): + pass + + def get_networks(self): + return None diff --git a/system/hardware/power_monitoring.py b/system/hardware/power_monitoring.py deleted file mode 100644 index f8b0e8b6291723..00000000000000 --- a/system/hardware/power_monitoring.py +++ /dev/null @@ -1,126 +0,0 @@ -import time -import threading - -from openpilot.common.params import Params -from openpilot.system.hardware import HARDWARE -from openpilot.common.swaglog import cloudlog -from openpilot.system.statsd import statlog - -CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1)) - -# While driving, a battery charges completely in about 30-60 minutes -CAR_BATTERY_CAPACITY_uWh = 30e6 -CAR_CHARGING_RATE_W = 45 - -VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery voltage -MAX_TIME_OFFROAD_S = 30*3600 -MIN_ON_TIME_S = 3600 -DELAY_SHUTDOWN_TIME_S = 300 # Wait at least DELAY_SHUTDOWN_TIME_S seconds after offroad_time to shutdown. -VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60 - -class PowerMonitoring: - def __init__(self): - self.params = Params() - self.last_measurement_time = None # Used for integration delta - self.last_save_time = 0 # Used for saving current value in a param - self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad - self.next_pulsed_measurement_time = None - self.car_voltage_mV = 12e3 # Low-passed version of peripheralState voltage - self.car_voltage_instant_mV = 12e3 # Last value of peripheralState voltage - self.integration_lock = threading.Lock() - - car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") or 0 - - # Reset capacity if it's low - self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), car_battery_capacity_uWh) - - # Calculation tick - def calculate(self, voltage: int | None, ignition: bool): - try: - now = time.monotonic() - - # If peripheralState is None, we're probably not in a car, so we don't care - if voltage is None: - with self.integration_lock: - self.last_measurement_time = None - self.next_pulsed_measurement_time = None - self.power_used_uWh = 0 - return - - # Low-pass battery voltage - self.car_voltage_instant_mV = voltage - self.car_voltage_mV = ((voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) - statlog.gauge("car_voltage", self.car_voltage_mV / 1e3) - - # Cap the car battery power and save it in a param every 10-ish seconds - self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) - self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh) - if now - self.last_save_time >= 10: - self.params.put_nonblocking("CarBatteryCapacity", int(self.car_battery_capacity_uWh)) - self.last_save_time = now - - # First measurement, set integration time - with self.integration_lock: - if self.last_measurement_time is None: - self.last_measurement_time = now - return - - if ignition: - # If there is ignition, we integrate the charging rate of the car - with self.integration_lock: - self.power_used_uWh = 0 - integration_time_h = (now - self.last_measurement_time) / 3600 - if integration_time_h < 0: - raise ValueError(f"Negative integration time: {integration_time_h}h") - self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h) - self.last_measurement_time = now - else: - # Get current power draw somehow - current_power = HARDWARE.get_current_power_draw() - - # Do the integration - self._perform_integration(now, current_power) - except Exception: - cloudlog.exception("Power monitoring calculation failed") - - def _perform_integration(self, t: float, current_power: float) -> None: - with self.integration_lock: - try: - if self.last_measurement_time: - integration_time_h = (t - self.last_measurement_time) / 3600 - power_used = (current_power * 1000000) * integration_time_h - if power_used < 0: - raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") - self.power_used_uWh += power_used - self.car_battery_capacity_uWh -= power_used - self.last_measurement_time = t - except Exception: - cloudlog.exception("Integration failed") - - # Get the power usage - def get_power_used(self) -> int: - return int(self.power_used_uWh) - - def get_car_battery_capacity(self) -> int: - return int(self.car_battery_capacity_uWh) - - # See if we need to shutdown - def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool): - if offroad_timestamp is None: - return False - - now = time.monotonic() - should_shutdown = False - offroad_time = (now - offroad_timestamp) - low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and - offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S) - should_shutdown |= offroad_time > MAX_TIME_OFFROAD_S - should_shutdown |= low_voltage_shutdown - should_shutdown |= (self.car_battery_capacity_uWh <= 0) - should_shutdown &= not ignition - should_shutdown &= (not self.params.get_bool("DisablePowerDown")) - should_shutdown &= in_car - should_shutdown &= offroad_time > DELAY_SHUTDOWN_TIME_S - should_shutdown |= self.params.get_bool("ForcePowerDown") - should_shutdown &= started_seen or (now > MIN_ON_TIME_S) - return should_shutdown diff --git a/system/hardware/tests/test_fan_controller.py b/system/hardware/tests/test_fan_controller.py deleted file mode 100644 index ee39a24f8748b4..00000000000000 --- a/system/hardware/tests/test_fan_controller.py +++ /dev/null @@ -1,50 +0,0 @@ -import pytest - -from openpilot.system.hardware.fan_controller import FanController - -ALL_CONTROLLERS = [FanController] - -def patched_controller(mocker, controller_class): - mocker.patch("os.system", new=mocker.Mock()) - return controller_class(2) - -class TestFanController: - def wind_up(self, controller, ignition=True): - for _ in range(1000): - controller.update(100, ignition) - - def wind_down(self, controller, ignition=False): - for _ in range(1000): - controller.update(10, ignition) - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_hot_onroad(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller) - assert controller.update(100, True) >= 70 - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_offroad_limits(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller) - assert controller.update(100, False) <= 30 - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_no_fan_wear(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_down(controller) - assert controller.update(10, False) == 0 - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_limited(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller, True) - assert controller.update(100, True) == 100 - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_windup_speed(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_down(controller, True) - for _ in range(10): - controller.update(90, True) - assert controller.update(90, True) >= 60 diff --git a/system/hardware/tests/test_power_monitoring.py b/system/hardware/tests/test_power_monitoring.py deleted file mode 100644 index 1dff6c6c5f728e..00000000000000 --- a/system/hardware/tests/test_power_monitoring.py +++ /dev/null @@ -1,199 +0,0 @@ -import pytest - -from openpilot.common.params import Params -from openpilot.system.hardware.power_monitoring import PowerMonitoring, CAR_BATTERY_CAPACITY_uWh, \ - CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING, DELAY_SHUTDOWN_TIME_S - -# Create fake time -ssb = 0. -def mock_time_monotonic(): - global ssb - ssb += 1. - return ssb - -TEST_DURATION_S = 50 -GOOD_VOLTAGE = 12 * 1e3 -VOLTAGE_BELOW_PAUSE_CHARGING = (VBATT_PAUSE_CHARGING - 1) * 1e3 - -def pm_patch(mocker, name, value, constant=False): - if constant: - mocker.patch(f"openpilot.system.hardware.power_monitoring.{name}", value) - else: - mocker.patch(f"openpilot.system.hardware.power_monitoring.{name}", return_value=value) - - -@pytest.fixture(autouse=True) -def mock_time(mocker): - mocker.patch("time.monotonic", mock_time_monotonic) - - -class TestPowerMonitoring: - def setup_method(self): - self.params = Params() - - # Test to see that it doesn't do anything when pandaState is None - def test_panda_state_present(self): - pm = PowerMonitoring() - for _ in range(10): - pm.calculate(None, None) - assert pm.get_power_used() == 0 - assert pm.get_car_battery_capacity() == (CAR_BATTERY_CAPACITY_uWh / 10) - - # Test to see that it doesn't integrate offroad when ignition is True - def test_offroad_ignition(self): - pm = PowerMonitoring() - for _ in range(10): - pm.calculate(GOOD_VOLTAGE, True) - assert pm.get_power_used() == 0 - - # Test to see that it integrates with discharging battery - def test_offroad_integration_discharging(self, mocker): - POWER_DRAW = 4 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - for _ in range(TEST_DURATION_S + 1): - pm.calculate(GOOD_VOLTAGE, False) - expected_power_usage = ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) - assert abs(pm.get_power_used() - expected_power_usage) < 10 - - # Test to check positive integration of car_battery_capacity - def test_car_battery_integration_onroad(self, mocker): - POWER_DRAW = 4 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = 0 - for _ in range(TEST_DURATION_S + 1): - pm.calculate(GOOD_VOLTAGE, True) - expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) - assert abs(pm.get_car_battery_capacity() - expected_capacity) < 10 - - # Test to check positive integration upper limit - def test_car_battery_integration_upper_limit(self, mocker): - POWER_DRAW = 4 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 - for _ in range(TEST_DURATION_S + 1): - pm.calculate(GOOD_VOLTAGE, True) - estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) - assert abs(pm.get_car_battery_capacity() - estimated_capacity) < 10 - - # Test to check negative integration of car_battery_capacity - def test_car_battery_integration_offroad(self, mocker): - POWER_DRAW = 4 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - for _ in range(TEST_DURATION_S + 1): - pm.calculate(GOOD_VOLTAGE, False) - expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) - assert abs(pm.get_car_battery_capacity() - expected_capacity) < 10 - - # Test to check negative integration lower limit - def test_car_battery_integration_lower_limit(self, mocker): - POWER_DRAW = 4 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = 1000 - for _ in range(TEST_DURATION_S + 1): - pm.calculate(GOOD_VOLTAGE, False) - estimated_capacity = 0 - ((1/3600) * POWER_DRAW * 1e6) - assert abs(pm.get_car_battery_capacity() - estimated_capacity) < 10 - - # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S - def test_max_time_offroad(self, mocker): - MOCKED_MAX_OFFROAD_TIME = 3600 - POWER_DRAW = 0 # To stop shutting down for other reasons - pm_patch(mocker, "MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True) - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - start_time = ssb - ignition = False - while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: - pm.calculate(GOOD_VOLTAGE, ignition) - if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: - assert not pm.should_shutdown(ignition, True, start_time, False) - assert pm.should_shutdown(ignition, True, start_time, False) - - def test_car_voltage(self, mocker): - POWER_DRAW = 0 # To stop shutting down for other reasons - TEST_TIME = 350 - VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 50 - pm_patch(mocker, "VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S", VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S, constant=True) - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - ignition = False - start_time = ssb - for i in range(TEST_TIME): - pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) - if i % 10 == 0: - assert pm.should_shutdown(ignition, True, start_time, True) == \ - (pm.car_voltage_mV < VBATT_PAUSE_CHARGING * 1e3 and \ - (ssb - start_time) > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S and \ - (ssb - start_time) > DELAY_SHUTDOWN_TIME_S) - assert pm.should_shutdown(ignition, True, start_time, True) - - # Test to check policy of not stopping charging when DisablePowerDown is set - def test_disable_power_down(self, mocker): - POWER_DRAW = 0 # To stop shutting down for other reasons - TEST_TIME = 100 - self.params.put_bool("DisablePowerDown", True) - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - ignition = False - for i in range(TEST_TIME): - pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) - if i % 10 == 0: - assert not pm.should_shutdown(ignition, True, ssb, False) - assert not pm.should_shutdown(ignition, True, ssb, False) - - # Test to check policy of not stopping charging when ignition - def test_ignition(self, mocker): - POWER_DRAW = 0 # To stop shutting down for other reasons - TEST_TIME = 100 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - ignition = True - for i in range(TEST_TIME): - pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) - if i % 10 == 0: - assert not pm.should_shutdown(ignition, True, ssb, False) - assert not pm.should_shutdown(ignition, True, ssb, False) - - # Test to check policy of not stopping charging when harness is not connected - def test_harness_connection(self, mocker): - POWER_DRAW = 0 # To stop shutting down for other reasons - TEST_TIME = 100 - pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - - ignition = False - for i in range(TEST_TIME): - pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) - if i % 10 == 0: - assert not pm.should_shutdown(ignition, False, ssb, False) - assert not pm.should_shutdown(ignition, False, ssb, False) - - def test_delay_shutdown_time(self): - pm = PowerMonitoring() - pm.car_battery_capacity_uWh = 0 - ignition = False - in_car = True - offroad_timestamp = ssb - started_seen = True - pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) - - while ssb < offroad_timestamp + DELAY_SHUTDOWN_TIME_S: - assert not pm.should_shutdown(ignition, in_car, - offroad_timestamp, - started_seen), \ - f"Should not shutdown before {DELAY_SHUTDOWN_TIME_S} seconds offroad time" - assert pm.should_shutdown(ignition, in_car, - offroad_timestamp, - started_seen), \ - f"Should shutdown after {DELAY_SHUTDOWN_TIME_S} seconds offroad time" diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index e33a26bb2e4c82..7ccea95ee78380 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,84 +1,52 @@ [ { - "name": "xbl", - "url": "https://commadist.azureedge.net/agnosupdate/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz", - "hash": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6", - "hash_raw": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6", - "size": 3282256, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "d47a08914d2376557b03f1231b7233508222c04b57d781f9daf77c63eab92c2e" - }, - { - "name": "xbl_config", - "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz", - "hash": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9", - "hash_raw": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9", - "size": 98124, + "name": "boot", + "url": "https://commadist.azureedge.net/agnosupdate/boot-243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8.img.xz", + "hash": "243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8", + "hash_raw": "243ddbb9e2256aa7af7fed0daf8cff4017a3c838c759373a634b8539f271bfb8", + "size": 14780416, "sparse": false, "full_check": true, - "has_ab": true, - "ondevice_hash": "e7d04d9f040c9c040cdf013335d0b6d6e9346311458baeb2461b193e954f5f1c" + "has_ab": true }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz", - "hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", - "hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", + "url": "https://commadist.azureedge.net/agnosupdate/abl-ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82.img.xz", + "hash": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", + "hash_raw": "ab4068f005ed9cb7fbca55c6d658880df1abfb1a4e6afb64f3fc5e64dac6fc82", "size": 274432, "sparse": false, "full_check": true, - "has_ab": true, - "ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee" + "has_ab": true }, { - "name": "aop", - "url": "https://commadist.azureedge.net/agnosupdate/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz", - "hash": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788", - "hash_raw": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788", - "size": 184364, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "3aa0a79149ec57f4bc8c38f7bbdf4f6630dd659e49a111ce6258d2d06a07c8e5" - }, - { - "name": "devcfg", - "url": "https://commadist.azureedge.net/agnosupdate/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz", - "hash": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585", - "hash_raw": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585", - "size": 40336, + "name": "xbl", + "url": "https://commadist.azureedge.net/agnosupdate/xbl-2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35.img.xz", + "hash": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", + "hash_raw": "2b1b67aa918cd127f2b0b4ed0a372f3a93676cf9d270bd3e56329516efdc5a35", + "size": 3670016, "sparse": false, "full_check": true, - "has_ab": true, - "ondevice_hash": "3d7bb33588491a2a40091a7e1cf6cb65e6dd503f69b640aba484d723f1ad47e8" + "has_ab": true }, { - "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756.img.xz", - "hash": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756", - "hash_raw": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756", - "size": 17496064, + "name": "xbl_config", + "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad.img.xz", + "hash": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", + "hash_raw": "3aa926394b4cec464300bfc0e7ab77d50889b38041138c60cd84c397930b38ad", + "size": 364544, "sparse": false, "full_check": true, - "has_ab": true, - "ondevice_hash": "0ee1ab104bb46d0f72e7d0b7d3e94629a7644a368896c6d4c558554fb955a08a" + "has_ab": true }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img.xz", - "hash": "7c58308be461126677ba02e9c9739556520ee02958934733867d86ecfe2e58e9", - "hash_raw": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd", - "size": 4718592000, + "url": "https://commadist.azureedge.net/agnosupdate/system-59622eddd068d49f2e9df69ef5115e3f205ad369539690a5b240c8c93796dd13.img.xz", + "hash": "44da205d17b44b2be7c94854a6bb3efb2928ec9a9889fe62af8b322d2295b74f", + "hash_raw": "59622eddd068d49f2e9df69ef5115e3f205ad369539690a5b240c8c93796dd13", + "size": 10737418240, "sparse": true, "full_check": false, - "has_ab": true, - "ondevice_hash": "826790516410c325aa30265846946d06a556f0a7b23c957f65fd11c055a663da", - "alt": { - "hash": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd", - "url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img", - "size": 4718592000 - } + "has_ab": true } -] \ No newline at end of file +] diff --git a/system/hardware/tici/agnos.py b/system/hardware/tici/agnos.py index f5261953d5c468..51998bf8b735ea 100755 --- a/system/hardware/tici/agnos.py +++ b/system/hardware/tici/agnos.py @@ -6,46 +6,37 @@ import struct import subprocess import time -from collections.abc import Generator +from typing import Dict, Generator, List, Tuple, Union import requests -import openpilot.system.updated.casync.casync as casync +import system.hardware.tici.casync as casync SPARSE_CHUNK_FMT = struct.Struct('H2xI4x') CAIBX_URL = "https://commadist.azureedge.net/agnosupdate/" -AGNOS_MANIFEST_FILE = "system/hardware/tici/agnos.json" - class StreamingDecompressor: def __init__(self, url: str) -> None: self.buf = b"" - self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}, timeout=60) + self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}) # type: ignore # pylint: disable=missing-timeout self.it = self.req.iter_content(chunk_size=1024 * 1024) self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO) self.eof = False self.sha256 = hashlib.sha256() def read(self, length: int) -> bytes: - while len(self.buf) < length and not self.eof: - if self.decompressor.needs_input: - self.req.raise_for_status() - - try: - compressed = next(self.it) - except StopIteration: - self.eof = True - break - else: - compressed = b'' + while len(self.buf) < length: + self.req.raise_for_status() - self.buf += self.decompressor.decompress(compressed, max_length=length) - - if self.decompressor.eof: + try: + compressed = next(self.it) + except StopIteration: self.eof = True break + out = self.decompressor.decompress(compressed) + self.buf += out result = self.buf[:length] self.buf = self.buf[length:] @@ -90,8 +81,8 @@ def unsparsify(f: StreamingDecompressor) -> Generator[bytes, None, None]: # noop wrapper with same API as unsparsify() for non sparse images def noop(f: StreamingDecompressor) -> Generator[bytes, None, None]: - while len(chunk := f.read(1024 * 1024)) > 0: - yield chunk + while not f.eof: + yield f.read(1024 * 1024) def get_target_slot_number() -> int: @@ -126,7 +117,7 @@ def get_raw_hash(path: str, partition_size: int) -> str: return raw_hash.hexdigest().lower() -def verify_partition(target_slot_number: int, partition: dict[str, str | int], force_full_check: bool = False) -> bool: +def verify_partition(target_slot_number: int, partition: Dict[str, Union[str, int]], force_full_check: bool = False) -> bool: full_check = partition['full_check'] or force_full_check path = get_partition_path(target_slot_number, partition) @@ -193,7 +184,7 @@ def extract_casync_image(target_slot_number: int, partition: dict, cloudlog): target = casync.parse_caibx(partition['casync_caibx']) - sources: list[tuple[str, casync.ChunkReader, casync.ChunkDict]] = [] + sources: List[Tuple[str, casync.ChunkReader, casync.ChunkDict]] = [] # First source is the current partition. try: diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json deleted file mode 100644 index b6718fe97db1b2..00000000000000 --- a/system/hardware/tici/all-partitions.json +++ /dev/null @@ -1,400 +0,0 @@ -[ - { - "name": "gpt_main_0", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_0-8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd.img.xz", - "hash": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd", - "hash_raw": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd", - "gpt": { - "lun": 0, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "gpt_main_1", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_1-fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6.img.xz", - "hash": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6", - "hash_raw": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6", - "gpt": { - "lun": 1, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "gpt_main_2", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_2-5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21.img.xz", - "hash": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21", - "hash_raw": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21", - "gpt": { - "lun": 2, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "gpt_main_3", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_3-c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159.img.xz", - "hash": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159", - "hash_raw": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159", - "gpt": { - "lun": 3, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "gpt_main_4", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_4-e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e.img.xz", - "hash": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e", - "hash_raw": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e", - "gpt": { - "lun": 4, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "gpt_main_5", - "url": "https://commadist.azureedge.net/agnosupdate/gpt_main_5-21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3.img.xz", - "hash": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3", - "hash_raw": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3", - "size": 24576, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3", - "gpt": { - "lun": 5, - "start_sector": 0, - "num_sectors": 6 - } - }, - { - "name": "persist", - "url": "https://commadist.azureedge.net/agnosupdate/persist-d6af4ec18df180c7417353b52a9e05e43a6480b29425f087874136436cefe786.img.xz", - "hash": "d6af4ec18df180c7417353b52a9e05e43a6480b29425f087874136436cefe786", - "hash_raw": "d6af4ec18df180c7417353b52a9e05e43a6480b29425f087874136436cefe786", - "size": 4096, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "d6af4ec18df180c7417353b52a9e05e43a6480b29425f087874136436cefe786" - }, - { - "name": "systemrw", - "url": "https://commadist.azureedge.net/agnosupdate/systemrw-8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e.img.xz", - "hash": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e", - "hash_raw": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e", - "size": 16777216, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e" - }, - { - "name": "cache", - "url": "https://commadist.azureedge.net/agnosupdate/cache-ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4.img.xz", - "hash": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4", - "hash_raw": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4", - "size": 134217728, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4" - }, - { - "name": "xbl", - "url": "https://commadist.azureedge.net/agnosupdate/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz", - "hash": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6", - "hash_raw": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6", - "size": 3282256, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "d47a08914d2376557b03f1231b7233508222c04b57d781f9daf77c63eab92c2e" - }, - { - "name": "xbl_config", - "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz", - "hash": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9", - "hash_raw": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9", - "size": 98124, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "e7d04d9f040c9c040cdf013335d0b6d6e9346311458baeb2461b193e954f5f1c" - }, - { - "name": "abl", - "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": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee" - }, - { - "name": "aop", - "url": "https://commadist.azureedge.net/agnosupdate/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz", - "hash": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788", - "hash_raw": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788", - "size": 184364, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "3aa0a79149ec57f4bc8c38f7bbdf4f6630dd659e49a111ce6258d2d06a07c8e5" - }, - { - "name": "bluetooth", - "url": "https://commadist.azureedge.net/agnosupdate/bluetooth-9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533.img.xz", - "hash": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533", - "hash_raw": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533", - "size": 1048576, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533" - }, - { - "name": "cmnlib64", - "url": "https://commadist.azureedge.net/agnosupdate/cmnlib64-1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3.img.xz", - "hash": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3", - "hash_raw": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3", - "size": 524288, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3" - }, - { - "name": "cmnlib", - "url": "https://commadist.azureedge.net/agnosupdate/cmnlib-63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82.img.xz", - "hash": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82", - "hash_raw": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82", - "size": 524288, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82" - }, - { - "name": "devcfg", - "url": "https://commadist.azureedge.net/agnosupdate/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz", - "hash": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585", - "hash_raw": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585", - "size": 40336, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "3d7bb33588491a2a40091a7e1cf6cb65e6dd503f69b640aba484d723f1ad47e8" - }, - { - "name": "devinfo", - "url": "https://commadist.azureedge.net/agnosupdate/devinfo-143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3.img.xz", - "hash": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3", - "hash_raw": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3", - "size": 4096, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3" - }, - { - "name": "dsp", - "url": "https://commadist.azureedge.net/agnosupdate/dsp-4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248.img.xz", - "hash": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248", - "hash_raw": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248", - "size": 33554432, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248" - }, - { - "name": "hyp", - "url": "https://commadist.azureedge.net/agnosupdate/hyp-ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927.img.xz", - "hash": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927", - "hash_raw": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927", - "size": 524288, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927" - }, - { - "name": "keymaster", - "url": "https://commadist.azureedge.net/agnosupdate/keymaster-5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04.img.xz", - "hash": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04", - "hash_raw": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04", - "size": 524288, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04" - }, - { - "name": "limits", - "url": "https://commadist.azureedge.net/agnosupdate/limits-94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1.img.xz", - "hash": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1", - "hash_raw": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1", - "size": 4096, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1" - }, - { - "name": "logfs", - "url": "https://commadist.azureedge.net/agnosupdate/logfs-b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220.img.xz", - "hash": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220", - "hash_raw": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220", - "size": 8388608, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220" - }, - { - "name": "modem", - "url": "https://commadist.azureedge.net/agnosupdate/modem-a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994.img.xz", - "hash": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994", - "hash_raw": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994", - "size": 125829120, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994" - }, - { - "name": "qupfw", - "url": "https://commadist.azureedge.net/agnosupdate/qupfw-64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a.img.xz", - "hash": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a", - "hash_raw": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a", - "size": 65536, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a" - }, - { - "name": "splash", - "url": "https://commadist.azureedge.net/agnosupdate/splash-5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08.img.xz", - "hash": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08", - "hash_raw": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08", - "size": 34226176, - "sparse": false, - "full_check": true, - "has_ab": false, - "ondevice_hash": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08" - }, - { - "name": "storsec", - "url": "https://commadist.azureedge.net/agnosupdate/storsec-4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce.img.xz", - "hash": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce", - "hash_raw": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce", - "size": 131072, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce" - }, - { - "name": "tz", - "url": "https://commadist.azureedge.net/agnosupdate/tz-e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16.img.xz", - "hash": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16", - "hash_raw": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16", - "size": 2097152, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16" - }, - { - "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756.img.xz", - "hash": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756", - "hash_raw": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756", - "size": 17496064, - "sparse": false, - "full_check": true, - "has_ab": true, - "ondevice_hash": "0ee1ab104bb46d0f72e7d0b7d3e94629a7644a368896c6d4c558554fb955a08a" - }, - { - "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img.xz", - "hash": "7c58308be461126677ba02e9c9739556520ee02958934733867d86ecfe2e58e9", - "hash_raw": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd", - "size": 4718592000, - "sparse": true, - "full_check": false, - "has_ab": true, - "ondevice_hash": "826790516410c325aa30265846946d06a556f0a7b23c957f65fd11c055a663da", - "alt": { - "hash": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd", - "url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img", - "size": 4718592000 - } - }, - { - "name": "userdata_90", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-ec31b8116125a95755adb32853c401c462a14a74f538535532bf2c34d72c60eb.img.xz", - "hash": "aa0f0fe32187493e6135aee9e984d3f9705fc58560d537b34687bb6b51a38428", - "hash_raw": "ec31b8116125a95755adb32853c401c462a14a74f538535532bf2c34d72c60eb", - "size": 96636764160, - "sparse": true, - "full_check": true, - "has_ab": false, - "ondevice_hash": "9c916b7d05543d4608b0401bc867639f44ce9671639a1a6da83b6d58b4eaa1b4" - }, - { - "name": "userdata_89", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-7f092cc841124c10300e43574e90e3367e983bfbe4faa0969024e79e5ce90b11.img.xz", - "hash": "fa83d4b7096857136820b0b0a8785c90677256b054c5c14039cd7b9b1065a90b", - "hash_raw": "7f092cc841124c10300e43574e90e3367e983bfbe4faa0969024e79e5ce90b11", - "size": 95563022336, - "sparse": true, - "full_check": true, - "has_ab": false, - "ondevice_hash": "1699e38de769eb32c21dfa6a5ac21eb3ad620a362c7b8abf1a2c0afe0f717530" - }, - { - "name": "userdata_30", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-3df2dcd5e1f426c90b090fdbcd1a95b035d96a4bdaf88d5517245db5ee84f5ed.img.xz", - "hash": "890910f20b1ad88a728ee822a47b1234eb3d70cab28ca8a935679c8c2d33cbe9", - "hash_raw": "3df2dcd5e1f426c90b090fdbcd1a95b035d96a4bdaf88d5517245db5ee84f5ed", - "size": 32212254720, - "sparse": true, - "full_check": true, - "has_ab": false, - "ondevice_hash": "8e7cb392dd6e49c7d59fa850be7d1f44901314c86ba9c88be5bb27a0cd1123c9" - } -] \ No newline at end of file diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py index 09436e6ff4625a..a8b27986308aaa 100755 --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -1,21 +1,37 @@ -#!/usr/bin/env python3 -import time +#!/usr/bin/env python +from smbus2 import SMBus from collections import namedtuple -from openpilot.common.i2c import SMBus - # https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask']) - -CONFIG = [ +EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2']) + +def configs_from_eq_params(base, eq_params): + return [ + AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF), + AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF), + AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF), + AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF), + AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF), + AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF), + AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF), + AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF), + AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF), + AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF), + ] + +BASE_CONFIG = [ AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000), AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000), AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011), + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000), AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000), AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100), AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100), + AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000), AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000), AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111), @@ -26,6 +42,7 @@ AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000), AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000), AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001), + AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000), AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111), AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000), @@ -44,31 +61,14 @@ AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000), AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000), AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000), - - AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), - AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), - AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), - AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), - AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), - AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), - - AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), - AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), - AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), - AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), - AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000), - AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111), - AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000), - AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001), - AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000), - AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000), - AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000), - AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000), - AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), - AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), - AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), ] +BASE_CONFIG += configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)) +BASE_CONFIG += configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)) +BASE_CONFIG += configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)) +BASE_CONFIG += configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)) +BASE_CONFIG += configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)) + class Amplifier: AMP_I2C_BUS = 0 AMP_ADDRESS = 0x10 @@ -76,48 +76,29 @@ class Amplifier: def __init__(self, debug=False): self.debug = debug - def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig: - return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000) - - def _set_configs(self, configs: list[AmpConfig]) -> None: + def set_config(self, config): with SMBus(self.AMP_I2C_BUS) as bus: - for config in configs: - if self.debug: - print(f"Setting \"{config.name}\" to {config.value}:") - - old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) - new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) - bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) - - if self.debug: - print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") - - def set_configs(self, configs: list[AmpConfig]) -> bool: - # retry in case panda is using the amp - tries = 15 - backoff = 0. - for i in range(tries): - try: - self._set_configs(configs) - return True - except OSError: - backoff += 0.1 - time.sleep(backoff) - print(f"Failed to set amp config, {tries - i - 1} retries left") - return False - - def set_global_shutdown(self, amp_disabled: bool) -> bool: - return self.set_configs([self._get_shutdown_config(amp_disabled), ]) - - def initialize_configuration(self) -> bool: - cfgs = [ - self._get_shutdown_config(True), - *CONFIG, - self._get_shutdown_config(False), - ] - return self.set_configs(cfgs) + if self.debug: + print(f"Setting \"{config.name}\" to {config.value}:") + + old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) + new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) + bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) + + if self.debug: + print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") + + def set_global_shutdown(self, amp_disabled): + self.set_config(AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)) + + def initialize_configuration(self): + self.set_global_shutdown(amp_disabled=True) + + for config in BASE_CONFIG: + self.set_config(config) + + self.set_global_shutdown(amp_disabled=False) if __name__ == "__main__": - amp = Amplifier() - amp.initialize_configuration() + Amplifier(debug=True).initialize_configuration() diff --git a/system/hardware/tici/casync.py b/system/hardware/tici/casync.py new file mode 100755 index 00000000000000..8ae42fa7149813 --- /dev/null +++ b/system/hardware/tici/casync.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python3 +import io +import lzma +import os +import struct +import sys +import time +from abc import ABC, abstractmethod +from collections import defaultdict, namedtuple +from typing import Callable, Dict, List, Optional, Tuple + +import requests +from Crypto.Hash import SHA512 + +CA_FORMAT_INDEX = 0x96824d9c7b129ff9 +CA_FORMAT_TABLE = 0xe75b9e112f17417d +CA_FORMAT_TABLE_TAIL_MARKER = 0xe75b9e112f17417 +FLAGS = 0xb000000000000000 + +CA_HEADER_LEN = 48 +CA_TABLE_HEADER_LEN = 16 +CA_TABLE_ENTRY_LEN = 40 +CA_TABLE_MIN_LEN = CA_TABLE_HEADER_LEN + CA_TABLE_ENTRY_LEN + +CHUNK_DOWNLOAD_TIMEOUT = 60 +CHUNK_DOWNLOAD_RETRIES = 3 + +CAIBX_DOWNLOAD_TIMEOUT = 120 + +Chunk = namedtuple('Chunk', ['sha', 'offset', 'length']) +ChunkDict = Dict[bytes, Chunk] + + +class ChunkReader(ABC): + @abstractmethod + def read(self, chunk: Chunk) -> bytes: + ... + + +class FileChunkReader(ChunkReader): + """Reads chunks from a local file""" + def __init__(self, fn: str) -> None: + super().__init__() + self.f = open(fn, 'rb') + + def __del__(self): + self.f.close() + + def read(self, chunk: Chunk) -> bytes: + self.f.seek(chunk.offset) + return self.f.read(chunk.length) + + +class RemoteChunkReader(ChunkReader): + """Reads lzma compressed chunks from a remote store""" + + def __init__(self, url: str) -> None: + super().__init__() + self.url = url + self.session = requests.Session() + + def read(self, chunk: Chunk) -> bytes: + sha_hex = chunk.sha.hex() + url = os.path.join(self.url, sha_hex[:4], sha_hex + ".cacnk") + + if os.path.isfile(url): + with open(url, 'rb') as f: + contents = f.read() + else: + for i in range(CHUNK_DOWNLOAD_RETRIES): + try: + resp = self.session.get(url, timeout=CHUNK_DOWNLOAD_TIMEOUT) + break + except Exception: + if i == CHUNK_DOWNLOAD_RETRIES - 1: + raise + time.sleep(CHUNK_DOWNLOAD_TIMEOUT) + + resp.raise_for_status() + contents = resp.content + + decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO) + return decompressor.decompress(contents) + + +def parse_caibx(caibx_path: str) -> List[Chunk]: + """Parses the chunks from a caibx file. Can handle both local and remote files. + Returns a list of chunks with hash, offset and length""" + if os.path.isfile(caibx_path): + caibx = open(caibx_path, 'rb') + else: + resp = requests.get(caibx_path, timeout=CAIBX_DOWNLOAD_TIMEOUT) + resp.raise_for_status() + caibx = io.BytesIO(resp.content) + + caibx.seek(0, os.SEEK_END) + caibx_len = caibx.tell() + caibx.seek(0, os.SEEK_SET) + + # Parse header + length, magic, flags, min_size, _, max_size = struct.unpack("= min_size + + chunks.append(Chunk(sha, offset, length)) + offset = new_offset + + caibx.close() + return chunks + + +def build_chunk_dict(chunks: List[Chunk]) -> ChunkDict: + """Turn a list of chunks into a dict for faster lookups based on hash. + Keep first chunk since it's more likely to be already downloaded.""" + r = {} + for c in chunks: + if c.sha not in r: + r[c.sha] = c + return r + + +def extract(target: List[Chunk], + sources: List[Tuple[str, ChunkReader, ChunkDict]], + out_path: str, + progress: Optional[Callable[[int], None]] = None): + stats: Dict[str, int] = defaultdict(int) + + mode = 'rb+' if os.path.exists(out_path) else 'wb' + with open(out_path, mode) as out: + for cur_chunk in target: + + # Find source for desired chunk + for name, chunk_reader, store_chunks in sources: + if cur_chunk.sha in store_chunks: + bts = chunk_reader.read(store_chunks[cur_chunk.sha]) + + # Check length + if len(bts) != cur_chunk.length: + continue + + # Check hash + if SHA512.new(bts, truncate="256").digest() != cur_chunk.sha: + continue + + # Write to output + out.seek(cur_chunk.offset) + out.write(bts) + + stats[name] += cur_chunk.length + + if progress is not None: + progress(sum(stats.values())) + + break + else: + raise RuntimeError("Desired chunk not found in provided stores") + + return stats + + +def print_stats(stats: Dict[str, int]): + total_bytes = sum(stats.values()) + print(f"Total size: {total_bytes / 1024 / 1024:.2f} MB") + for name, total in stats.items(): + print(f" {name}: {total / 1024 / 1024:.2f} MB ({total / total_bytes * 100:.1f}%)") + + +def extract_simple(caibx_path, out_path, store_path): + # (name, callback, chunks) + target = parse_caibx(caibx_path) + sources = [ + # (store_path, RemoteChunkReader(store_path), build_chunk_dict(target)), + (store_path, FileChunkReader(store_path), build_chunk_dict(target)), + ] + + return extract(target, sources, out_path) + + +if __name__ == "__main__": + caibx = sys.argv[1] + out = sys.argv[2] + store = sys.argv[3] + + stats = extract_simple(caibx, out, store) + print_stats(stats) diff --git a/system/hardware/tici/esim.nmconnection b/system/hardware/tici/esim.nmconnection deleted file mode 100644 index 74f6f8e82c74dd..00000000000000 --- a/system/hardware/tici/esim.nmconnection +++ /dev/null @@ -1,30 +0,0 @@ -[connection] -id=esim -uuid=fff6553c-3284-4707-a6b1-acc021caaafb -type=gsm -permissions= -autoconnect=true -autoconnect-retries=100 -autoconnect-priority=2 -metered=1 - -[gsm] -apn= -home-only=false -auto-config=true -sim-id= - -[ipv4] -route-metric=1000 -dns-priority=1000 -dns-search= -method=auto - -[ipv6] -ddr-gen-mode=stable-privacy -dns-search= -route-metric=1000 -dns-priority=1000 -method=auto - -[proxy] diff --git a/system/hardware/tici/esim.py b/system/hardware/tici/esim.py deleted file mode 100644 index b489286f50bbd0..00000000000000 --- a/system/hardware/tici/esim.py +++ /dev/null @@ -1,106 +0,0 @@ -import json -import os -import shutil -import subprocess -from typing import Literal - -from openpilot.system.hardware.base import LPABase, LPAError, LPAProfileNotFoundError, Profile - -class TiciLPA(LPABase): - def __init__(self, interface: Literal['qmi', 'at'] = 'qmi'): - self.env = os.environ.copy() - self.env['LPAC_APDU'] = interface - self.env['QMI_DEVICE'] = '/dev/cdc-wdm0' - self.env['AT_DEVICE'] = '/dev/ttyUSB2' - - self.timeout_sec = 45 - - if shutil.which('lpac') is None: - raise LPAError('lpac not found, must be installed!') - - def list_profiles(self) -> list[Profile]: - msgs = self._invoke('profile', 'list') - self._validate_successful(msgs) - return [Profile( - iccid=p['iccid'], - nickname=p['profileNickname'], - enabled=p['profileState'] == 'enabled', - provider=p['serviceProviderName'] - ) for p in msgs[-1]['payload']['data']] - - def get_active_profile(self) -> Profile | None: - return next((p for p in self.list_profiles() if p.enabled), None) - - def delete_profile(self, iccid: str) -> None: - self._validate_profile_exists(iccid) - latest = self.get_active_profile() - if latest is not None and latest.iccid == iccid: - raise LPAError('cannot delete active profile, switch to another profile first') - self._validate_successful(self._invoke('profile', 'delete', iccid)) - self._process_notifications() - - def download_profile(self, qr: str, nickname: str | None = None) -> None: - 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) - if new_profile is None: - raise LPAError('no new profile found') - if nickname: - self.nickname_profile(new_profile['payload']['data']['iccid'], nickname) - self._process_notifications() - - def nickname_profile(self, iccid: str, nickname: str) -> None: - self._validate_profile_exists(iccid) - self._validate_successful(self._invoke('profile', 'nickname', iccid, nickname)) - - def switch_profile(self, iccid: str) -> None: - self._validate_profile_exists(iccid) - latest = self.get_active_profile() - if latest and latest.iccid == iccid: - return - self._validate_successful(self._invoke('profile', 'enable', iccid)) - self._process_notifications() - - def _invoke(self, *cmd: str): - proc = subprocess.Popen(['sudo', '-E', 'lpac'] + list(cmd), stdout=subprocess.PIPE, stderr=subprocess.PIPE, env=self.env) - try: - out, err = proc.communicate(timeout=self.timeout_sec) - except subprocess.TimeoutExpired as e: - proc.kill() - raise LPAError(f"lpac {cmd} timed out after {self.timeout_sec} seconds") from e - - messages = [] - for line in out.decode().strip().splitlines(): - if line.startswith('{'): - message = json.loads(line) - - # lpac response format validations - assert 'type' in message, 'expected type in message' - assert message['type'] == 'lpa' or message['type'] == 'progress', 'expected lpa or progress message type' - assert 'payload' in message, 'expected payload in message' - assert 'code' in message['payload'], 'expected code in message payload' - assert 'data' in message['payload'], 'expected data in message payload' - - msg_ret_code = message['payload']['code'] - if msg_ret_code != 0: - raise LPAError(f"lpac {' '.join(cmd)} failed with code {msg_ret_code}: <{message['payload']['message']}> {message['payload']['data']}") - - messages.append(message) - - if len(messages) == 0: - raise LPAError(f"lpac {cmd} returned no messages") - - return messages - - def _process_notifications(self) -> None: - """ - Process notifications stored on the eUICC, typically to activate/deactivate the profile with the carrier. - """ - self._validate_successful(self._invoke('notification', 'process', '-a', '-r')) - - def _validate_profile_exists(self, iccid: str) -> None: - if not any(p.iccid == iccid for p in self.list_profiles()): - raise LPAProfileNotFoundError(f'profile {iccid} does not exist') - - def _validate_successful(self, msgs: list[dict]) -> None: - assert msgs[-1]['payload']['message'] == 'success', 'expected success notification' diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index d59b45efcb96e4..dcccb9f3d1f6fb 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -1,90 +1,44 @@ #pragma once #include -#include #include -#include -#include -#include // for std::clamp +#include "common/params.h" #include "common/util.h" #include "system/hardware/base.h" class HardwareTici : public HardwareNone { public: - static std::string get_name() { - std::string model = util::read_file("/sys/firmware/devicetree/base/model"); - return util::strip(model.substr(std::string("comma ").size())); - } - - static cereal::InitData::DeviceType get_device_type() { - static const std::map device_map = { - {"tici", cereal::InitData::DeviceType::TICI}, - {"tizi", cereal::InitData::DeviceType::TIZI}, - {"mici", cereal::InitData::DeviceType::MICI} - }; - auto it = device_map.find(get_name()); - assert(it != device_map.end()); - return it->second; - } - - static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); } - static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); } - - static std::string get_serial() { - static std::string serial(""); - if (serial.empty()) { - std::ifstream stream("/proc/cmdline"); - std::string cmdline; - std::getline(stream, cmdline); - - auto start = cmdline.find("serialno="); - if (start == std::string::npos) { - serial = "cccccc"; - } else { - auto end = cmdline.find(" ", start + 9); - serial = cmdline.substr(start + 9, end - start - 9); - } - } - return serial; - } - - static void set_ir_power(int percent) { - auto device = get_device_type(); - if (device == cereal::InitData::DeviceType::TICI || - device == cereal::InitData::DeviceType::TIZI) { - return; + static constexpr float MAX_VOLUME = 0.9; + static constexpr float MIN_VOLUME = 0.2; + static bool TICI() { return true; } + static bool AGNOS() { return true; } + static std::string get_os_version() { + return "AGNOS " + util::read_file("/VERSION"); + }; + static std::string get_name() { return "tici"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; }; + static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }; + static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }; + + + static void reboot() { std::system("sudo reboot"); }; + static void poweroff() { std::system("sudo poweroff"); }; + static void set_brightness(int percent) { + std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness"); + if (brightness_control.is_open()) { + brightness_control << (percent * (int)(1023/100.)) << "\n"; + brightness_control.close(); } - - int value = util::map_val(std::clamp(percent, 0, 100), 0, 100, 0, 300); - std::ofstream("/sys/class/leds/led:switch_2/brightness") << 0 << "\n"; - std::ofstream("/sys/class/leds/led:torch_2/brightness") << value << "\n"; - std::ofstream("/sys/class/leds/led:switch_2/brightness") << value << "\n"; - } - - static std::map get_init_logs() { - std::map ret = { - {"/BUILD", util::read_file("/BUILD")}, - {"lsblk", util::check_output("lsblk -o NAME,SIZE,STATE,VENDOR,MODEL,REV,SERIAL")}, - {"SOM ID", util::read_file("/sys/devices/platform/vendor/vendor:gpio-som-id/som_id")}, - }; - - std::string bs = util::check_output("abctl --boot_slot"); - ret["boot slot"] = bs.substr(0, bs.find_first_of("\n")); - - std::string temp = util::read_file("/dev/disk/by-partlabel/ssd"); - temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1); - ret["boot temp"] = temp; - - // TODO: log something from system and boot - for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) { - for (std::string slot : {"a", "b"}) { - std::string partition = part + "_" + slot; - std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition); - ret[partition] = hash.substr(0, hash.find_first_of(" ")); - } + }; + static void set_display_power(bool on) { + std::ofstream bl_power_control("/sys/class/backlight/panel0-backlight/bl_power"); + if (bl_power_control.is_open()) { + bl_power_control << (on ? "0" : "4") << "\n"; + bl_power_control.close(); } + }; - return ret; - } + static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; + static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; }; diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 5a84afce03b29c..340093b6043896 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -1,20 +1,18 @@ +import json import math import os import subprocess import time -import tempfile from enum import IntEnum -from functools import cached_property, lru_cache +from functools import cached_property from pathlib import Path from cereal import log -from openpilot.common.utils import sudo_read, sudo_write -from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action -from openpilot.system.hardware.base import HardwareBase, LPABase, ThermalConfig, ThermalZone -from openpilot.system.hardware.tici import iwlist -from openpilot.system.hardware.tici.esim import TiciLPA -from openpilot.system.hardware.tici.pins import GPIO -from openpilot.system.hardware.tici.amplifier import Amplifier +from common.gpio import gpio_set, gpio_init +from system.hardware.base import HardwareBase, ThermalConfig +from system.hardware.tici import iwlist +from system.hardware.tici.pins import GPIO +from system.hardware.tici.amplifier import Amplifier NM = 'org.freedesktop.NetworkManager' NM_CON_ACT = NM + '.Connection.Active' @@ -62,40 +60,29 @@ class NMMetered(IntEnum): MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 -def affine_irq(val, action): - irqs = get_irqs_for_action(action) - if len(irqs) == 0: - print(f"No IRQs found for '{action}'") - return +def sudo_write(val, path): + os.system(f"sudo su -c 'echo {val} > {path}'") - for i in irqs: - sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list") +def affine_irq(val, irq): + sudo_write(str(val), f"/proc/irq/{irq}/smp_affinity_list") -@lru_cache -def get_device_type(): - # lru_cache and cache can cause memory leaks when used in classes - with open("/sys/firmware/devicetree/base/model") as f: - model = f.read().strip('\x00') - return model.split('comma ')[-1] class Tici(HardwareBase): @cached_property def bus(self): - import dbus + import dbus # pylint: disable=import-error return dbus.SystemBus() @cached_property def nm(self): return self.bus.get_object(NM, '/org/freedesktop/NetworkManager') - @property # this should not be cached, in case the modemmanager restarts + @cached_property def mm(self): return self.bus.get_object(MM, '/org/freedesktop/ModemManager1') @cached_property def amplifier(self): - if self.get_device_type() == "mici": - return None return Amplifier() def get_os_version(self): @@ -103,7 +90,11 @@ def get_os_version(self): return f.read().strip() def get_device_type(self): - return get_device_type() + return "tici" + + def get_sound_card_online(self): + return (os.path.isfile('/proc/asound/card0/state') and + open('/proc/asound/card0/state').read().strip() == 'ONLINE') def reboot(self, reason=None): subprocess.check_output(["sudo", "reboot"]) @@ -116,26 +107,6 @@ def uninstall(self): def get_serial(self): return self.get_cmdline()['androidboot.serialno'] - def get_voltage(self): - with open("/sys/class/hwmon/hwmon1/in1_input") as f: - return int(f.read()) - - def get_current(self): - with open("/sys/class/hwmon/hwmon1/curr1_input") as f: - return int(f.read()) - - def set_ir_power(self, percent: int): - if self.get_device_type() == "tizi": - return - - value = int((percent / 100) * 300) - with open("/sys/class/leds/led:switch_2/brightness", "w") as f: - f.write("0\n") - with open("/sys/class/leds/led:torch_2/brightness", "w") as f: - f.write(f"{value}\n") - with open("/sys/class/leds/led:switch_2/brightness", "w") as f: - f.write(f"{value}\n") - def get_network_type(self): try: primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) @@ -200,8 +171,8 @@ def get_sim_info(self): 'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED, } - def get_sim_lpa(self) -> LPABase: - return TiciLPA() + def get_subscriber_info(self): + return "" def get_imei(self, slot): if slot != 0: @@ -210,10 +181,8 @@ def get_imei(self, slot): return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) def get_network_info(self): - if self.get_device_type() == "mici": - return None + modem = self.get_modem() try: - modem = self.get_modem() info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) @@ -294,6 +263,65 @@ def get_network_metered(self, network_type) -> bool: return super().get_network_metered(network_type) + @staticmethod + def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None: + upload_speed_kbps = int(upload_speed_kbps) # Ensure integer value + download_speed_kbps = int(download_speed_kbps) # Ensure integer value + + adapter = "wwan0" + ifb = "ifb0" + + sudo = ["sudo"] + tc = sudo + ["tc"] + + # check, cmd + cleanup = [ + # Clean up old rules + (False, tc + ["qdisc", "del", "dev", adapter, "root"]), + (False, tc + ["qdisc", "del", "dev", ifb, "root"]), + (False, tc + ["qdisc", "del", "dev", adapter, "ingress"]), + (False, tc + ["qdisc", "del", "dev", ifb, "ingress"]), + + # Bring ifb0 down + (False, sudo + ["ip", "link", "set", "dev", ifb, "down"]), + ] + + upload = [ + # Create root Hierarchy Token Bucket that sends all traffic to 1:20 + (True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]), + + # Create class 1:20 with specified rate limit + (True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]), + + # Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class + (True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", "10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]), + ] + + download = [ + # Bring ifb0 up + (True, sudo + ["ip", "link", "set", "dev", ifb, "up"]), + + # Redirect ingress (incoming) to egress ifb0 + (True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]), + (True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", "match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]), + + # Add class and rules for virtual interface + (True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]), + (True, tc + ["class", "add", "dev", ifb, "parent", "2:", "classid", "2:1", "htb", "rate", f"{download_speed_kbps}kbit"]), + + # Add filter to rule for IP address + (True, tc + ["filter", "add", "dev", ifb, "protocol", "ip", "parent", "2:", "prio", "1", "u32", "match", "ip", "src", "0.0.0.0/0", "flowid", "2:1"]), + ] + + commands = cleanup + if upload_speed_kbps != -1: + commands += upload + if download_speed_kbps != -1: + commands += download + + for check, cmd in commands: + subprocess.run(cmd, check=check) + def get_modem_version(self): try: modem = self.get_modem() @@ -301,15 +329,41 @@ def get_modem_version(self): except Exception: return None + def get_modem_nv(self): + timeout = 0.2 # Default timeout is too short + files = ( + '/nv/item_files/modem/mmode/ue_usage_setting', + '/nv/item_files/ims/IMS_enable', + '/nv/item_files/modem/mmode/sms_only', + ) + try: + modem = self.get_modem() + return { fn: str(modem.Command(f'AT+QNVFR="{fn}"', math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)) for fn in files} + except Exception: + return None + def get_modem_temperatures(self): timeout = 0.2 # Default timeout is too short try: modem = self.get_modem() temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout) - return list(filter(lambda t: t != 255, map(int, temps.split(' ')[1].split(',')))) + return list(map(int, temps.split(' ')[1].split(','))) except Exception: return [] + def get_nvme_temperatures(self): + ret = [] + try: + out = subprocess.check_output("sudo smartctl -aj /dev/nvme0", shell=True) + dat = json.loads(out) + ret = list(map(int, dat["nvme_smart_health_information_log"]["temperature_sensors"])) + except Exception: + pass + return ret + + def get_usb_present(self): + # Not sure if relevant on tici, but the file exists + return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) def get_current_power_draw(self): return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6) @@ -321,119 +375,83 @@ def shutdown(self): os.system("sudo poweroff") def get_thermal_config(self): - intake, exhaust, case = None, None, None - if self.get_device_type() == "mici": - case = ThermalZone("case") - intake = ThermalZone("intake") - exhaust = ThermalZone("exhaust") - return ThermalConfig(cpu=[ThermalZone(f"cpu{i}-silver-usr") for i in range(4)] + - [ThermalZone(f"cpu{i}-gold-usr") for i in range(4)], - gpu=[ThermalZone("gpu0-usr"), ThermalZone("gpu1-usr")], - dsp=ThermalZone("compute-hvx-usr"), - memory=ThermalZone("ddr-usr"), - pmic=[ThermalZone("pm8998_tz"), ThermalZone("pm8005_tz")], - intake=intake, - exhaust=exhaust, - case=case) - - def set_display_power(self, on): - try: - with open("/sys/class/backlight/panel0-backlight/bl_power", "w") as f: - f.write("0" if on else "4") - except Exception: - pass + return ThermalConfig(cpu=(["cpu%d-silver-usr" % i for i in range(4)] + + ["cpu%d-gold-usr" % i for i in range(4)], 1000), + gpu=(("gpu0-usr", "gpu1-usr"), 1000), + mem=("ddr-usr", 1000), + bat=(None, 1), + ambient=("xo-therm-adc", 1000), + pmic=(("pm8998_tz", "pm8005_tz"), 1000)) def set_screen_brightness(self, percentage): try: - with open("/sys/class/backlight/panel0-backlight/max_brightness") as f: - max_brightness = float(f.read().strip()) - - val = int(percentage * (max_brightness / 100.)) with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f: - f.write(str(val)) + f.write(str(int(percentage * 10.23))) except Exception: pass def get_screen_brightness(self): try: - with open("/sys/class/backlight/panel0-backlight/max_brightness") as f: - max_brightness = float(f.read().strip()) - with open("/sys/class/backlight/panel0-backlight/brightness") as f: - return int(float(f.read()) / (max_brightness / 100.)) + return int(float(f.read()) / 10.23) except Exception: return 0 def set_power_save(self, powersave_enabled): # amplifier, 100mW at idle - if self.amplifier is not None: - self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) - if not powersave_enabled: - self.amplifier.initialize_configuration() + self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) + if not powersave_enabled: + self.amplifier.initialize_configuration() # *** CPU config *** - # offline big cluster - for i in range(4, 8): + # offline big cluster, leave core 4 online for boardd + for i in range(5, 8): val = '0' if powersave_enabled else '1' sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online') for n in ('0', '4'): - if powersave_enabled and n == '4': - continue gov = 'ondemand' if powersave_enabled else 'performance' sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor') # *** IRQ config *** - - # GPU, modeld core - affine_irq(7, "kgsl-3d0") - - # camerad core - camera_irqs = ("a5", "cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid-lite", "ife-lite") - for n in camera_irqs: - affine_irq(6, n) + affine_irq(5, 565) # kgsl-3d0 + affine_irq(4, 740) # xhci-hcd:usb1 goes on the boardd core + affine_irq(4, 1069) # xhci-hcd:usb3 goes on the boardd core + for irq in range(237, 246): + affine_irq(5, irq) # camerad def get_gpu_usage_percent(self): try: - with open('/sys/class/kgsl/kgsl-3d0/gpubusy') as f: - used, total = f.read().strip().split() + used, total = open('/sys/class/kgsl/kgsl-3d0/gpubusy').read().strip().split() return 100.0 * int(used) / int(total) except Exception: return 0 def initialize_hardware(self): - if self.amplifier is not None: - self.amplifier.initialize_configuration() + self.amplifier.initialize_configuration() - # Allow hardwared to write engagement status to kmsg + # Allow thermald to write engagement status to kmsg os.system("sudo chmod a+w /dev/kmsg") - # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL - gpio_init(GPIO.SOM_ST_IO, True) - gpio_set(GPIO.SOM_ST_IO, 1) - # *** IRQ config *** - # mask off big cluster from default affinity - sudo_write("f", "/proc/irq/default_smp_affinity") - # move these off the default core - affine_irq(1, "msm_vidc") # encoders - affine_irq(1, "i2c_geni") # sensors + affine_irq(1, 7) # msm_drm + affine_irq(1, 250) # msm_vidc + affine_irq(1, 8) # i2c_geni (sensord) + sudo_write("f", "/proc/irq/default_smp_affinity") # *** GPU config *** # https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216 - affine_irq(5, "fts_ts") # touch - affine_irq(5, "msm_drm") # display sudo_write("1", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel") sudo_write("1", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel") sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on") sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on") sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on") - sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer") + sudo_write("1000000", "/sys/class/kgsl/kgsl-3d0/idle_timer") sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor") - sudo_write("710", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz") + sudo_write("596", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz") # setup governors sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor") @@ -444,88 +462,25 @@ def initialize_hardware(self): sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling") sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation") - # pandad core - affine_irq(3, "spi_geni") # SPI - try: - pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip() - subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid]) - subprocess.call(["sudo", "taskset", "-pc", "3", pid]) - except subprocess.CalledProcessException as e: - print(str(e)) - def configure_modem(self): sim_id = self.get_sim_info().get('sim_id', '') + # configure modem as data-centric + cmds = [ + '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', + ] modem = self.get_modem() - try: - manufacturer = str(modem.Get(MM_MODEM, 'Manufacturer', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) - except Exception: - manufacturer = None - - cmds = [] - - 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', - ] - elif manufacturer == 'Cavli Inc.': - cmds += [ - 'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM - 'AT$QCSIMSLEEP=0', # disable SIM sleep - 'AT$QCSIMCFG=SimPowerSave,0', # more sleep disable - - # ethernet config - 'AT$QCPCFG=usbNet,0', - 'AT$QCNETDEVCTL=3,1', - ] - else: - # this modem gets upset with too many AT commands - if sim_id is None or len(sim_id) == 0: - cmds += [ - # SIM sleep disable - 'AT$QCSIMSLEEP=0', - 'AT$QCSIMCFG=SimPowerSave,0', - - # ethernet config - 'AT$QCPCFG=usbNet,1', - ] - for cmd in cmds: try: modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) except Exception: pass - # eSIM prime - dest = "/etc/NetworkManager/system-connections/esim.nmconnection" - 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}") - tf.write(dat) - tf.flush() - - # needs to be root - 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 + # blue prime config + if sim_id.startswith('8901410'): + os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') def get_networks(self): r = {} @@ -570,16 +525,11 @@ def get_modem_data_usage(self): except Exception: return -1, -1 - def has_internal_panda(self): - return True - def reset_internal_panda(self): gpio_init(GPIO.STM_RST_N, True) - gpio_init(GPIO.STM_BOOT0, True) gpio_set(GPIO.STM_RST_N, 1) - gpio_set(GPIO.STM_BOOT0, 0) - time.sleep(1) + time.sleep(2) gpio_set(GPIO.STM_RST_N, 0) def recover_internal_panda(self): @@ -588,21 +538,12 @@ def recover_internal_panda(self): gpio_set(GPIO.STM_RST_N, 1) gpio_set(GPIO.STM_BOOT0, 1) - time.sleep(0.5) + time.sleep(2) gpio_set(GPIO.STM_RST_N, 0) - time.sleep(0.5) gpio_set(GPIO.STM_BOOT0, 0) - def booted(self): - # this normally boots within 8s, but on rare occasions takes 30+s - encoder_state = sudo_read("/sys/kernel/debug/msm_vidc/core0/info") - if "Core state: 0" in encoder_state and (time.monotonic() < 60*2): - return False - return True if __name__ == "__main__": t = Tici() - t.configure_modem() t.initialize_hardware() t.set_power_save(False) - print(t.get_sim_info()) diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py index bdbea591fbf840..61e528d3046afd 100644 --- a/system/hardware/tici/pins.py +++ b/system/hardware/tici/pins.py @@ -1,17 +1,14 @@ +# 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 HUB_RST_N = 30 UBLOX_RST_N = 32 UBLOX_SAFEBOOT_N = 33 - GNSS_PWR_EN = 34 # SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN - + UBLOX_PWR_EN = 34 STM_RST_N = 124 STM_BOOT0 = 134 - STM_PWR_EN_N = 41 # because STM32H7 RST doesn't generate a full power-on-reset - - SIREN = 42 - SOM_ST_IO = 49 LTE_RST_N = 50 LTE_PWRKEY = 116 @@ -22,6 +19,3 @@ class GPIO: CAM0_RSTN = 9 CAM1_RSTN = 7 CAM2_RSTN = 12 - - # Sensor interrupts - LSM_INT = 84 diff --git a/system/hardware/tici/power_draw_test.py b/system/hardware/tici/power_draw_test.py new file mode 100755 index 00000000000000..bde92ae4a5856e --- /dev/null +++ b/system/hardware/tici/power_draw_test.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +import os +import time +import numpy as np +from system.hardware.tici.hardware import Tici +from system.hardware.tici.pins import GPIO +from common.gpio import gpio_init, gpio_set + +def read_power(): + with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/in1_input") as f: + voltage_total = int(f.read()) / 1000. + + with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/curr1_input") as f: + current_total = int(f.read()) + + with open("/sys/class/power_supply/bms/voltage_now") as f: + voltage = int(f.read()) / 1e6 # volts + + with open("/sys/class/power_supply/bms/current_now") as f: + current = int(f.read()) / 1e3 # ma + + power_som = voltage*current + power_total = voltage_total*current_total + + return power_total, power_som + +def read_power_avg(): + pwrs = [] + for _ in range(100): + pwrs.append(read_power()) + time.sleep(0.01) + power_total, power_som = np.mean([x[0] for x in pwrs]), np.mean([x[1] for x in pwrs]) + return "total %7.2f mW SOM %7.2f mW" % (power_total, power_som) + + +def gpio_export(pin): + try: + with open("/sys/class/gpio/export", 'w') as f: + f.write(str(pin)) + except Exception: + print(f"Failed to export gpio {pin}") + +if __name__ == "__main__": + gpio_export(GPIO.CAM0_AVDD_EN) + gpio_export(GPIO.CAM0_RSTN) + gpio_export(GPIO.CAM1_RSTN) + gpio_export(GPIO.CAM2_RSTN) + print("hello") + os.system('kill $(pgrep -f "manager.py")') + os.system('kill $(pgrep -f "python -m selfdrive.athena.manage_athenad")') + os.system('kill $(pgrep -f "selfdrive.athena.athenad")') + # stopping weston turns off lcd3v3 + os.system("sudo service weston stop") + os.system("sudo service ModemManager stop") + print("services stopped") + + t = Tici() + t.initialize_hardware() + t.set_power_save(True) + t.set_screen_brightness(0) + gpio_init(GPIO.STM_RST_N, True) + gpio_init(GPIO.HUB_RST_N, True) + gpio_init(GPIO.UBLOX_PWR_EN, True) + gpio_init(GPIO.LTE_RST_N, True) + gpio_init(GPIO.LTE_PWRKEY, True) + gpio_init(GPIO.CAM0_AVDD_EN, True) + gpio_init(GPIO.CAM0_RSTN, True) + gpio_init(GPIO.CAM1_RSTN, True) + gpio_init(GPIO.CAM2_RSTN, True) + + + os.system("sudo su -c 'echo 0 > /sys/kernel/debug/regulator/camera_rear_ldo/enable'") # cam 1v2 off + gpio_set(GPIO.CAM0_AVDD_EN, False) # cam 2v8 off + gpio_set(GPIO.LTE_RST_N, True) # quectel off + gpio_set(GPIO.UBLOX_PWR_EN, False) # gps off + gpio_set(GPIO.STM_RST_N, True) # panda off + gpio_set(GPIO.HUB_RST_N, False) # hub off + # cameras in reset + gpio_set(GPIO.CAM0_RSTN, False) + gpio_set(GPIO.CAM1_RSTN, False) + gpio_set(GPIO.CAM2_RSTN, False) + time.sleep(8) + + print("baseline: ", read_power_avg()) + gpio_set(GPIO.CAM0_AVDD_EN, True) + time.sleep(2) + print("cam avdd: ", read_power_avg()) + os.system("sudo su -c 'echo 1 > /sys/kernel/debug/regulator/camera_rear_ldo/enable'") + time.sleep(2) + print("cam dvdd: ", read_power_avg()) + gpio_set(GPIO.CAM0_RSTN, True) + gpio_set(GPIO.CAM1_RSTN, True) + gpio_set(GPIO.CAM2_RSTN, True) + time.sleep(2) + print("cams up: ", read_power_avg()) + gpio_set(GPIO.HUB_RST_N, True) + time.sleep(2) + print("usb hub: ", read_power_avg()) + gpio_set(GPIO.STM_RST_N, False) + time.sleep(5) + print("panda: ", read_power_avg()) + gpio_set(GPIO.UBLOX_PWR_EN, True) + time.sleep(5) + print("gps: ", read_power_avg()) + gpio_set(GPIO.LTE_RST_N, False) + time.sleep(1) + gpio_set(GPIO.LTE_PWRKEY, True) + time.sleep(1) + gpio_set(GPIO.LTE_PWRKEY, False) + time.sleep(5) + print("quectel: ", read_power_avg()) + diff --git a/system/hardware/tici/power_monitor.py b/system/hardware/tici/power_monitor.py index 296290dae84da5..f9d1e3cc410913 100755 --- a/system/hardware/tici/power_monitor.py +++ b/system/hardware/tici/power_monitor.py @@ -3,17 +3,17 @@ import time import datetime import numpy as np -from collections import deque +from typing import List -from openpilot.common.realtime import Ratekeeper -from openpilot.common.filter_simple import FirstOrderFilter +from common.realtime import Ratekeeper +from common.filter_simple import FirstOrderFilter def read_power(): with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f: return int(f.read()) / 1e6 -def sample_power(seconds=5) -> list[float]: +def sample_power(seconds=5) -> List[float]: rate = 123 rk = Ratekeeper(rate, print_delay_threshold=None) @@ -27,15 +27,6 @@ def get_power(seconds=5): pwrs = sample_power(seconds) return np.mean(pwrs) -def wait_for_power(min_pwr, max_pwr, min_secs_in_range, timeout): - start_time = time.monotonic() - pwrs = deque([min_pwr - 1.]*min_secs_in_range, maxlen=min_secs_in_range) - while (time.monotonic() - start_time < timeout): - pwrs.append(get_power(1)) - if all(min_pwr <= p <= max_pwr for p in pwrs): - break - return np.mean(pwrs) - if __name__ == "__main__": duration = None diff --git a/system/hardware/tici/precise_power_measure.py b/system/hardware/tici/precise_power_measure.py index 52fe0850ab3e15..5d6885136792db 100755 --- a/system/hardware/tici/precise_power_measure.py +++ b/system/hardware/tici/precise_power_measure.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 import numpy as np -from openpilot.system.hardware.tici.power_monitor import sample_power +from system.hardware.tici.power_monitor import sample_power if __name__ == '__main__': print("measuring for 5 seconds") for _ in range(3): pwrs = sample_power() - print(f"mean {np.mean(pwrs):.2f} std {np.std(pwrs):.2f}") + print("mean %.2f std %.2f" % (np.mean(pwrs), np.std(pwrs))) diff --git a/system/hardware/tici/restart_modem.sh b/system/hardware/tici/restart_modem.sh index 741dc72050fe32..26c04e4fbaccf3 100755 --- a/system/hardware/tici/restart_modem.sh +++ b/system/hardware/tici/restart_modem.sh @@ -1,4 +1,4 @@ -#!/usr/bin/env bash +#!/usr/bin/bash #nmcli connection modify --temporary lte gsm.home-only yes #nmcli connection modify --temporary lte gsm.auto-config yes @@ -7,7 +7,7 @@ sudo nmcli connection reload sudo systemctl stop ModemManager nmcli con down lte -nmcli con down blue-prime +nmcli con down magenta-prime # power cycle modem /usr/comma/lte/lte.sh stop_blocking diff --git a/system/hardware/tici/test_agnos_updater.py b/system/hardware/tici/test_agnos_updater.py new file mode 100755 index 00000000000000..e0d6ed88145a5c --- /dev/null +++ b/system/hardware/tici/test_agnos_updater.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import json +import os +import unittest +import requests + +AGNOS_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__))) +MANIFEST = os.path.join(AGNOS_DIR, "agnos.json") + + +class TestAgnosUpdater(unittest.TestCase): + + def test_manifest(self): + with open(MANIFEST) as f: + m = json.load(f) + + for img in m: + r = requests.head(img['url'], timeout=10) + r.raise_for_status() + self.assertEqual(r.headers['Content-Type'], "application/x-xz") + if not img['sparse']: + assert img['hash'] == img['hash_raw'] + + +if __name__ == "__main__": + unittest.main() diff --git a/system/hardware/tici/test_power_draw.py b/system/hardware/tici/test_power_draw.py new file mode 100755 index 00000000000000..4b380372b933ae --- /dev/null +++ b/system/hardware/tici/test_power_draw.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +import unittest +import time +import math +from dataclasses import dataclass + +from system.hardware import HARDWARE, TICI +from system.hardware.tici.power_monitor import get_power +from selfdrive.manager.process_config import managed_processes +from selfdrive.manager.manager import manager_cleanup + + +@dataclass +class Proc: + name: str + power: float + rtol: float = 0.05 + atol: float = 0.1 + warmup: float = 6. + +PROCS = [ + Proc('camerad', 2.15), + Proc('modeld', 1.0, atol=0.15), + Proc('dmonitoringmodeld', 0.35), + Proc('encoderd', 0.23), +] + + +class TestPowerDraw(unittest.TestCase): + + @classmethod + def setUpClass(cls): + if not TICI: + raise unittest.SkipTest + + def setUp(self): + HARDWARE.initialize_hardware() + HARDWARE.set_power_save(False) + + # wait a bit for power save to disable + time.sleep(5) + + def tearDown(self): + manager_cleanup() + + def test_camera_procs(self): + baseline = get_power() + + prev = baseline + used = {} + for proc in PROCS: + managed_processes[proc.name].start() + time.sleep(proc.warmup) + + now = get_power(8) + used[proc.name] = now - prev + prev = now + + manager_cleanup() + + print("-"*35) + print(f"Baseline {baseline:.2f}W\n") + for proc in PROCS: + cur = used[proc.name] + expected = proc.power + print(f"{proc.name.ljust(20)} {expected:.2f}W {cur:.2f}W") + with self.subTest(proc=proc.name): + self.assertTrue(math.isclose(cur, expected, rel_tol=proc.rtol, abs_tol=proc.atol)) + print("-"*35) + + +if __name__ == "__main__": + unittest.main() diff --git a/system/hardware/tici/tests/compare_casync_manifest.py b/system/hardware/tici/tests/compare_casync_manifest.py index 7de66d91d0cdd9..b462e93ebc83bc 100755 --- a/system/hardware/tici/tests/compare_casync_manifest.py +++ b/system/hardware/tici/tests/compare_casync_manifest.py @@ -3,11 +3,12 @@ import collections import multiprocessing import os +from typing import Dict, List import requests from tqdm import tqdm -import openpilot.system.hardware.tici.casync as casync +import system.hardware.tici.casync as casync def get_chunk_download_size(chunk): @@ -39,9 +40,9 @@ def get_chunk_download_size(chunk): # Get content-length for each chunk with multiprocessing.Pool() as pool: szs = list(tqdm(pool.imap(get_chunk_download_size, to), total=len(to))) - chunk_sizes = {t.sha: sz for (t, sz) in zip(to, szs, strict=True)} + chunk_sizes = {t.sha: sz for (t, sz) in zip(to, szs)} - sources: dict[str, list[int]] = { + sources: Dict[str, List[int]] = { 'seed': [], 'remote_uncompressed': [], 'remote_compressed': [], diff --git a/system/hardware/tici/tests/test_agnos_updater.py b/system/hardware/tici/tests/test_agnos_updater.py deleted file mode 100644 index a1bbd363fd04d0..00000000000000 --- a/system/hardware/tici/tests/test_agnos_updater.py +++ /dev/null @@ -1,20 +0,0 @@ -import json -import os -import requests - -TEST_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__))) -MANIFEST = os.path.join(TEST_DIR, "../agnos.json") - - -class TestAgnosUpdater: - - def test_manifest(self): - with open(MANIFEST) as f: - m = json.load(f) - - for img in m: - r = requests.head(img['url'], timeout=10) - r.raise_for_status() - assert r.headers['Content-Type'] == "application/x-xz" - if not img['sparse']: - assert img['hash'] == img['hash_raw'] diff --git a/system/hardware/tici/tests/test_amplifier.py b/system/hardware/tici/tests/test_amplifier.py deleted file mode 100644 index 9ce00c3ff26c26..00000000000000 --- a/system/hardware/tici/tests/test_amplifier.py +++ /dev/null @@ -1,69 +0,0 @@ -import pytest -import time -import random -import subprocess - -from panda import Panda -from openpilot.system.hardware import TICI, HARDWARE -from openpilot.system.hardware.tici.amplifier import Amplifier - - -class TestAmplifier: - - @classmethod - def setup_class(cls): - if not TICI: - pytest.skip() - - def setup_method(self): - # clear dmesg - subprocess.check_call("sudo dmesg -C", shell=True) - - HARDWARE.reset_internal_panda() - Panda.wait_for_panda(None, 30) - self.panda = Panda() - - def teardown_method(self): - HARDWARE.reset_internal_panda() - - def _check_for_i2c_errors(self, expected): - dmesg = subprocess.check_output("dmesg", shell=True, encoding='utf8') - i2c_lines = [l for l in dmesg.strip().splitlines() if 'i2c_geni a88000.i2c' in l] - i2c_str = '\n'.join(i2c_lines) - - if not expected: - return len(i2c_lines) == 0 - else: - return "i2c error :-107" in i2c_str or "Bus arbitration lost" in i2c_str - - def test_init(self): - amp = Amplifier(debug=True) - r = amp.initialize_configuration() - assert r - assert self._check_for_i2c_errors(False) - - def test_shutdown(self): - amp = Amplifier(debug=True) - for _ in range(10): - r = amp.set_global_shutdown(True) - r = amp.set_global_shutdown(False) - # amp config should be successful, with no i2c errors - assert r - assert self._check_for_i2c_errors(False) - - def test_init_while_siren_play(self): - for _ in range(10): - self.panda.set_siren(False) - time.sleep(0.1) - - self.panda.set_siren(True) - time.sleep(random.randint(0, 5)) - - amp = Amplifier(debug=True) - r = amp.initialize_configuration() - assert r - - if self._check_for_i2c_errors(True): - break - else: - pytest.fail("didn't hit any i2c errors") diff --git a/system/hardware/tici/tests/test_casync.py b/system/hardware/tici/tests/test_casync.py new file mode 100755 index 00000000000000..8724575ad64659 --- /dev/null +++ b/system/hardware/tici/tests/test_casync.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +import os +import unittest +import tempfile +import subprocess + +import system.hardware.tici.casync as casync + +# dd if=/dev/zero of=/tmp/img.raw bs=1M count=2 +# sudo losetup -f /tmp/img.raw +# losetup -a | grep img.raw +LOOPBACK = os.environ.get('LOOPBACK', None) + + +class TestCasync(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.tmpdir = tempfile.TemporaryDirectory() + + # Build example contents + chunk_a = [i % 256 for i in range(1024)] * 512 + chunk_b = [(256 - i) % 256 for i in range(1024)] * 512 + zeroes = [0] * (1024 * 128) + contents = chunk_a + chunk_b + zeroes + chunk_a + + cls.contents = bytes(contents) + + # Write to file + cls.orig_fn = os.path.join(cls.tmpdir.name, 'orig.bin') + with open(cls.orig_fn, 'wb') as f: + f.write(cls.contents) + + # Create casync files + cls.manifest_fn = os.path.join(cls.tmpdir.name, 'orig.caibx') + cls.store_fn = os.path.join(cls.tmpdir.name, 'store') + subprocess.check_output(["casync", "make", "--compression=xz", "--store", cls.store_fn, cls.manifest_fn, cls.orig_fn]) + + target = casync.parse_caibx(cls.manifest_fn) + hashes = [c.sha.hex() for c in target] + + # Ensure we have chunk reuse + assert len(hashes) > len(set(hashes)) + + def setUp(self): + # Clear target_lo + if LOOPBACK is not None: + self.target_lo = LOOPBACK + with open(self.target_lo, 'wb') as f: + f.write(b"0" * len(self.contents)) + + self.target_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) + self.seed_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) + + def tearDown(self): + for fn in [self.target_fn, self.seed_fn]: + try: + os.unlink(fn) + except FileNotFoundError: + pass + + def test_simple_extract(self): + target = casync.parse_caibx(self.manifest_fn) + + sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as target_f: + self.assertEqual(target_f.read(), self.contents) + + self.assertEqual(stats['remote'], len(self.contents)) + + def test_seed(self): + target = casync.parse_caibx(self.manifest_fn) + + # Populate seed with half of the target contents + with open(self.seed_fn, 'wb') as seed_f: + seed_f.write(self.contents[:len(self.contents) // 2]) + + sources = [('seed', casync.FileChunkReader(self.seed_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as target_f: + self.assertEqual(target_f.read(), self.contents) + + self.assertGreater(stats['seed'], 0) + self.assertLess(stats['remote'], len(self.contents)) + + def test_already_done(self): + """Test that an already flashed target doesn't download any chunks""" + target = casync.parse_caibx(self.manifest_fn) + + with open(self.target_fn, 'wb') as f: + f.write(self.contents) + + sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as f: + self.assertEqual(f.read(), self.contents) + + self.assertEqual(stats['target'], len(self.contents)) + + def test_chunk_reuse(self): + """Test that chunks that are reused are only downloaded once""" + target = casync.parse_caibx(self.manifest_fn) + + # Ensure target exists + with open(self.target_fn, 'wb'): + pass + + sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as f: + self.assertEqual(f.read(), self.contents) + + self.assertLess(stats['remote'], len(self.contents)) + + @unittest.skipUnless(LOOPBACK, "requires loopback device") + def test_lo_simple_extract(self): + target = casync.parse_caibx(self.manifest_fn) + sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_lo) + + with open(self.target_lo, 'rb') as target_f: + self.assertEqual(target_f.read(len(self.contents)), self.contents) + + self.assertEqual(stats['remote'], len(self.contents)) + + @unittest.skipUnless(LOOPBACK, "requires loopback device") + def test_lo_chunk_reuse(self): + """Test that chunks that are reused are only downloaded once""" + target = casync.parse_caibx(self.manifest_fn) + + sources = [('target', casync.FileChunkReader(self.target_lo), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_lo) + + with open(self.target_lo, 'rb') as f: + self.assertEqual(f.read(len(self.contents)), self.contents) + + self.assertLess(stats['remote'], len(self.contents)) + + +if __name__ == "__main__": + unittest.main() diff --git a/system/hardware/tici/tests/test_esim.py b/system/hardware/tici/tests/test_esim.py deleted file mode 100644 index 6fab931ccedc95..00000000000000 --- a/system/hardware/tici/tests/test_esim.py +++ /dev/null @@ -1,51 +0,0 @@ -import pytest - -from openpilot.system.hardware import HARDWARE, TICI -from openpilot.system.hardware.base import LPAProfileNotFoundError - -# https://euicc-manual.osmocom.org/docs/rsp/known-test-profile -# iccid is always the same for the given activation code -TEST_ACTIVATION_CODE = 'LPA:1$rsp.truphone.com$QRF-BETTERROAMING-PMRDGIR2EARDEIT5' -TEST_ICCID = '8944476500001944011' - -TEST_NICKNAME = 'test_profile' - -def cleanup(): - lpa = HARDWARE.get_sim_lpa() - try: - lpa.delete_profile(TEST_ICCID) - except LPAProfileNotFoundError: - pass - lpa.process_notifications() - -class TestEsim: - - @classmethod - def setup_class(cls): - if not TICI: - pytest.skip() - cleanup() - - @classmethod - def teardown_class(cls): - cleanup() - - def test_provision_enable_disable(self): - lpa = HARDWARE.get_sim_lpa() - current_active = lpa.get_active_profile() - - lpa.download_profile(TEST_ACTIVATION_CODE, TEST_NICKNAME) - assert any(p.iccid == TEST_ICCID and p.nickname == TEST_NICKNAME for p in lpa.list_profiles()) - - lpa.enable_profile(TEST_ICCID) - new_active = lpa.get_active_profile() - assert new_active is not None - assert new_active.iccid == TEST_ICCID - assert new_active.nickname == TEST_NICKNAME - - lpa.disable_profile(TEST_ICCID) - new_active = lpa.get_active_profile() - assert new_active is None - - if current_active: - lpa.enable_profile(current_active.iccid) diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py deleted file mode 100644 index 4fbde816736a75..00000000000000 --- a/system/hardware/tici/tests/test_power_draw.py +++ /dev/null @@ -1,128 +0,0 @@ -from collections import defaultdict, deque -import pytest -import time -import numpy as np -from dataclasses import dataclass -from tabulate import tabulate - -import cereal.messaging as messaging -from cereal.services import SERVICE_LIST -from opendbc.car.car_helpers import get_demo_car_params -from openpilot.common.mock import mock_messages -from openpilot.common.params import Params -from openpilot.system.hardware.tici.power_monitor import get_power -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.manager.manager import manager_cleanup - -SAMPLE_TIME = 8 # seconds to sample power -MAX_WARMUP_TIME = 30 # seconds to wait for SAMPLE_TIME consecutive valid samples - -@dataclass -class Proc: - procs: list[str] - power: float - msgs: list[str] - rtol: float = 0.05 - atol: float = 0.12 - - @property - def name(self): - return '+'.join(self.procs) - - -PROCS = [ - 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=[]), -] - - -@pytest.mark.tici -class TestPowerDraw: - - def setup_method(self): - Params().put("CarParams", get_demo_car_params().to_bytes()) - - # wait a bit for power save to disable - time.sleep(5) - - def teardown_method(self): - manager_cleanup() - - def get_expected_messages(self, proc): - return int(sum(SAMPLE_TIME * SERVICE_LIST[msg].frequency for msg in proc.msgs)) - - def valid_msg_count(self, proc, msg_counts): - msgs_received = sum(msg_counts[msg] for msg in proc.msgs) - msgs_expected = self.get_expected_messages(proc) - return np.isclose(msgs_expected, msgs_received, rtol=.02, atol=2) - - def valid_power_draw(self, proc, used): - return np.isclose(used, proc.power, rtol=proc.rtol, atol=proc.atol) - - def tabulate_msg_counts(self, msgs_and_power): - msg_counts = defaultdict(int) - for _, counts in msgs_and_power: - for msg, count in counts.items(): - msg_counts[msg] += count - return msg_counts - - def get_power_with_warmup_for_target(self, proc, prev): - socks = {msg: messaging.sub_sock(msg) for msg in proc.msgs} - for sock in socks.values(): - messaging.drain_sock_raw(sock) - - msgs_and_power = deque([], maxlen=SAMPLE_TIME) - - start_time = time.monotonic() - - while (time.monotonic() - start_time) < MAX_WARMUP_TIME: - power = get_power(1) - iteration_msg_counts = {} - for msg,sock in socks.items(): - iteration_msg_counts[msg] = len(messaging.drain_sock_raw(sock)) - msgs_and_power.append((power, iteration_msg_counts)) - - if len(msgs_and_power) < SAMPLE_TIME: - continue - - msg_counts = self.tabulate_msg_counts(msgs_and_power) - now = np.mean([m[0] for m in msgs_and_power]) - - if self.valid_msg_count(proc, msg_counts) and self.valid_power_draw(proc, now - prev): - break - - return now, msg_counts, time.monotonic() - start_time - SAMPLE_TIME - - @mock_messages(['livePose']) - def test_camera_procs(self, subtests): - baseline = get_power() - - prev = baseline - used = {} - warmup_time = {} - msg_counts = {} - - for proc in PROCS: - for p in proc.procs: - managed_processes[p].start() - now, local_msg_counts, warmup_time[proc.name] = self.get_power_with_warmup_for_target(proc, prev) - msg_counts.update(local_msg_counts) - - used[proc.name] = now - prev - prev = now - - manager_cleanup() - - tab = [['process', 'expected (W)', 'measured (W)', '# msgs expected', '# msgs received', "warmup time (s)"]] - for proc in PROCS: - cur = used[proc.name] - expected = proc.power - msgs_received = sum(msg_counts[msg] for msg in proc.msgs) - tab.append([proc.name, round(expected, 2), round(cur, 2), self.get_expected_messages(proc), msgs_received, round(warmup_time[proc.name], 2)]) - with subtests.test(proc=proc.name): - assert self.valid_msg_count(proc, msg_counts), f"expected {self.get_expected_messages(proc)} msgs, got {msgs_received} msgs" - assert self.valid_power_draw(proc, cur), f"expected {expected:.2f}W, got {cur:.2f}W" - print(tabulate(tab)) - print(f"Baseline {baseline:.2f}W\n") diff --git a/system/hardware/tici/updater b/system/hardware/tici/updater index 69ce323a1064e9..1c577512005494 100755 Binary files a/system/hardware/tici/updater and b/system/hardware/tici/updater differ diff --git a/system/hardware/tici/updater_magic b/system/hardware/tici/updater_magic deleted file mode 100755 index ec586dbcb3c100..00000000000000 --- a/system/hardware/tici/updater_magic +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c44fb88b3b1643b6b44ae8ac9880348bd0257ff90f4084cbe889de91d71653fe -size 25111329 diff --git a/system/hardware/tici/updater_weston b/system/hardware/tici/updater_weston deleted file mode 100755 index 23cdc140f452a1..00000000000000 --- a/system/hardware/tici/updater_weston +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eba5f44e6a763e1f74d1c718993218adcc72cba4caafe99b595fa701151a4c54 -size 10448792 diff --git a/system/journald.py b/system/journald.py deleted file mode 100755 index 37158b9251dd5b..00000000000000 --- a/system/journald.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/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 new file mode 100644 index 00000000000000..c66f7622d96b86 --- /dev/null +++ b/system/logcatd/.gitignore @@ -0,0 +1 @@ +logcatd diff --git a/system/logcatd/SConscript b/system/logcatd/SConscript new file mode 100644 index 00000000000000..6bd7c6ff3e8561 --- /dev/null +++ b/system/logcatd/SConscript @@ -0,0 +1,3 @@ +Import('env', 'cereal', 'messaging', 'common') + +env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) diff --git a/system/logcatd/logcatd_systemd.cc b/system/logcatd/logcatd_systemd.cc new file mode 100644 index 00000000000000..70467a9c151905 --- /dev/null +++ b/system/logcatd/logcatd_systemd.cc @@ -0,0 +1,75 @@ +#include + +#include +#include +#include +#include + +#include "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/SConscript b/system/loggerd/SConscript deleted file mode 100644 index cf169f4dc6124b..00000000000000 --- a/system/loggerd/SConscript +++ /dev/null @@ -1,26 +0,0 @@ -Import('env', 'arch', 'messaging', 'common', 'visionipc') - -libs = [common, messaging, visionipc, - 'avformat', 'avcodec', 'avutil', - 'yuv', 'OpenCL', 'pthread', 'zstd'] - -src = ['logger.cc', 'zstd_writer.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc', 'encoder/jpeg_encoder.cc'] -if arch != "larch64": - src += ['encoder/ffmpeg_encoder.cc'] - -if arch == "Darwin": - # fix OpenCL - del libs[libs.index('OpenCL')] - env['FRAMEWORKS'] = ['OpenCL'] - # exclude v4l - del src[src.index('encoder/v4l_encoder.cc')] - -logger_lib = env.Library('logger', src) -libs.insert(0, logger_lib) - -env.Program('loggerd', ['loggerd.cc'], LIBS=libs) -env.Program('encoderd', ['encoderd.cc'], LIBS=libs + ["jpeg"]) -env.Program('bootlog.cc', LIBS=libs) - -if GetOption('extras'): - env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc', 'tests/test_zstd_writer.cc'], LIBS=libs + ['curl', 'crypto']) diff --git a/system/loggerd/bootlog.cc b/system/loggerd/bootlog.cc deleted file mode 100644 index 1b87ce394f9a0f..00000000000000 --- a/system/loggerd/bootlog.cc +++ /dev/null @@ -1,68 +0,0 @@ -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "system/loggerd/logger.h" -#include "system/loggerd/zstd_writer.h" - - -static kj::Array build_boot_log() { - MessageBuilder msg; - auto boot = msg.initEvent().initBoot(); - - boot.setWallTimeNanos(nanos_since_epoch()); - - std::string pstore = "/sys/fs/pstore"; - std::map pstore_map = util::read_files_in_dir(pstore); - - int i = 0; - auto lpstore = boot.initPstore().initEntries(pstore_map.size()); - for (auto& kv : pstore_map) { - auto lentry = lpstore[i]; - lentry.setKey(kv.first); - lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size())); - i++; - } - - // Gather output of commands - std::vector bootlog_commands = { - "[ -x \"$(command -v journalctl)\" ] && journalctl -o short-monotonic", - }; - - - auto commands = boot.initCommands().initEntries(bootlog_commands.size()); - for (int j = 0; j < bootlog_commands.size(); j++) { - auto lentry = commands[j]; - - lentry.setKey(bootlog_commands[j]); - - const std::string result = util::check_output(bootlog_commands[j]); - lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); - } - - boot.setLaunchLog(util::read_file("/tmp/launch_log")); - return capnp::messageToFlatArray(msg); -} - -int main(int argc, char** argv) { - const std::string id = logger_get_identifier("BootCount"); - const std::string path = Path::log_root() + "/boot/" + id + ".zst"; - LOGW("bootlog to %s", path.c_str()); - - // Open bootlog - bool r = util::create_directories(Path::log_root() + "/boot/", 0775); - assert(r); - - ZstdFileWriter file(path, LOG_COMPRESSION_LEVEL); - // Write initdata - file.write(logger_build_init_data().asBytes()); - // Write bootlog - file.write(build_boot_log().asBytes()); - - // Write out bootlog param to match routes with bootlog - Params().put("CurrentBootlog", id.c_str()); - - return 0; -} diff --git a/system/loggerd/config.py b/system/loggerd/config.py deleted file mode 100644 index e1c47c768df93f..00000000000000 --- a/system/loggerd/config.py +++ /dev/null @@ -1,29 +0,0 @@ -import os -from openpilot.system.hardware.hw import Paths - - -CAMERA_FPS = 20 -SEGMENT_LENGTH = 60 - -STATS_DIR_FILE_LIMIT = 10000 -STATS_SOCKET = "ipc:///tmp/stats" -STATS_FLUSH_TIME_S = 60 - -def get_available_percent(default: float) -> float: - try: - statvfs = os.statvfs(Paths.log_root()) - available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks - except OSError: - available_percent = default - - return available_percent - - -def get_available_bytes(default: int) -> int: - try: - statvfs = os.statvfs(Paths.log_root()) - available_bytes = statvfs.f_bavail * statvfs.f_frsize - except OSError: - available_bytes = default - - return available_bytes diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py deleted file mode 100755 index eb8fd35f21813c..00000000000000 --- a/system/loggerd/deleter.py +++ /dev/null @@ -1,80 +0,0 @@ -#!/usr/bin/env python3 -import os -import shutil -import threading -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import cloudlog -from openpilot.system.loggerd.config import get_available_bytes, get_available_percent -from openpilot.system.loggerd.uploader import listdir_by_creation -from openpilot.system.loggerd.xattr_cache import getxattr - -MIN_BYTES = 5 * 1024 * 1024 * 1024 -MIN_PERCENT = 10 - -DELETE_LAST = ['boot', 'crash'] - -PRESERVE_ATTR_NAME = 'user.preserve' -PRESERVE_ATTR_VALUE = b'1' -PRESERVE_COUNT = 5 - - -def has_preserve_xattr(d: str) -> bool: - return getxattr(os.path.join(Paths.log_root(), d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE - - -def get_preserved_segments(dirs_by_creation: list[str]) -> set[str]: - # skip deleting most recent N preserved segments (and their prior segment) - preserved = set() - for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))): - if n == PRESERVE_COUNT: - break - date_str, _, seg_str = d.rpartition("--") - - # ignore non-segment directories - if not date_str: - continue - try: - seg_num = int(seg_str) - except ValueError: - continue - - # preserve segment and two prior - for _seg_num in range(max(0, seg_num - 2), seg_num + 1): - preserved.add(f"{date_str}--{_seg_num}") - - return preserved - - -def deleter_thread(exit_event: threading.Event): - while not exit_event.is_set(): - out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES - out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT - - if out_of_percent or out_of_bytes: - dirs = listdir_by_creation(Paths.log_root()) - preserved_dirs = get_preserved_segments(dirs) - - # remove the earliest directory we can - for delete_dir in sorted(dirs, key=lambda d: (d in DELETE_LAST, d in preserved_dirs)): - delete_path = os.path.join(Paths.log_root(), delete_dir) - - if any(name.endswith(".lock") for name in os.listdir(delete_path)): - continue - - try: - cloudlog.info(f"deleting {delete_path}") - shutil.rmtree(delete_path) - break - except OSError: - cloudlog.exception(f"issue deleting {delete_path}") - exit_event.wait(.1) - else: - exit_event.wait(30) - - -def main(): - deleter_thread(threading.Event()) - - -if __name__ == "__main__": - main() diff --git a/system/loggerd/encoder/encoder.cc b/system/loggerd/encoder/encoder.cc deleted file mode 100644 index 06922b0cd07a12..00000000000000 --- a/system/loggerd/encoder/encoder.cc +++ /dev/null @@ -1,42 +0,0 @@ -#include "system/loggerd/encoder/encoder.h" - -VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height) - : encoder_info(encoder_info), in_width(in_width), in_height(in_height) { - - out_width = encoder_info.frame_width > 0 ? encoder_info.frame_width : in_width; - out_height = encoder_info.frame_height > 0 ? encoder_info.frame_height : in_height; - - pm.reset(new PubMaster(std::vector{encoder_info.publish_name})); -} - -void VideoEncoder::publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, - unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat) { - MessageBuilder msg; - auto event = msg.initEvent(true); - auto edat = (event.*(encoder_info.init_encode_data_func))(); - auto edata = edat.initIdx(); - struct timespec ts; - timespec_get(&ts, TIME_UTC); - edat.setUnixTimestampNanos((uint64_t)ts.tv_sec*1000000000 + ts.tv_nsec); - edata.setFrameId(extra.frame_id); - edata.setTimestampSof(extra.timestamp_sof); - edata.setTimestampEof(extra.timestamp_eof); - edata.setType(encoder_info.get_settings(in_width).encode_type); - edata.setEncodeId(cnt++); - edata.setSegmentNum(segment_num); - edata.setSegmentId(idx); - edata.setFlags(flags); - edata.setLen(dat.size()); - edat.adoptData(msg.getOrphanage().referenceExternalData(dat)); - edat.setWidth(out_width); - edat.setHeight(out_height); - if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header); - - uint32_t bytes_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word); - if (msg_cache.size() < bytes_size) { - msg_cache.resize(bytes_size); - } - kj::ArrayOutputStream output_stream(kj::ArrayPtr(msg_cache.data(), bytes_size)); - capnp::writeMessage(output_stream, msg); - pm->send(encoder_info.publish_name, msg_cache.data(), bytes_size); -} diff --git a/system/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h deleted file mode 100644 index 57146fafc3b87a..00000000000000 --- a/system/loggerd/encoder/encoder.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -// has to be in this order -#ifdef __linux__ -#include "third_party/linux/include/v4l2-controls.h" -#include -#else -#define V4L2_BUF_FLAG_KEYFRAME 8 -#endif - -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "msgq/visionipc/visionipc.h" -#include "common/queue.h" -#include "system/loggerd/loggerd.h" - -class VideoEncoder { -public: - VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height); - virtual ~VideoEncoder() {} - virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; - virtual void encoder_open() = 0; - virtual void encoder_close() = 0; - - void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat); - -protected: - int in_width, in_height; - int out_width, out_height; - const EncoderInfo encoder_info; - -private: - // total frames encoded - int cnt = 0; - std::unique_ptr pm; - std::vector msg_cache; -}; diff --git a/system/loggerd/encoder/ffmpeg_encoder.h b/system/loggerd/encoder/ffmpeg_encoder.h deleted file mode 100644 index cd5ac1e13a69eb..00000000000000 --- a/system/loggerd/encoder/ffmpeg_encoder.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -extern "C" { -#include -#include -#include -} - -#include "system/loggerd/encoder/encoder.h" -#include "system/loggerd/loggerd.h" - -class FfmpegEncoder : public VideoEncoder { -public: - FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height); - ~FfmpegEncoder(); - int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); - void encoder_open(); - void encoder_close(); - -private: - int segment_num = -1; - int counter = 0; - bool is_open = false; - - AVCodecContext *codec_ctx; - AVFrame *frame = NULL; - std::vector convert_buf; - std::vector downscale_buf; -}; diff --git a/system/loggerd/encoder/jpeg_encoder.cc b/system/loggerd/encoder/jpeg_encoder.cc deleted file mode 100644 index 6bb946157c2279..00000000000000 --- a/system/loggerd/encoder/jpeg_encoder.cc +++ /dev/null @@ -1,105 +0,0 @@ -#include "system/loggerd/encoder/jpeg_encoder.h" - -#include -#include - -JpegEncoder::JpegEncoder(const std::string &pusblish_name, int width, int height) - : publish_name(pusblish_name), thumbnail_width(width), thumbnail_height(height) { - yuv_buffer.resize((thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2); - pm = std::make_unique(std::vector{pusblish_name.c_str()}); -} - -JpegEncoder::~JpegEncoder() { - if (out_buffer) { - free(out_buffer); - } -} - -void JpegEncoder::pushThumbnail(VisionBuf *buf, const VisionIpcBufExtra &extra) { - generateThumbnail(buf->y, buf->uv, buf->width, buf->height, buf->stride); - - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initThumbnail(); - thumbnaild.setFrameId(extra.frame_id); - thumbnaild.setTimestampEof(extra.timestamp_eof); - thumbnaild.setThumbnail({out_buffer, out_size}); - - pm->send(publish_name.c_str(), msg); -} - -void JpegEncoder::generateThumbnail(const uint8_t *y_addr, const uint8_t *uv_addr, int width, int height, int stride) { - int downscale = width / thumbnail_width; - assert(downscale * thumbnail_height == height); - - // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used. - uint8_t *y_plane = yuv_buffer.data(); - uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height; - uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4; - { - // subsampled conversion from nv12 to yuv - for (int hy = 0; hy < thumbnail_height / 2; hy++) { - for (int hx = 0; hx < thumbnail_width / 2; hx++) { - int ix = hx * downscale + (downscale - 1) / 2; - int iy = hy * downscale + (downscale - 1) / 2; - y_plane[(hy * 2 + 0) * thumbnail_width + (hx * 2 + 0)] = y_addr[(iy * 2 + 0) * stride + ix * 2 + 0]; - y_plane[(hy * 2 + 0) * thumbnail_width + (hx * 2 + 1)] = y_addr[(iy * 2 + 0) * stride + ix * 2 + 1]; - y_plane[(hy * 2 + 1) * thumbnail_width + (hx * 2 + 0)] = y_addr[(iy * 2 + 1) * stride + ix * 2 + 0]; - y_plane[(hy * 2 + 1) * thumbnail_width + (hx * 2 + 1)] = y_addr[(iy * 2 + 1) * stride + ix * 2 + 1]; - u_plane[hy * thumbnail_width / 2 + hx] = uv_addr[iy * stride + ix * 2 + 0]; - v_plane[hy * thumbnail_width / 2 + hx] = uv_addr[iy * stride + ix * 2 + 1]; - } - } - } - - compressToJpeg(y_plane, u_plane, v_plane); -} - -void JpegEncoder::compressToJpeg(uint8_t *y_plane, uint8_t *u_plane, uint8_t *v_plane) { - struct jpeg_compress_struct cinfo; - struct jpeg_error_mgr jerr; - cinfo.err = jpeg_std_error(&jerr); - jpeg_create_compress(&cinfo); - - if (out_buffer) { - free(out_buffer); - out_buffer = nullptr; - out_size = 0; - } - jpeg_mem_dest(&cinfo, &out_buffer, &out_size); - - cinfo.image_width = thumbnail_width; - cinfo.image_height = thumbnail_height; - cinfo.input_components = 3; - - jpeg_set_defaults(&cinfo); - jpeg_set_colorspace(&cinfo, JCS_YCbCr); - // configure sampling factors for yuv420. - cinfo.comp_info[0].h_samp_factor = 2; // Y - cinfo.comp_info[0].v_samp_factor = 2; - cinfo.comp_info[1].h_samp_factor = 1; // U - cinfo.comp_info[1].v_samp_factor = 1; - cinfo.comp_info[2].h_samp_factor = 1; // V - cinfo.comp_info[2].v_samp_factor = 1; - cinfo.raw_data_in = TRUE; - - jpeg_set_quality(&cinfo, 50, TRUE); - jpeg_start_compress(&cinfo, TRUE); - - JSAMPROW y[16], u[8], v[8]; - JSAMPARRAY planes[3]{y, u, v}; - - for (int line = 0; line < cinfo.image_height; line += 16) { - for (int i = 0; i < 16; ++i) { - y[i] = y_plane + (line + i) * cinfo.image_width; - if (i % 2 == 0) { - int offset = (cinfo.image_width / 2) * ((i + line) / 2); - u[i / 2] = u_plane + offset; - v[i / 2] = v_plane + offset; - } - } - jpeg_write_raw_data(&cinfo, planes, 16); - } - - jpeg_finish_compress(&cinfo); - jpeg_destroy_compress(&cinfo); -} diff --git a/system/loggerd/encoder/jpeg_encoder.h b/system/loggerd/encoder/jpeg_encoder.h deleted file mode 100644 index af1427c19a3d2d..00000000000000 --- a/system/loggerd/encoder/jpeg_encoder.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include "cereal/messaging/messaging.h" -#include "msgq/visionipc/visionbuf.h" - -class JpegEncoder { -public: - JpegEncoder(const std::string &pusblish_name, int width, int height); - ~JpegEncoder(); - void pushThumbnail(VisionBuf *buf, const VisionIpcBufExtra &extra); - -private: - void generateThumbnail(const uint8_t *y, const uint8_t *uv, int width, int height, int stride); - void compressToJpeg(uint8_t *y_plane, uint8_t *u_plane, uint8_t *v_plane); - - int thumbnail_width; - int thumbnail_height; - std::string publish_name; - std::vector yuv_buffer; - std::unique_ptr pm; - - // JPEG output buffer - unsigned char* out_buffer = nullptr; - unsigned long out_size = 0; -}; diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc deleted file mode 100644 index 6ee3af13b0b4ce..00000000000000 --- a/system/loggerd/encoder/v4l_encoder.cc +++ /dev/null @@ -1,323 +0,0 @@ -#include -#include -#include -#include - -#include "system/loggerd/encoder/v4l_encoder.h" -#include "common/util.h" -#include "common/timing.h" - -#include "third_party/libyuv/include/libyuv.h" -#include "third_party/linux/include/msm_media_info.h" - -// has to be in this order -#include "third_party/linux/include/v4l2-controls.h" -#include -#define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000 -#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000 - -/* - kernel debugging: - 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 -*/ -const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0; - -static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) { - v4l2_plane plane = {0}; - v4l2_buffer v4l_buf = { - .type = buf_type, - .memory = V4L2_MEMORY_USERPTR, - .m = { .planes = &plane, }, - .length = 1, - }; - util::safe_ioctl(fd, VIDIOC_DQBUF, &v4l_buf, "VIDIOC_DQBUF failed"); - - if (index) *index = v4l_buf.index; - if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused; - if (flags) *flags = v4l_buf.flags; - if (timestamp) *timestamp = v4l_buf.timestamp; - assert(v4l_buf.m.planes[0].data_offset == 0); -} - -static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) { - v4l2_plane plane = { - .length = (unsigned int)buf->len, - .m = { .userptr = (unsigned long)buf->addr, }, - .bytesused = (uint32_t)buf->len, - .reserved = {(unsigned int)buf->fd} - }; - - v4l2_buffer v4l_buf = { - .type = buf_type, - .index = index, - .memory = V4L2_MEMORY_USERPTR, - .m = { .planes = &plane, }, - .length = 1, - .flags = V4L2_BUF_FLAG_TIMESTAMP_COPY, - .timestamp = timestamp - }; - util::safe_ioctl(fd, VIDIOC_QBUF, &v4l_buf, "VIDIOC_QBUF failed"); -} - -static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) { - struct v4l2_requestbuffers reqbuf = { - .type = buf_type, - .memory = V4L2_MEMORY_USERPTR, - .count = count - }; - util::safe_ioctl(fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed"); -} - -void V4LEncoder::dequeue_handler(V4LEncoder *e) { - std::string dequeue_thread_name = "dq-"+std::string(e->encoder_info.publish_name); - util::set_thread_name(dequeue_thread_name.c_str()); - - e->segment_num++; - uint32_t idx = -1; - bool exit = false; - - // POLLIN is capture, POLLOUT is frame - struct pollfd pfd; - pfd.events = POLLIN | POLLOUT; - pfd.fd = e->fd; - - // save the header - kj::Array header; - - while (!exit) { - int rc = poll(&pfd, 1, 1000); - if (rc < 0) { - if (errno != EINTR) { - // TODO: exit encoder? - // ignore the error and keep going - LOGE("poll failed (%d - %d)", rc, errno); - } - continue; - } else if (rc == 0) { - LOGE("encoder dequeue poll timeout"); - continue; - } - - if (env_debug_encoder >= 2) { - printf("%20s poll %x at %.2f ms\n", e->encoder_info.publish_name, pfd.revents, millis_since_boot()); - } - - int frame_id = -1; - if (pfd.revents & POLLIN) { - unsigned int bytesused, flags, index; - struct timeval timestamp; - dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &index, &bytesused, &flags, ×tamp); - e->buf_out[index].sync(VISIONBUF_SYNC_FROM_DEVICE); - uint8_t *buf = (uint8_t*)e->buf_out[index].addr; - int64_t ts = timestamp.tv_sec * 1000000 + timestamp.tv_usec; - - // eof packet, we exit - if (flags & V4L2_QCOM_BUF_FLAG_EOS) { - exit = true; - } else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) { - // save header - header = kj::heapArray(buf, bytesused); - } else { - VisionIpcBufExtra extra = e->extras.pop(); - assert(extra.timestamp_eof/1000 == ts); // stay in sync - frame_id = extra.frame_id; - ++idx; - e->publisher_publish(e->segment_num, idx, extra, flags, header, kj::arrayPtr(buf, bytesused)); - } - - if (env_debug_encoder) { - printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n", - e->encoder_info.publish_name, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size()); - } - - // requeue the buffer - queue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, &e->buf_out[index]); - } - - if (pfd.revents & POLLOUT) { - unsigned int index; - dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index); - e->free_buf_in.push(index); - } - } -} - -V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height) - : VideoEncoder(encoder_info, in_width, in_height) { - fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK)); - assert(fd >= 0); - - struct v4l2_capability cap; - util::safe_ioctl(fd, VIDIOC_QUERYCAP, &cap, "VIDIOC_QUERYCAP failed"); - LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd); - assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0); - assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0); - - EncoderSettings encoder_settings = encoder_info.get_settings(in_width); - bool is_h265 = encoder_settings.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C; - - struct v4l2_format fmt_out = { - .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, - .fmt = { - .pix_mp = { - // downscales are free with v4l - .width = (unsigned int)(out_width), - .height = (unsigned int)(out_height), - .pixelformat = is_h265 ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264, - .field = V4L2_FIELD_ANY, - .colorspace = V4L2_COLORSPACE_DEFAULT, - } - } - }; - util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt_out, "VIDIOC_S_FMT failed"); - - v4l2_streamparm streamparm = { - .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, - .parm = { - .output = { - // TODO: more stuff here? we don't know - .timeperframe = { - .numerator = 1, - .denominator = (unsigned int)encoder_info.fps - } - } - } - }; - util::safe_ioctl(fd, VIDIOC_S_PARM, &streamparm, "VIDIOC_S_PARM failed"); - - struct v4l2_format fmt_in = { - .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, - .fmt = { - .pix_mp = { - .width = (unsigned int)in_width, - .height = (unsigned int)in_height, - .pixelformat = V4L2_PIX_FMT_NV12, - .field = V4L2_FIELD_ANY, - .colorspace = V4L2_COLORSPACE_470_SYSTEM_BG, - } - } - }; - util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt_in, "VIDIOC_S_FMT failed"); - - LOGD("in buffer size %d, out buffer size %d", - fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage, - fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage); - - // shared ctrls - { - struct v4l2_control ctrls[] = { - { .id = V4L2_CID_MPEG_VIDEO_BITRATE, .value = encoder_settings.bitrate}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = encoder_settings.gop_size - encoder_settings.b_frames - 1}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = encoder_settings.b_frames}, - { .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE, .value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL, .value = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1}, - }; - for (auto ctrl : ctrls) { - util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); - } - } - - if (is_h265) { - struct v4l2_control ctrls[] = { - { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_VUI_TIMING_INFO, .value = V4L2_MPEG_VIDC_VIDEO_VUI_TIMING_INFO_ENABLED}, - }; - for (auto ctrl : ctrls) { - util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); - } - } else { - struct v4l2_control ctrls[] = { - { .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH}, - { .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN}, - { .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC}, - { .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0}, - { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE, .value = 0}, - { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, .value = 0}, - { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, .value = 0}, - { .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0}, - }; - for (auto ctrl : ctrls) { - util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl, "VIDIOC_S_CTRL failed"); - } - } - - // allocate buffers - request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, BUF_OUT_COUNT); - request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, BUF_IN_COUNT); - - // start encoder - v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; - util::safe_ioctl(fd, VIDIOC_STREAMON, &buf_type, "VIDIOC_STREAMON failed"); - buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; - util::safe_ioctl(fd, VIDIOC_STREAMON, &buf_type, "VIDIOC_STREAMON failed"); - - // queue up output buffers - for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) { - buf_out[i].allocate(fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage); - queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, i, &buf_out[i]); - } - // queue up input buffers - for (unsigned int i = 0; i < BUF_IN_COUNT; i++) { - free_buf_in.push(i); - } -} - -void V4LEncoder::encoder_open() { - dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this); - this->is_open = true; - this->counter = 0; -} - -int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { - struct timeval timestamp { - .tv_sec = (long)(extra->timestamp_eof/1000000000), - .tv_usec = (long)((extra->timestamp_eof/1000) % 1000000), - }; - - // reserve buffer - int buffer_in = free_buf_in.pop(); - - // push buffer - extras.push(*extra); - //buf->sync(VISIONBUF_SYNC_TO_DEVICE); - queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp); - - return this->counter++; -} - -void V4LEncoder::encoder_close() { - if (this->is_open) { - // pop all the frames before closing, then put the buffers back - for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.pop(); - for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i); - // no frames, stop the encoder - struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP }; - util::safe_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd, "VIDIOC_ENCODER_CMD failed"); - // join waits for V4L2_QCOM_BUF_FLAG_EOS - dequeue_handler_thread.join(); - assert(extras.empty()); - } - this->is_open = false; -} - -V4LEncoder::~V4LEncoder() { - encoder_close(); - v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; - util::safe_ioctl(fd, VIDIOC_STREAMOFF, &buf_type, "VIDIOC_STREAMOFF failed"); - request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0); - buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; - util::safe_ioctl(fd, VIDIOC_STREAMOFF, &buf_type, "VIDIOC_STREAMOFF failed"); - request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0); - close(fd); - - for (int i = 0; i < BUF_OUT_COUNT; i++) { - if (buf_out[i].free() != 0) { - LOGE("Failed to free buffer"); - } - } -} diff --git a/system/loggerd/encoder/v4l_encoder.h b/system/loggerd/encoder/v4l_encoder.h deleted file mode 100644 index 58011d60e1a881..00000000000000 --- a/system/loggerd/encoder/v4l_encoder.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include "common/queue.h" -#include "system/loggerd/encoder/encoder.h" - -#define BUF_IN_COUNT 7 -#define BUF_OUT_COUNT 6 - -class V4LEncoder : public VideoEncoder { -public: - V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height); - ~V4LEncoder(); - int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); - void encoder_open(); - void encoder_close(); - -private: - int fd; - - bool is_open = false; - int segment_num = -1; - int counter = 0; - - SafeQueue extras; - - static void dequeue_handler(V4LEncoder *e); - std::thread dequeue_handler_thread; - - VisionBuf buf_out[BUF_OUT_COUNT]; - SafeQueue free_buf_in; -}; diff --git a/system/loggerd/encoderd.cc b/system/loggerd/encoderd.cc deleted file mode 100644 index 9d4b81a3f90230..00000000000000 --- a/system/loggerd/encoderd.cc +++ /dev/null @@ -1,173 +0,0 @@ -#include - -#include "system/loggerd/loggerd.h" -#include "system/loggerd/encoder/jpeg_encoder.h" - -#ifdef __TICI__ -#include "system/loggerd/encoder/v4l_encoder.h" -#define Encoder V4LEncoder -#else -#include "system/loggerd/encoder/ffmpeg_encoder.h" -#define Encoder FfmpegEncoder -#endif - -ExitHandler do_exit; - -struct EncoderdState { - int max_waiting = 0; - - // Sync logic for startup - std::atomic encoders_ready = 0; - std::atomic start_frame_id = 0; - bool camera_ready[VISION_STREAM_WIDE_ROAD + 1] = {}; - bool camera_synced[VISION_STREAM_WIDE_ROAD + 1] = {}; -}; - -// Handle initial encoder syncing by waiting for all encoders to reach the same frame id -bool sync_encoders(EncoderdState *s, VisionStreamType cam_type, uint32_t frame_id) { - if (s->camera_synced[cam_type]) return true; - - if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) { - // add a small margin to the start frame id in case one of the encoders already dropped the next frame - update_max_atomic(s->start_frame_id, frame_id + 2); - if (std::exchange(s->camera_ready[cam_type], true) == false) { - ++s->encoders_ready; - LOGD("camera %d encoder ready", cam_type); - } - return false; - } else { - if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id); - bool synced = frame_id >= s->start_frame_id; - s->camera_synced[cam_type] = synced; - if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); - return synced; - } -} - - -void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { - util::set_thread_name(cam_info.thread_name); - - std::vector> encoders; - VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false); - - std::unique_ptr jpeg_encoder; - - int cur_seg = 0; - while (!do_exit) { - if (!vipc_client.connect(false)) { - util::sleep_for(5); - continue; - } - - // init encoders - if (encoders.empty()) { - const VisionBuf &buf_info = vipc_client.buffers[0]; - LOGW("encoder %s init %zux%zu", cam_info.thread_name, buf_info.width, buf_info.height); - assert(buf_info.width > 0 && buf_info.height > 0); - - for (const auto &encoder_info : cam_info.encoder_infos) { - auto &e = encoders.emplace_back(new Encoder(encoder_info, buf_info.width, buf_info.height)); - e->encoder_open(); - } - - // Only one thumbnail can be generated per camera stream - if (auto thumbnail_name = cam_info.encoder_infos[0].thumbnail_name) { - jpeg_encoder = std::make_unique(thumbnail_name, buf_info.width / 4, buf_info.height / 4); - } - } - - bool lagging = false; - while (!do_exit) { - VisionIpcBufExtra extra; - VisionBuf* buf = vipc_client.recv(&extra); - if (buf == nullptr) continue; - - // detect loop around and drop the frames - if (buf->get_frame_id() != extra.frame_id) { - if (!lagging) { - LOGE("encoder %s lag buffer id: %" PRIu64 " extra id: %d", cam_info.thread_name, buf->get_frame_id(), extra.frame_id); - lagging = true; - } - continue; - } - lagging = false; - - if (!sync_encoders(s, cam_info.stream_type, extra.frame_id)) { - continue; - } - if (do_exit) break; - - // do rotation if required - const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS; - if (cur_seg >= 0 && extra.frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) { - for (auto &e : encoders) { - e->encoder_close(); - e->encoder_open(); - } - ++cur_seg; - } - - // encode a frame - for (int i = 0; i < encoders.size(); ++i) { - int out_id = encoders[i]->encode_frame(buf, &extra); - - if (out_id == -1) { - LOGE("Failed to encode frame. frame_id: %d", extra.frame_id); - } - } - - if (jpeg_encoder && (extra.frame_id % 1200 == 100)) { - jpeg_encoder->pushThumbnail(buf, extra); - } - } - } -} - -template -void encoderd_thread(const LogCameraInfo (&cameras)[N]) { - EncoderdState s; - - std::set streams; - while (!do_exit) { - streams = VisionIpcClient::getAvailableStreams("camerad", false); - if (!streams.empty()) { - break; - } - util::sleep_for(100); - } - - if (!streams.empty()) { - std::vector encoder_threads; - for (auto stream : streams) { - auto it = std::find_if(std::begin(cameras), std::end(cameras), - [stream](auto &cam) { return cam.stream_type == stream; }); - assert(it != std::end(cameras)); - ++s.max_waiting; - encoder_threads.push_back(std::thread(encoder_thread, &s, *it)); - } - - for (auto &t : encoder_threads) t.join(); - } -} - -int main(int argc, char* argv[]) { - if (!Hardware::PC()) { - int ret; - ret = util::set_realtime_priority(52); - assert(ret == 0); - ret = util::set_core_affinity({3}); - assert(ret == 0); - } - if (argc > 1) { - std::string arg1(argv[1]); - if (arg1 == "--stream") { - encoderd_thread(stream_cameras_logged); - } else { - LOGE("Argument '%s' is not supported", arg1.c_str()); - } - } else { - encoderd_thread(cameras_logged); - } - return 0; -} diff --git a/system/loggerd/logger.cc b/system/loggerd/logger.cc deleted file mode 100644 index 0ebe323939ab53..00000000000000 --- a/system/loggerd/logger.cc +++ /dev/null @@ -1,202 +0,0 @@ -#include "system/loggerd/logger.h" - -#include -#include -#include -#include -#include -#include - -#include "common/params.h" -#include "common/swaglog.h" -#include "common/version.h" - -// ***** log metadata ***** -kj::Array logger_build_init_data() { - uint64_t wall_time = nanos_since_epoch(); - - MessageBuilder msg; - auto init = msg.initEvent().initInitData(); - - init.setWallTimeNanos(wall_time); - init.setVersion(COMMA_VERSION); - init.setDirty(!getenv("CLEAN")); - init.setDeviceType(Hardware::get_device_type()); - - // log kernel args - std::ifstream cmdline_stream("/proc/cmdline"); - std::vector kernel_args; - std::string buf; - while (cmdline_stream >> buf) { - kernel_args.push_back(buf); - } - - auto lkernel_args = init.initKernelArgs(kernel_args.size()); - for (int i=0; i params_map = params.readAll(); - - init.setGitCommit(params_map["GitCommit"]); - init.setGitCommitDate(params_map["GitCommitDate"]); - init.setGitBranch(params_map["GitBranch"]); - init.setGitRemote(params_map["GitRemote"]); - init.setPassive(false); - init.setDongleId(params_map["DongleId"]); - - // for prebuilt branches - init.setGitSrcCommit(util::read_file("../../git_src_commit")); - init.setGitSrcCommitDate(util::read_file("../../git_src_commit_date")); - - auto lparams = init.initParams().initEntries(params_map.size()); - int j = 0; - for (auto& [key, value] : params_map) { - auto lentry = lparams[j]; - lentry.setKey(key); - if ( !(params.getKeyFlag(key) & DONT_LOG) ) { - lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size())); - } - j++; - } - - // log commands - std::vector log_commands = { - "df -h", // usage for all filesystems - }; - - auto hw_logs = Hardware::get_init_logs(); - - auto commands = init.initCommands().initEntries(log_commands.size() + hw_logs.size()); - for (int i = 0; i < log_commands.size(); i++) { - auto lentry = commands[i]; - - lentry.setKey(log_commands[i]); - - const std::string result = util::check_output(log_commands[i]); - lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size())); - } - - int i = log_commands.size(); - for (auto &[key, value] : hw_logs) { - auto lentry = commands[i]; - lentry.setKey(key); - lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size())); - i++; - } - - return capnp::messageToFlatArray(msg); -} - -std::string logger_get_identifier(std::string key) { - // a log identifier is a 32 bit counter, plus a 10 character unique ID. - // e.g. 000001a3--c20ba54385 - - Params params; - uint32_t cnt; - try { - cnt = std::stoul(params.get(key)); - } catch (std::exception &e) { - cnt = 0; - } - params.put(key, std::to_string(cnt + 1)); - - std::stringstream ss; - std::random_device rd; - std::mt19937 mt(rd()); - std::uniform_int_distribution dist(0, 15); - for (int i = 0; i < 10; ++i) { - ss << std::hex << dist(mt); - } - - return util::string_format("%08x--%s", cnt, ss.str().c_str()); -} - -std::string zstd_decompress(const std::string &in) { - ZSTD_DCtx *dctx = ZSTD_createDCtx(); - assert(dctx != nullptr); - - // Initialize input and output buffers - ZSTD_inBuffer input = {in.data(), in.size(), 0}; - - // Estimate and reserve memory for decompressed data - size_t estimatedDecompressedSize = ZSTD_getFrameContentSize(in.data(), in.size()); - if (estimatedDecompressedSize == ZSTD_CONTENTSIZE_ERROR || estimatedDecompressedSize == ZSTD_CONTENTSIZE_UNKNOWN) { - estimatedDecompressedSize = in.size() * 2; // Use a fallback size - } - - std::string decompressedData; - decompressedData.reserve(estimatedDecompressedSize); - - const size_t bufferSize = ZSTD_DStreamOutSize(); // Recommended output buffer size - std::string outputBuffer(bufferSize, '\0'); - - while (input.pos < input.size) { - ZSTD_outBuffer output = {outputBuffer.data(), bufferSize, 0}; - - size_t result = ZSTD_decompressStream(dctx, &output, &input); - if (ZSTD_isError(result)) { - break; - } - - decompressedData.append(outputBuffer.data(), output.pos); - } - - ZSTD_freeDCtx(dctx); - decompressedData.shrink_to_fit(); - return decompressedData; -} - - -static void log_sentinel(LoggerState *log, SentinelType type, int exit_signal = 0) { - MessageBuilder msg; - auto sen = msg.initEvent().initSentinel(); - sen.setType(type); - sen.setSignal(exit_signal); - log->write(msg.toBytes(), true); -} - -LoggerState::LoggerState(const std::string &log_root) { - route_name = logger_get_identifier("RouteCount"); - route_path = log_root + "/" + route_name; - init_data = logger_build_init_data(); -} - -LoggerState::~LoggerState() { - if (rlog) { - log_sentinel(this, SentinelType::END_OF_ROUTE, exit_signal); - std::remove(lock_file.c_str()); - } -} - -bool LoggerState::next() { - if (rlog) { - log_sentinel(this, SentinelType::END_OF_SEGMENT); - std::remove(lock_file.c_str()); - } - - segment_path = route_path + "--" + std::to_string(++part); - bool ret = util::create_directories(segment_path, 0775); - assert(ret == true); - - lock_file = segment_path + "/rlog.lock"; - std::ofstream{lock_file}; - - rlog.reset(new ZstdFileWriter(segment_path + "/rlog.zst", LOG_COMPRESSION_LEVEL)); - qlog.reset(new ZstdFileWriter(segment_path + "/qlog.zst", LOG_COMPRESSION_LEVEL)); - - // log init data & sentinel type. - write(init_data.asBytes(), true); - log_sentinel(this, part > 0 ? SentinelType::START_OF_SEGMENT : SentinelType::START_OF_ROUTE); - return true; -} - -void LoggerState::write(uint8_t* data, size_t size, bool in_qlog) { - rlog->write(data, size); - if (in_qlog) qlog->write(data, size); -} diff --git a/system/loggerd/logger.h b/system/loggerd/logger.h deleted file mode 100644 index 18d07b5f383237..00000000000000 --- a/system/loggerd/logger.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/util.h" -#include "system/hardware/hw.h" -#include "system/loggerd/zstd_writer.h" - -constexpr int LOG_COMPRESSION_LEVEL = 10; - -typedef cereal::Sentinel::SentinelType SentinelType; - -class LoggerState { -public: - LoggerState(const std::string& log_root = Path::log_root()); - ~LoggerState(); - bool next(); - void write(uint8_t* data, size_t size, bool in_qlog); - inline int segment() const { return part; } - inline const std::string& segmentPath() const { return segment_path; } - inline const std::string& routeName() const { return route_name; } - inline void write(kj::ArrayPtr bytes, bool in_qlog) { write(bytes.begin(), bytes.size(), in_qlog); } - inline void setExitSignal(int signal) { exit_signal = signal; } - -protected: - int part = -1, exit_signal = 0; - std::string route_path, route_name, segment_path, lock_file; - kj::Array init_data; - std::unique_ptr rlog, qlog; -}; - -kj::Array logger_build_init_data(); -std::string logger_get_identifier(std::string key); -std::string zstd_decompress(const std::string &in); diff --git a/system/loggerd/loggerd.cc b/system/loggerd/loggerd.cc deleted file mode 100644 index 47da321024c96a..00000000000000 --- a/system/loggerd/loggerd.cc +++ /dev/null @@ -1,358 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include "common/params.h" -#include "system/loggerd/encoder/encoder.h" -#include "system/loggerd/loggerd.h" -#include "system/loggerd/video_writer.h" - -ExitHandler do_exit; - -struct LoggerdState { - LoggerState logger; - std::atomic last_camera_seen_tms{0.0}; - std::atomic ready_to_rotate{0}; // count of encoders ready to rotate - int max_waiting = 0; - double last_rotate_tms = 0.; // last rotate time in ms -}; - -void logger_rotate(LoggerdState *s) { - bool ret =s->logger.next(); - assert(ret); - s->ready_to_rotate = 0; - s->last_rotate_tms = millis_since_boot(); - LOGW((s->logger.segment() == 0) ? "logging to %s" : "rotated to %s", s->logger.segmentPath().c_str()); -} - -void rotate_if_needed(LoggerdState *s) { - // all encoders ready, trigger rotation - bool all_ready = s->ready_to_rotate == s->max_waiting; - - // fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions - bool timed_out = false; - double tms = millis_since_boot(); - double seg_length_secs = (tms - s->last_rotate_tms) / 1000.; - if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) { - // TODO: might be nice to put these reasons in the sentinel - if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) { - timed_out = true; - LOGE("no camera packets seen. auto rotating"); - } else if (seg_length_secs > SEGMENT_LENGTH*1.2) { - timed_out = true; - LOGE("segment too long. auto rotating"); - } - } - - if (all_ready || timed_out) { - logger_rotate(s); - } -} - -struct RemoteEncoder { - std::unique_ptr writer; - int encoderd_segment_offset; - int current_segment = -1; - std::vector q; - int dropped_frames = 0; - bool recording = false; - bool marked_ready_to_rotate = false; - bool seen_first_packet = false; - bool audio_initialized = false; -}; - -size_t write_encode_data(LoggerdState *s, cereal::Event::Reader event, RemoteEncoder &re, const EncoderInfo &encoder_info) { - auto edata = (event.*(encoder_info.get_encode_data_func))(); - auto idx = edata.getIdx(); - auto flags = idx.getFlags(); - - // if we aren't recording yet, try to start, since we are in the correct segment - if (!re.recording) { - if (flags & V4L2_BUF_FLAG_KEYFRAME) { - // only create on iframe - if (re.dropped_frames) { - // this should only happen for the first segment, maybe - LOGW("%s: dropped %d non iframe packets before init", encoder_info.publish_name, re.dropped_frames); - re.dropped_frames = 0; - } - if (encoder_info.record) { - // write the header - auto header = edata.getHeader(); - re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof() / 1000, true, false); - } - re.recording = true; - } else { - // this is a sad case when we aren't recording, but don't have an iframe - // nothing we can do but drop the frame - ++re.dropped_frames; - return 0; - } - } - - // we have to be recording if we are here - assert(re.recording); - - // if we are actually writing the video file, do so - if (re.writer) { - auto data = edata.getData(); - re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof() / 1000, false, flags & V4L2_BUF_FLAG_KEYFRAME); - } - - // put it in log stream as the idx packet - MessageBuilder bmsg; - auto evt = bmsg.initEvent(event.getValid()); - evt.setLogMonoTime(event.getLogMonoTime()); - (evt.*(encoder_info.set_encode_idx_func))(idx); - auto new_msg = bmsg.toBytes(); - s->logger.write((uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog? - return new_msg.size(); -} - -int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct RemoteEncoder &re, const EncoderInfo &encoder_info) { - int bytes_count = 0; - - // extract the message - capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word))); - auto event = cmsg.getRoot(); - auto edata = (event.*(encoder_info.get_encode_data_func))(); - auto idx = edata.getIdx(); - - // encoderd can have started long before loggerd - if (!re.seen_first_packet) { - re.seen_first_packet = true; - re.encoderd_segment_offset = idx.getSegmentNum(); - LOGD("%s: has encoderd offset %d", name.c_str(), re.encoderd_segment_offset); - } - int offset_segment_num = idx.getSegmentNum() - re.encoderd_segment_offset; - - if (offset_segment_num == s->logger.segment()) { - // loggerd is now on the segment that matches this packet - - // if this is a new segment, we close any possible old segments, move to the new, and process any queued packets - if (re.current_segment != s->logger.segment()) { - // if we aren't actually recording, don't create the writer - if (encoder_info.record) { - assert(encoder_info.filename != NULL); - re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(), - encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C, - edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType())); - re.recording = false; - re.audio_initialized = false; - } - re.current_segment = s->logger.segment(); - re.marked_ready_to_rotate = false; - } - if (re.audio_initialized || !encoder_info.include_audio) { - // we are in this segment now, process any queued messages before this one - if (!re.q.empty()) { - for (auto qmsg : re.q) { - capnp::FlatArrayMessageReader reader({(capnp::word *)qmsg->getData(), qmsg->getSize() / sizeof(capnp::word)}); - bytes_count += write_encode_data(s, reader.getRoot(), re, encoder_info); - delete qmsg; - } - re.q.clear(); - } - bytes_count += write_encode_data(s, event, re, encoder_info); - delete msg; - } else if (re.q.size() > MAIN_FPS*10) { - LOGE_100("%s: dropping frame waiting for audio initialization, queue is too large", name.c_str()); - delete msg; - } else { - re.q.push_back(msg); // queue up all the new segment messages, they go in after audio is initialized - } - } else if (offset_segment_num > s->logger.segment()) { - // encoderd packet has a newer segment, this means encoderd has rolled over - if (!re.marked_ready_to_rotate) { - re.marked_ready_to_rotate = true; - ++s->ready_to_rotate; - LOGD("rotate %d -> %d ready %d/%d for %s", - s->logger.segment(), offset_segment_num, - s->ready_to_rotate.load(), s->max_waiting, name.c_str()); - } - - // TODO: define this behavior, but for now don't leak - if (re.q.size() > MAIN_FPS*10) { - LOGE_100("%s: dropping frame, queue is too large", name.c_str()); - delete msg; - } else { - // queue up all the new segment messages, they go in after the rotate - re.q.push_back(msg); - } - } else { - LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->logger.segment():%d re.encoderd_segment_offset:%d", - name.c_str(), idx.getSegmentNum(), s->logger.segment(), re.encoderd_segment_offset); - // free the message, it's useless. this should never happen - // actually, this can happen if you restart encoderd - re.encoderd_segment_offset = -s->logger.segment(); - delete msg; - } - - return bytes_count; -} - -void handle_preserve_segment(LoggerdState *s) { - static int prev_segment = -1; - if (s->logger.segment() == prev_segment) return; - - LOGW("preserving %s", s->logger.segmentPath().c_str()); - -#ifdef __APPLE__ - int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0); -#else - int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0); -#endif - if (ret) { - LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->logger.segmentPath().c_str(), strerror(errno)); - } - - // mark route for uploading - Params params; - std::string routes = params.get("AthenadRecentlyViewedRoutes"); - params.put("AthenadRecentlyViewedRoutes", routes + "," + s->logger.routeName()); - - prev_segment = s->logger.segment(); -} - -void loggerd_thread() { - // setup messaging - typedef struct ServiceState { - std::string name; - int counter, freq; - bool encoder, preserve_segment, record_audio; - } ServiceState; - std::unordered_map service_state; - std::unordered_map remote_encoders; - - std::unique_ptr ctx(Context::create()); - std::unique_ptr poller(Poller::create()); - - // subscribe to all socks - for (const auto& [_, it] : services) { - const bool encoder = util::ends_with(it.name, "EncodeData"); - const bool livestream_encoder = util::starts_with(it.name, "livestream"); - const bool record_audio = (it.name == "rawAudioData") && Params().getBool("RecordAudio"); - if (it.should_log || (encoder && !livestream_encoder) || record_audio) { - LOGD("logging %s", it.name.c_str()); - - SubSocket * sock = SubSocket::create(ctx.get(), it.name, "127.0.0.1", false, true, it.queue_size); - assert(sock != NULL); - poller->registerSocket(sock); - service_state[sock] = { - .name = it.name, - .counter = 0, - .freq = it.decimation, - .encoder = encoder, - .preserve_segment = (it.name == "userBookmark") || (it.name == "audioFeedback"), - .record_audio = record_audio, - }; - } - } - - LoggerdState s; - // init logger - logger_rotate(&s); - Params().put("CurrentRoute", s.logger.routeName()); - - std::map encoder_infos_dict; - std::vector encoders_with_audio; - for (const auto &cam : cameras_logged) { - for (const auto &encoder_info : cam.encoder_infos) { - encoder_infos_dict[encoder_info.publish_name] = encoder_info; - s.max_waiting++; - } - } - - for (auto &[sock, service] : service_state) { - auto it = encoder_infos_dict.find(service.name); - if (it != encoder_infos_dict.end() && it->second.include_audio) { - encoders_with_audio.push_back(&remote_encoders[sock]); - } - } - - uint64_t msg_count = 0, bytes_count = 0; - double start_ts = millis_since_boot(); - while (!do_exit) { - // poll for new messages on all sockets - for (auto sock : poller->poll(1000)) { - if (do_exit) break; - - ServiceState &service = service_state[sock]; - if (service.preserve_segment) { - handle_preserve_segment(&s); - } - - // drain socket - int count = 0; - Message *msg = nullptr; - while (!do_exit && (msg = sock->receive(true))) { - const bool in_qlog = service.freq != -1 && (service.counter++ % service.freq == 0); - - if (service.record_audio) { - capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word))); - auto event = cmsg.getRoot(); - auto audio_data = event.getRawAudioData().getData(); - auto sample_rate = event.getRawAudioData().getSampleRate(); - for (auto* encoder : encoders_with_audio) { - if (encoder && encoder->writer) { - encoder->writer->write_audio((uint8_t*)audio_data.begin(), audio_data.size(), event.getLogMonoTime() / 1000, sample_rate); - encoder->audio_initialized = true; - } - } - } - - if (service.encoder) { - s.last_camera_seen_tms = millis_since_boot(); - bytes_count += handle_encoder_msg(&s, msg, service.name, remote_encoders[sock], encoder_infos_dict[service.name]); - } else { - s.logger.write((uint8_t *)msg->getData(), msg->getSize(), in_qlog); - bytes_count += msg->getSize(); - delete msg; - } - - rotate_if_needed(&s); - - if ((++msg_count % 10000) == 0) { - double seconds = (millis_since_boot() - start_ts) / 1000.0; - LOGD("%" PRIu64 " messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds); - } - - count++; - if (count >= 200) { - LOGD("large volume of '%s' messages", service.name.c_str()); - break; - } - } - } - } - - LOGW("closing logger"); - s.logger.setExitSignal(do_exit.signal); - - if (do_exit.power_failure) { - LOGE("power failure"); - sync(); - LOGE("sync done"); - } - - // messaging cleanup - for (auto &[sock, service] : service_state) delete sock; -} - -int main(int argc, char** argv) { - if (!Hardware::PC()) { - int ret; - ret = util::set_core_affinity({0, 1, 2, 3}); - assert(ret == 0); - // TODO: why does this impact camerad timings? - //ret = util::set_realtime_priority(1); - //assert(ret == 0); - } - - loggerd_thread(); - - return 0; -} diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h deleted file mode 100644 index 8e3a74d2d98fb7..00000000000000 --- a/system/loggerd/loggerd.h +++ /dev/null @@ -1,172 +0,0 @@ -#pragma once - -#include -#include - -#include "cereal/messaging/messaging.h" -#include "cereal/services.h" -#include "msgq/visionipc/visionipc_client.h" -#include "system/hardware/hw.h" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/util.h" - -#include "system/loggerd/logger.h" - -constexpr int MAIN_FPS = 20; -const auto MAIN_ENCODE_TYPE = Hardware::PC() ? cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS : cereal::EncodeIndex::Type::FULL_H_E_V_C; -#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead - -#define INIT_ENCODE_FUNCTIONS(encode_type) \ - .get_encode_data_func = &cereal::Event::Reader::get##encode_type##Data, \ - .set_encode_idx_func = &cereal::Event::Builder::set##encode_type##Idx, \ - .init_encode_data_func = &cereal::Event::Builder::init##encode_type##Data - -const bool LOGGERD_TEST = getenv("LOGGERD_TEST"); -const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60; - -constexpr char PRESERVE_ATTR_NAME[] = "user.preserve"; -constexpr char PRESERVE_ATTR_VALUE = '1'; - -struct EncoderSettings { - cereal::EncodeIndex::Type encode_type; - int bitrate; - int gop_size; - int b_frames = 0; // we don't use b frames - - static EncoderSettings MainEncoderSettings(int in_width) { - if (in_width <= 1344) { - return EncoderSettings{.encode_type = MAIN_ENCODE_TYPE, .bitrate = 5'000'000, .gop_size = 20}; - } else { - return EncoderSettings{.encode_type = MAIN_ENCODE_TYPE, .bitrate = 10'000'000, .gop_size = 30}; - } - } - - static EncoderSettings QcamEncoderSettings() { - return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = 256'000, .gop_size = 15}; - } - - static EncoderSettings StreamEncoderSettings() { - int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 1'000'000; - return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 15}; - } -}; - -class EncoderInfo { -public: - const char *publish_name; - const char *thumbnail_name = NULL; - const char *filename = NULL; - bool record = true; - bool include_audio = false; - int frame_width = -1; - int frame_height = -1; - int fps = MAIN_FPS; - std::function get_settings; - - ::cereal::EncodeData::Reader (cereal::Event::Reader::*get_encode_data_func)() const; - void (cereal::Event::Builder::*set_encode_idx_func)(::cereal::EncodeIndex::Reader); - cereal::EncodeData::Builder (cereal::Event::Builder::*init_encode_data_func)(); -}; - -class LogCameraInfo { -public: - const char *thread_name; - int fps = MAIN_FPS; - VisionStreamType stream_type; - std::vector encoder_infos; -}; - -const EncoderInfo main_road_encoder_info = { - .publish_name = "roadEncodeData", - .thumbnail_name = "thumbnail", - .filename = "fcamera.hevc", - .get_settings = [](int in_width){return EncoderSettings::MainEncoderSettings(in_width);}, - INIT_ENCODE_FUNCTIONS(RoadEncode), -}; - -const EncoderInfo main_wide_road_encoder_info = { - .publish_name = "wideRoadEncodeData", - .filename = "ecamera.hevc", - .get_settings = [](int in_width){return EncoderSettings::MainEncoderSettings(in_width);}, - INIT_ENCODE_FUNCTIONS(WideRoadEncode), -}; - -const EncoderInfo main_driver_encoder_info = { - .publish_name = "driverEncodeData", - .filename = "dcamera.hevc", - .record = Params().getBool("RecordFront"), - .get_settings = [](int in_width){return EncoderSettings::MainEncoderSettings(in_width);}, - INIT_ENCODE_FUNCTIONS(DriverEncode), -}; - -const EncoderInfo stream_road_encoder_info = { - .publish_name = "livestreamRoadEncodeData", - //.thumbnail_name = "thumbnail", - .record = false, - .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, - INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode), -}; - -const EncoderInfo stream_wide_road_encoder_info = { - .publish_name = "livestreamWideRoadEncodeData", - .record = false, - .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, - INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode), -}; - -const EncoderInfo stream_driver_encoder_info = { - .publish_name = "livestreamDriverEncodeData", - .record = false, - .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, - INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode), -}; - -const EncoderInfo qcam_encoder_info = { - .publish_name = "qRoadEncodeData", - .filename = "qcamera.ts", - .get_settings = [](int){return EncoderSettings::QcamEncoderSettings();}, - .frame_width = 526, - .frame_height = 330, - .include_audio = Params().getBool("RecordAudio"), - INIT_ENCODE_FUNCTIONS(QRoadEncode), -}; - -const LogCameraInfo road_camera_info{ - .thread_name = "road_cam_encoder", - .stream_type = VISION_STREAM_ROAD, - .encoder_infos = {main_road_encoder_info, qcam_encoder_info} -}; - -const LogCameraInfo wide_road_camera_info{ - .thread_name = "wide_road_cam_encoder", - .stream_type = VISION_STREAM_WIDE_ROAD, - .encoder_infos = {main_wide_road_encoder_info} -}; - -const LogCameraInfo driver_camera_info{ - .thread_name = "driver_cam_encoder", - .stream_type = VISION_STREAM_DRIVER, - .encoder_infos = {main_driver_encoder_info} -}; - -const LogCameraInfo stream_road_camera_info{ - .thread_name = "road_cam_encoder", - .stream_type = VISION_STREAM_ROAD, - .encoder_infos = {stream_road_encoder_info} -}; - -const LogCameraInfo stream_wide_road_camera_info{ - .thread_name = "wide_road_cam_encoder", - .stream_type = VISION_STREAM_WIDE_ROAD, - .encoder_infos = {stream_wide_road_encoder_info} -}; - -const LogCameraInfo stream_driver_camera_info{ - .thread_name = "driver_cam_encoder", - .stream_type = VISION_STREAM_DRIVER, - .encoder_infos = {stream_driver_encoder_info} -}; - -const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info}; -const LogCameraInfo stream_cameras_logged[] = {stream_road_camera_info, stream_wide_road_camera_info, stream_driver_camera_info}; diff --git a/system/loggerd/tests/loggerd_tests_common.py b/system/loggerd/tests/loggerd_tests_common.py deleted file mode 100644 index 8bf609ae8d906a..00000000000000 --- a/system/loggerd/tests/loggerd_tests_common.py +++ /dev/null @@ -1,90 +0,0 @@ -import os -import random -from pathlib import Path - - -import openpilot.system.loggerd.deleter as deleter -import openpilot.system.loggerd.uploader as uploader -from openpilot.common.params import Params -from openpilot.system.hardware.hw import Paths -from openpilot.system.loggerd.xattr_cache import setxattr - - -def create_random_file(file_path: Path, size_mb: float, lock: bool = False, upload_xattr: bytes | None = None) -> None: - file_path.parent.mkdir(parents=True, exist_ok=True) - - if lock: - lock_path = str(file_path) + ".lock" - os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL)) - - chunks = 128 - chunk_bytes = int(size_mb * 1024 * 1024 / chunks) - data = os.urandom(chunk_bytes) - - with open(file_path, "wb") as f: - for _ in range(chunks): - f.write(data) - - if upload_xattr is not None: - setxattr(str(file_path), uploader.UPLOAD_ATTR_NAME, upload_xattr) - -class MockResponse: - def __init__(self, text, status_code): - self.text = text - self.status_code = status_code - -class MockApi: - def __init__(self, dongle_id): - pass - - def get(self, *args, **kwargs): - return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}', 200) - - def get_token(self): - return "fake-token" - -class MockApiIgnore: - def __init__(self, dongle_id): - pass - - def get(self, *args, **kwargs): - return MockResponse('', 412) - - def get_token(self): - return "fake-token" - -class UploaderTestCase: - f_type = "UNKNOWN" - - root: Path - seg_num: int - seg_format: str - seg_format2: str - seg_dir: str - - def set_ignore(self): - uploader.Api = MockApiIgnore - - def setup_method(self): - uploader.Api = MockApi - uploader.fake_upload = True - uploader.force_wifi = True - uploader.allow_sleep = False - self.seg_num = random.randint(1, 300) - self.seg_format = "00000004--0ac3964c96--{}" - self.seg_format2 = "00000005--4c4e99b08b--{}" - self.seg_dir = self.seg_format.format(self.seg_num) - - self.params = Params() - self.params.put("IsOffroad", True) - self.params.put("DongleId", "0000000000000000") - - def make_file_with_data(self, f_dir: str, fn: str, size_mb: float = .1, lock: bool = False, - upload_xattr: bytes | None = None, preserve_xattr: bytes | None = None) -> Path: - file_path = Path(Paths.log_root()) / f_dir / fn - create_random_file(file_path, size_mb, lock, upload_xattr) - - if preserve_xattr is not None: - setxattr(str(file_path.parent), deleter.PRESERVE_ATTR_NAME, preserve_xattr) - - return file_path diff --git a/system/loggerd/tests/test_deleter.py b/system/loggerd/tests/test_deleter.py deleted file mode 100644 index 6222ea253ba0db..00000000000000 --- a/system/loggerd/tests/test_deleter.py +++ /dev/null @@ -1,117 +0,0 @@ -import time -import threading -from collections import namedtuple -from pathlib import Path -from collections.abc import Sequence - -import openpilot.system.loggerd.deleter as deleter -from openpilot.common.timeout import Timeout, TimeoutException -from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase - -Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize']) - - -class TestDeleter(UploaderTestCase): - def fake_statvfs(self, d): - return self.fake_stats - - def setup_method(self): - self.f_type = "fcamera.hevc" - super().setup_method() - self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096) - deleter.os.statvfs = self.fake_statvfs - - def start_thread(self): - self.end_event = threading.Event() - self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event]) - self.del_thread.daemon = True - self.del_thread.start() - - def join_thread(self): - self.end_event.set() - self.del_thread.join() - - def test_delete(self): - f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1) - - self.start_thread() - - try: - with Timeout(2, "Timeout waiting for file to be deleted"): - while f_path.exists(): - time.sleep(0.01) - finally: - self.join_thread() - - def assertDeleteOrder(self, f_paths: Sequence[Path], timeout: int = 5) -> None: - deleted_order = [] - - self.start_thread() - try: - with Timeout(timeout, "Timeout waiting for files to be deleted"): - while True: - for f in f_paths: - if not f.exists() and f not in deleted_order: - deleted_order.append(f) - if len(deleted_order) == len(f_paths): - break - time.sleep(0.01) - except TimeoutException: - print("Not deleted:", [f for f in f_paths if f not in deleted_order]) - raise - finally: - self.join_thread() - - assert deleted_order == f_paths, "Files not deleted in expected order" - - def test_delete_order(self): - self.assertDeleteOrder([ - self.make_file_with_data(self.seg_format.format(0), self.f_type), - self.make_file_with_data(self.seg_format.format(1), self.f_type), - self.make_file_with_data(self.seg_format2.format(0), self.f_type), - ]) - - def test_delete_many_preserved(self): - self.assertDeleteOrder([ - self.make_file_with_data(self.seg_format.format(0), self.f_type), - self.make_file_with_data(self.seg_format.format(1), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), - self.make_file_with_data(self.seg_format.format(2), self.f_type), - ] + [ - self.make_file_with_data(self.seg_format2.format(i), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE) - for i in range(5) - ]) - - def test_delete_last(self): - self.assertDeleteOrder([ - self.make_file_with_data(self.seg_format.format(1), self.f_type), - self.make_file_with_data(self.seg_format2.format(0), self.f_type), - self.make_file_with_data(self.seg_format.format(0), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), - self.make_file_with_data("boot", self.seg_format[:-4]), - self.make_file_with_data("crash", self.seg_format2[:-4]), - ]) - - def test_no_delete_when_available_space(self): - f_path = self.make_file_with_data(self.seg_dir, self.f_type) - - block_size = 4096 - available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free - self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size) - - self.start_thread() - start_time = time.monotonic() - while f_path.exists() and time.monotonic() - start_time < 2: - time.sleep(0.01) - self.join_thread() - - assert f_path.exists(), "File deleted with available space" - - def test_no_delete_with_lock_file(self): - f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) - - self.start_thread() - start_time = time.monotonic() - while f_path.exists() and time.monotonic() - start_time < 2: - time.sleep(0.01) - self.join_thread() - - assert f_path.exists(), "File deleted when locked" diff --git a/system/loggerd/tests/test_encoder.py b/system/loggerd/tests/test_encoder.py deleted file mode 100644 index e4dabd3df930e4..00000000000000 --- a/system/loggerd/tests/test_encoder.py +++ /dev/null @@ -1,152 +0,0 @@ -import math -import os -import pytest -import random -import shutil -import subprocess -import time -from pathlib import Path - -from parameterized import parameterized -from tqdm import trange - -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.system.hardware import TICI -from openpilot.system.manager.process_config import managed_processes -from openpilot.tools.lib.logreader import LogReader -from openpilot.system.hardware.hw import Paths - -SEGMENT_LENGTH = 2 -FULL_SIZE = 2507572 -def hevc_size(w): return FULL_SIZE // 2 if w <= 1344 else FULL_SIZE -CAMERAS = [ - ("fcamera.hevc", 20, hevc_size, "roadEncodeIdx"), - ("dcamera.hevc", 20, hevc_size, "driverEncodeIdx"), - ("ecamera.hevc", 20, hevc_size, "wideRoadEncodeIdx"), - ("qcamera.ts", 20, lambda x: 130000, None), -] - -# we check frame count, so we don't have to be too strict on size -FILE_SIZE_TOLERANCE = 0.7 - - -@pytest.mark.tici # TODO: all of loggerd should work on PC -class TestEncoder: - - def setup_method(self): - self._clear_logs() - os.environ["LOGGERD_TEST"] = "1" - os.environ["LOGGERD_SEGMENT_LENGTH"] = str(SEGMENT_LENGTH) - - def teardown_method(self): - self._clear_logs() - - def _clear_logs(self): - if os.path.exists(Paths.log_root()): - shutil.rmtree(Paths.log_root()) - - def _get_latest_segment_path(self): - last_route = sorted(Path(Paths.log_root()).iterdir())[-1] - return os.path.join(Paths.log_root(), last_route) - - # TODO: this should run faster than real time - @parameterized.expand([(True, ), (False, )]) - def test_log_rotation(self, record_front): - Params().put_bool("RecordFront", record_front) - - managed_processes['sensord'].start() - managed_processes['loggerd'].start() - managed_processes['encoderd'].start() - - time.sleep(1.0) - managed_processes['camerad'].start() - - num_segments = int(os.getenv("SEGMENTS", random.randint(2, 8))) - - # wait for loggerd to make the dir for first segment - route_prefix_path = None - with Timeout(int(SEGMENT_LENGTH*3)): - while route_prefix_path is None: - try: - route_prefix_path = self._get_latest_segment_path().rsplit("--", 1)[0] - except Exception: - time.sleep(0.1) - - def check_seg(i): - # check each camera file size - counts = [] - first_frames = [] - for camera, fps, size_lambda, encode_idx_name in CAMERAS: - if not record_front and "dcamera" in camera: - continue - - file_path = f"{route_prefix_path}--{i}/{camera}" - - # check file exists - assert os.path.exists(file_path), f"segment #{i}: '{file_path}' missing" - - # TODO: this ffprobe call is really slow - # get width and check frame count - cmd = f"ffprobe -v error -select_streams v:0 -count_packets -show_entries stream=nb_read_packets,width -of csv=p=0 {file_path}" - if TICI: - cmd = "LD_LIBRARY_PATH=/usr/local/lib " + cmd - - expected_frames = fps * SEGMENT_LENGTH - probe = subprocess.check_output(cmd, shell=True, encoding='utf8').split('\n')[0].strip().split(',') - frame_width, frame_count = int(probe[0]), int(probe[1]) - counts.append(frame_count) - - assert frame_count == expected_frames, \ - f"segment #{i}: {camera} failed frame count check: expected {expected_frames}, got {frame_count}" - - # sanity check file size - file_size = os.path.getsize(file_path) - target_size = size_lambda(frame_width) - assert math.isclose(file_size, target_size, rel_tol=FILE_SIZE_TOLERANCE), \ - f"{file_path} size {file_size} isn't close to target size {target_size}" - - # Check encodeIdx - if encode_idx_name is not None: - rlog_path = f"{route_prefix_path}--{i}/rlog.zst" - msgs = [m for m in LogReader(rlog_path) if m.which() == encode_idx_name] - encode_msgs = [getattr(m, encode_idx_name) for m in msgs] - - valid = [m.valid for m in msgs] - segment_idxs = [m.segmentId for m in encode_msgs] - encode_idxs = [m.encodeId for m in encode_msgs] - frame_idxs = [m.frameId for m in encode_msgs] - - # Check frame count - assert frame_count == len(segment_idxs) - assert frame_count == len(encode_idxs) - - # Check for duplicates or skips - assert 0 == segment_idxs[0] - assert len(set(segment_idxs)) == len(segment_idxs) - - assert all(valid) - - assert expected_frames * i == encode_idxs[0] - first_frames.append(frame_idxs[0]) - assert len(set(encode_idxs)) == len(encode_idxs) - - assert 1 == len(set(first_frames)) - - if TICI: - expected_frames = fps * SEGMENT_LENGTH - assert min(counts) == expected_frames - shutil.rmtree(f"{route_prefix_path}--{i}") - - try: - for i in trange(num_segments): - # poll for next segment - with Timeout(int(SEGMENT_LENGTH*10), error_msg=f"timed out waiting for segment {i}"): - while Path(f"{route_prefix_path}--{i+1}") not in Path(Paths.log_root()).iterdir(): - time.sleep(0.1) - check_seg(i) - finally: - managed_processes['loggerd'].stop() - managed_processes['encoderd'].stop() - managed_processes['camerad'].stop() - managed_processes['sensord'].stop() diff --git a/system/loggerd/tests/test_logger.cc b/system/loggerd/tests/test_logger.cc deleted file mode 100644 index 40a45a68d5cdc3..00000000000000 --- a/system/loggerd/tests/test_logger.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include "catch2/catch.hpp" -#include "system/loggerd/logger.h" - -typedef cereal::Sentinel::SentinelType SentinelType; - -void verify_segment(const std::string &route_path, int segment, int max_segment, int required_event_cnt) { - const std::string segment_path = route_path + "--" + std::to_string(segment); - SentinelType begin_sentinel = segment == 0 ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT; - SentinelType end_sentinel = segment == max_segment - 1 ? SentinelType::END_OF_ROUTE : SentinelType::END_OF_SEGMENT; - - REQUIRE(!util::file_exists(segment_path + "/rlog.lock")); - for (const char *fn : {"/rlog.zst", "/qlog.zst"}) { - const std::string log_file = segment_path + fn; - std::string log = util::read_file(log_file); - REQUIRE(!log.empty()); - std::string decompressed_log = zstd_decompress(log); - int event_cnt = 0, i = 0; - kj::ArrayPtr words((capnp::word *)decompressed_log.data(), decompressed_log.size() / sizeof(capnp::word)); - while (words.size() > 0) { - try { - capnp::FlatArrayMessageReader reader(words); - auto event = reader.getRoot(); - words = kj::arrayPtr(reader.getEnd(), words.end()); - if (i == 0) { - REQUIRE(event.which() == cereal::Event::INIT_DATA); - } else if (i == 1) { - REQUIRE(event.which() == cereal::Event::SENTINEL); - REQUIRE(event.getSentinel().getType() == begin_sentinel); - REQUIRE(event.getSentinel().getSignal() == 0); - } else if (words.size() > 0) { - REQUIRE(event.which() == cereal::Event::CLOCKS); - ++event_cnt; - } else { - // the last event must be SENTINEL - REQUIRE(event.which() == cereal::Event::SENTINEL); - REQUIRE(event.getSentinel().getType() == end_sentinel); - REQUIRE(event.getSentinel().getSignal() == (end_sentinel == SentinelType::END_OF_ROUTE ? 1 : 0)); - } - ++i; - } catch (const kj::Exception &ex) { - INFO("failed parse " << i << " exception :" << ex.getDescription()); - REQUIRE(0); - break; - } - } - REQUIRE(event_cnt == required_event_cnt); - } -} - -void write_msg(LoggerState *logger) { - MessageBuilder msg; - msg.initEvent().initClocks(); - logger->write(msg.toBytes(), true); -} - -TEST_CASE("logger") { - const int segment_cnt = 100; - const std::string log_root = "/tmp/test_logger"; - system(("rm " + log_root + " -rf").c_str()); - std::string route_name; - { - LoggerState logger(log_root); - route_name = logger.routeName(); - for (int i = 0; i < segment_cnt; ++i) { - REQUIRE(logger.next()); - REQUIRE(util::file_exists(logger.segmentPath() + "/rlog.lock")); - REQUIRE(logger.segment() == i); - write_msg(&logger); - } - logger.setExitSignal(1); - } - for (int i = 0; i < segment_cnt; ++i) { - verify_segment(log_root + "/" + route_name, i, segment_cnt, 1); - } -} diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py deleted file mode 100644 index 9703ac2f5fe078..00000000000000 --- a/system/loggerd/tests/test_loggerd.py +++ /dev/null @@ -1,333 +0,0 @@ -import numpy as np -import os -import re -import random -import string -import subprocess -import time -from collections import defaultdict -from pathlib import Path -import pytest - -import cereal.messaging as messaging -from cereal import log -from cereal.services import SERVICE_LIST -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.system.hardware.hw import Paths -from openpilot.system.hardware import TICI -from openpilot.system.loggerd.xattr_cache import getxattr -from openpilot.system.loggerd.deleter import PRESERVE_ATTR_NAME, PRESERVE_ATTR_VALUE -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.version import get_version -from openpilot.tools.lib.helpers import RE -from openpilot.tools.lib.logreader import LogReader -from msgq.visionipc import VisionIpcServer, VisionStreamType - -SentinelType = log.Sentinel.SentinelType - -CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in SERVICE_LIST - and SERVICE_LIST[f].should_log and "encode" not in f.lower()] - - -class TestLoggerd: - def _get_latest_log_dir(self): - log_dirs = sorted(Path(Paths.log_root()).iterdir(), key=lambda f: f.stat().st_mtime) - return log_dirs[-1] - - def _get_log_dir(self, x): - for l in x.splitlines(): - for p in l.split(' '): - path = Path(p.strip()) - if path.is_dir(): - return path - return None - - def _get_log_fn(self, x): - for l in x.splitlines(): - for p in l.split(' '): - path = Path(p.strip()) - if path.is_file(): - return path - return None - - def _gen_bootlog(self): - with Timeout(5): - out = subprocess.check_output("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"), encoding='utf-8') - - log_fn = self._get_log_fn(out) - - # check existence - assert log_fn is not None - - return log_fn - - def _check_init_data(self, msgs): - msg = msgs[0] - assert msg.which() == 'initData' - - def _check_sentinel(self, msgs, route): - start_type = SentinelType.startOfRoute if route else SentinelType.startOfSegment - assert msgs[1].sentinel.type == start_type - - end_type = SentinelType.endOfRoute if route else SentinelType.endOfSegment - assert msgs[-1].sentinel.type == end_type - - def _publish_random_messages(self, services: list[str]) -> dict[str, list]: - pm = messaging.PubMaster(services) - - managed_processes["loggerd"].start() - for s in services: - assert pm.wait_for_readers_to_update(s, timeout=5) - - sent_msgs = defaultdict(list) - for _ in range(random.randint(2, 10) * 100): - for s in services: - try: - m = messaging.new_message(s) - except Exception: - m = messaging.new_message(s, random.randint(2, 10)) - pm.send(s, m) - sent_msgs[s].append(m) - - for s in services: - assert pm.wait_for_readers_to_update(s, timeout=5) - managed_processes["loggerd"].stop() - - return sent_msgs - - def _publish_camera_and_audio_messages(self, num_segs=1, segment_length=5): - # Use small frame sizes for testing (width, height, size, stride, uv_offset) - # NV12 format: size = stride * height * 1.5, uv_offset = stride * height - w, h = 320, 240 - frame_spec = (w, h, w * h * 3 // 2, w, w * h) - streams = [ - (VisionStreamType.VISION_STREAM_ROAD, frame_spec, "roadCameraState"), - (VisionStreamType.VISION_STREAM_DRIVER, frame_spec, "driverCameraState"), - (VisionStreamType.VISION_STREAM_WIDE_ROAD, frame_spec, "wideRoadCameraState"), - ] - - sm = messaging.SubMaster(["roadEncodeData"]) - pm = messaging.PubMaster([s for _, _, s in streams] + ["rawAudioData"]) - vipc_server = VisionIpcServer("camerad") - for stream_type, frame_spec, _ in streams: - vipc_server.create_buffers_with_sizes(stream_type, 40, *(frame_spec)) - vipc_server.start_listener() - - os.environ["LOGGERD_TEST"] = "1" - os.environ["LOGGERD_SEGMENT_LENGTH"] = str(segment_length) - managed_processes["loggerd"].start() - managed_processes["encoderd"].start() - assert pm.wait_for_readers_to_update("roadCameraState", timeout=5) - - fps = 20 - for n in range(1, int(num_segs * segment_length * fps) + 1): - # send video - for stream_type, frame_spec, state in streams: - dat = np.empty(frame_spec[2], dtype=np.uint8) - vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n / fps, n / fps) - - camera_state = messaging.new_message(state) - frame = getattr(camera_state, state) - frame.frameId = n - pm.send(state, camera_state) - - # send audio - msg = messaging.new_message('rawAudioData') - msg.rawAudioData.data = bytes(800 * 2) # 800 samples of int16 - msg.rawAudioData.sampleRate = 16000 - pm.send('rawAudioData', msg) - - for _, _, state in streams: - assert pm.wait_for_readers_to_update(state, timeout=5, dt=0.001) - - sm.update(100) # wait for encode data publish - - managed_processes["loggerd"].stop() - managed_processes["encoderd"].stop() - - def test_init_data_values(self): - os.environ["CLEAN"] = random.choice(["0", "1"]) - - dongle = ''.join(random.choice(string.printable) for n in range(random.randint(1, 100))) - fake_params = [ - # param, initData field, value - ("DongleId", "dongleId", dongle), - ("GitCommit", "gitCommit", "commit"), - ("GitCommitDate", "gitCommitDate", "date"), - ("GitBranch", "gitBranch", "branch"), - ("GitRemote", "gitRemote", "remote"), - ] - params = Params() - for k, _, v in fake_params: - params.put(k, v) - params.put("AccessToken", "abc") - - lr = list(LogReader(str(self._gen_bootlog()))) - initData = lr[0].initData - - assert initData.dirty != bool(os.environ["CLEAN"]) - assert initData.version == get_version() - - if os.path.isfile("/proc/cmdline"): - with open("/proc/cmdline") as f: - assert list(initData.kernelArgs) == f.read().strip().split(" ") - - with open("/proc/version") as f: - assert initData.kernelVersion == f.read() - - # check params - logged_params = {entry.key: entry.value for entry in initData.params.entries} - expected_params = {k for k, _, __ in fake_params} | {'AccessToken', 'BootCount'} - assert set(logged_params.keys()) == expected_params, set(logged_params.keys()) ^ expected_params - assert logged_params['AccessToken'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['AccessToken'])}" - for param_key, initData_key, v in fake_params: - assert getattr(initData, initData_key) == v - assert logged_params[param_key].decode() == v - - @pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing - def test_rotation(self): - Params().put("RecordFront", True) - - expected_files = {"rlog.zst", "qlog.zst", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} - - num_segs = random.randint(2, 3) - length = random.randint(4, 5) # H264 encoder uses 40 lookahead frames and does B-frame reordering, so minimum 3 seconds before qcam output - - self._publish_camera_and_audio_messages(num_segs=num_segs, segment_length=length) - - route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0] - for n in range(num_segs): - p = Path(f"{route_path}--{n}") - logged = {f.name for f in p.iterdir() if f.is_file()} - diff = logged ^ expected_files - assert len(diff) == 0, f"didn't get all expected files. seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}" - - def test_bootlog(self): - # generate bootlog with fake launch log - launch_log = ''.join(str(random.choice(string.printable)) for _ in range(100)) - with open("/tmp/launch_log", "w") as f: - f.write(launch_log) - - bootlog_path = self._gen_bootlog() - lr = list(LogReader(str(bootlog_path))) - - # check length - assert len(lr) == 2 # boot + initData - - self._check_init_data(lr) - - # check msgs - bootlog_msgs = [m for m in lr if m.which() == 'boot'] - assert len(bootlog_msgs) == 1 - - # sanity check values - boot = bootlog_msgs.pop().boot - assert abs(boot.wallTimeNanos - time.time_ns()) < 5*1e9 # within 5s - assert boot.launchLog == launch_log - - if TICI: - for fn in ["console-ramoops", "pmsg-ramoops-0"]: - path = Path(os.path.join("/sys/fs/pstore/", fn)) - if path.is_file(): - with open(path, "rb") as f: - expected_val = f.read() - bootlog_val = [e.value for e in boot.pstore.entries if e.key == fn][0] - assert expected_val == bootlog_val - else: - assert len(boot.pstore.entries) == 0 - - # next one should increment by one - bl1 = re.match(RE.LOG_ID_V2, bootlog_path.name) - bl2 = re.match(RE.LOG_ID_V2, self._gen_bootlog().name) - assert bl1.group('uid') != bl2.group('uid') - assert int(bl1.group('count')) == 0 and int(bl2.group('count')) == 1 - - def test_qlog(self): - qlog_services = [s for s in CEREAL_SERVICES if SERVICE_LIST[s].decimation is not None] - no_qlog_services = [s for s in CEREAL_SERVICES if SERVICE_LIST[s].decimation is None] - - services = random.sample(qlog_services, random.randint(2, min(10, len(qlog_services)))) + \ - random.sample(no_qlog_services, random.randint(2, min(10, len(no_qlog_services)))) - sent_msgs = self._publish_random_messages(services) - - qlog_path = os.path.join(self._get_latest_log_dir(), "qlog.zst") - lr = list(LogReader(qlog_path)) - - # check initData and sentinel - self._check_init_data(lr) - self._check_sentinel(lr, True) - - recv_msgs = defaultdict(list) - for m in lr: - recv_msgs[m.which()].append(m) - - for s, msgs in sent_msgs.items(): - recv_cnt = len(recv_msgs[s]) - - if s in no_qlog_services: - # check services with no specific decimation aren't in qlog - assert recv_cnt == 0, f"got {recv_cnt} {s} msgs in qlog" - else: - # check logged message count matches decimation - expected_cnt = (len(msgs) - 1) // SERVICE_LIST[s].decimation + 1 - assert recv_cnt == expected_cnt, f"expected {expected_cnt} msgs for {s}, got {recv_cnt}" - - def test_rlog(self): - services = random.sample(CEREAL_SERVICES, random.randint(5, 10)) - sent_msgs = self._publish_random_messages(services) - - lr = list(LogReader(os.path.join(self._get_latest_log_dir(), "rlog.zst"))) - - # check initData and sentinel - self._check_init_data(lr) - self._check_sentinel(lr, True) - - # check all messages were logged and in order - lr = lr[2:-1] # slice off initData and both sentinels - for m in lr: - sent = sent_msgs[m.which()].pop(0) - sent.clear_write_flag() - assert sent.to_bytes() == m.as_builder().to_bytes() - - def test_preserving_bookmarked_segments(self): - services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) | {"userBookmark"} - self._publish_random_messages(services) - - segment_dir = self._get_latest_log_dir() - assert getxattr(segment_dir, PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE - - def test_not_preserving_nonbookmarked_segments(self): - services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) - {"userBookmark", "audioFeedback"} - self._publish_random_messages(services) - - segment_dir = self._get_latest_log_dir() - assert getxattr(segment_dir, PRESERVE_ATTR_NAME) is None - - @pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing - @pytest.mark.parametrize("record_front", [True, False]) - def test_record_front(self, record_front): - params = Params() - params.put_bool("RecordFront", record_front) - - self._publish_camera_and_audio_messages() - - dcamera_hevc_exists = os.path.exists(os.path.join(self._get_latest_log_dir(), 'dcamera.hevc')) - assert dcamera_hevc_exists == record_front - - @pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing - @pytest.mark.parametrize("record_audio", [True, False]) - def test_record_audio(self, record_audio): - params = Params() - params.put_bool("RecordAudio", record_audio) - - self._publish_camera_and_audio_messages() - - qcamera_ts_path = os.path.join(self._get_latest_log_dir(), 'qcamera.ts') - ffprobe_cmd = f"ffprobe -i {qcamera_ts_path} -show_streams -select_streams a -loglevel error" - has_audio_stream = subprocess.run(ffprobe_cmd, shell=True, capture_output=True).stdout.strip() != b'' - assert has_audio_stream == record_audio - - raw_audio_in_rlog = any(m.which() == 'rawAudioData' for m in LogReader(os.path.join(self._get_latest_log_dir(), 'rlog.zst'))) - assert raw_audio_in_rlog == record_audio diff --git a/system/loggerd/tests/test_runner.cc b/system/loggerd/tests/test_runner.cc deleted file mode 100644 index 62bf7476a18996..00000000000000 --- a/system/loggerd/tests/test_runner.cc +++ /dev/null @@ -1,2 +0,0 @@ -#define CATCH_CONFIG_MAIN -#include "catch2/catch.hpp" diff --git a/system/loggerd/tests/test_uploader.py b/system/loggerd/tests/test_uploader.py deleted file mode 100644 index 562bc068eb80e8..00000000000000 --- a/system/loggerd/tests/test_uploader.py +++ /dev/null @@ -1,184 +0,0 @@ -import os -import time -import threading -import logging -import json -from pathlib import Path -from openpilot.system.hardware.hw import Paths - -from openpilot.common.swaglog import cloudlog -from openpilot.system.loggerd.uploader import main, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE - -from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase - - -class FakeLogHandler(logging.Handler): - def __init__(self): - logging.Handler.__init__(self) - self.reset() - - def reset(self): - self.upload_order = list() - self.upload_ignored = list() - - def emit(self, record): - try: - j = json.loads(record.getMessage()) - if j["event"] == "upload_success": - self.upload_order.append(j["key"]) - if j["event"] == "upload_ignored": - self.upload_ignored.append(j["key"]) - except Exception: - pass - -log_handler = FakeLogHandler() -cloudlog.addHandler(log_handler) - - -class TestUploader(UploaderTestCase): - def setup_method(self): - super().setup_method() - log_handler.reset() - - def start_thread(self): - self.end_event = threading.Event() - self.up_thread = threading.Thread(target=main, args=[self.end_event]) - self.up_thread.daemon = True - self.up_thread.start() - - def join_thread(self): - self.end_event.set() - self.up_thread.join() - - def gen_files(self, lock=False, xattr: bytes | None = None, boot=True) -> list[Path]: - f_paths = [] - for t in ["qlog", "rlog", "dcamera.hevc", "fcamera.hevc"]: - f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock, upload_xattr=xattr)) - - if boot: - f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock, upload_xattr=xattr)) - return f_paths - - def gen_order(self, seg1: list[int], seg2: list[int], boot=True) -> list[str]: - keys = [] - if boot: - keys += [f"boot/{self.seg_format.format(i)}.zst" for i in seg1] - keys += [f"boot/{self.seg_format2.format(i)}.zst" for i in seg2] - keys += [f"{self.seg_format.format(i)}/qlog.zst" for i in seg1] - keys += [f"{self.seg_format2.format(i)}/qlog.zst" for i in seg2] - return keys - - def test_upload(self): - self.gen_files(lock=False) - - self.start_thread() - # allow enough time that files could upload twice if there is a bug in the logic - time.sleep(1) - self.join_thread() - - exp_order = self.gen_order([self.seg_num], []) - - assert len(log_handler.upload_ignored) == 0, "Some files were ignored" - assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" - assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" - for f_path in exp_order: - assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" - - assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" - - def test_upload_with_wrong_xattr(self): - self.gen_files(lock=False, xattr=b'0') - - self.start_thread() - # allow enough time that files could upload twice if there is a bug in the logic - time.sleep(1) - self.join_thread() - - exp_order = self.gen_order([self.seg_num], []) - - assert len(log_handler.upload_ignored) == 0, "Some files were ignored" - assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" - assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" - for f_path in exp_order: - assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" - - assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" - - def test_upload_ignored(self): - self.set_ignore() - self.gen_files(lock=False) - - self.start_thread() - # allow enough time that files could upload twice if there is a bug in the logic - time.sleep(1) - self.join_thread() - - exp_order = self.gen_order([self.seg_num], []) - - assert len(log_handler.upload_order) == 0, "Some files were not ignored" - assert not len(log_handler.upload_ignored) < len(exp_order), "Some files failed to ignore" - assert not len(log_handler.upload_ignored) > len(exp_order), "Some files were ignored twice" - for f_path in exp_order: - assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not ignored" - - assert log_handler.upload_ignored == exp_order, "Files ignored in wrong order" - - def test_upload_files_in_create_order(self): - seg1_nums = [0, 1, 2, 10, 20] - for i in seg1_nums: - self.seg_dir = self.seg_format.format(i) - self.gen_files(boot=False) - seg2_nums = [5, 50, 51] - for i in seg2_nums: - self.seg_dir = self.seg_format2.format(i) - self.gen_files(boot=False) - - exp_order = self.gen_order(seg1_nums, seg2_nums, boot=False) - - self.start_thread() - # allow enough time that files could upload twice if there is a bug in the logic - time.sleep(1) - self.join_thread() - - assert len(log_handler.upload_ignored) == 0, "Some files were ignored" - assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" - assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" - for f_path in exp_order: - assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" - - assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" - - def test_no_upload_with_lock_file(self): - self.start_thread() - - time.sleep(0.25) - f_paths = self.gen_files(lock=True, boot=False) - - # allow enough time that files should have been uploaded if they would be uploaded - time.sleep(1) - self.join_thread() - - for f_path in f_paths: - fn = f_path.with_suffix(f_path.suffix.replace(".zst", "")) - uploaded = UPLOAD_ATTR_NAME in os.listxattr(fn) and os.getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE - assert not uploaded, "File upload when locked" - - def test_no_upload_with_xattr(self): - self.gen_files(lock=False, xattr=UPLOAD_ATTR_VALUE) - - self.start_thread() - # allow enough time that files could upload twice if there is a bug in the logic - time.sleep(1) - self.join_thread() - - assert len(log_handler.upload_order) == 0, "File uploaded again" - - def test_clear_locks_on_startup(self): - f_paths = self.gen_files(lock=True, boot=False) - self.start_thread() - time.sleep(0.25) - self.join_thread() - - for f_path in f_paths: - lock_path = f_path.with_suffix(f_path.suffix + ".lock") - assert not lock_path.is_file(), "File lock not cleared on startup" diff --git a/system/loggerd/tests/test_zstd_writer.cc b/system/loggerd/tests/test_zstd_writer.cc deleted file mode 100644 index 479e866a14ac99..00000000000000 --- a/system/loggerd/tests/test_zstd_writer.cc +++ /dev/null @@ -1,44 +0,0 @@ -#include - -#include -#include -#include - -#include "common/util.h" -#include "system/loggerd/logger.h" -#include "system/loggerd/zstd_writer.h" - -TEST_CASE("ZstdFileWriter writes and compresses data correctly in loops", "[ZstdFileWriter]") { - const std::string filename = "test_zstd_file.zst"; - const int iterations = 100; - const size_t dataSize = 1024; - - std::string totalTestData; - - // Step 1: Write compressed data to file in a loop - { - ZstdFileWriter writer(filename, LOG_COMPRESSION_LEVEL); - // Write various data sizes including edge cases - std::vector testSizes = {dataSize, 1, 0, dataSize * 2}; // Normal, minimal, empty, large - for (int i = 0; i < iterations; ++i) { - size_t currentSize = testSizes[i % testSizes.size()]; - std::string testData = util::random_string(currentSize); - totalTestData.append(testData); - - writer.write((void *)testData.c_str(), testData.size()); - } - } - - // Step 2: Decompress the file and verify the data - auto compressedContent = util::read_file(filename); - REQUIRE(compressedContent.size() > 0); - REQUIRE(compressedContent.size() < totalTestData.size()); - std::string decompressedData = zstd_decompress(compressedContent); - - // Step 3: Verify that the decompressed data matches the original accumulated data - REQUIRE(decompressedData.size() == totalTestData.size()); - REQUIRE(std::memcmp(decompressedData.data(), totalTestData.c_str(), totalTestData.size()) == 0); - - // Clean up the test file - std::remove(filename.c_str()); -} diff --git a/system/loggerd/tests/vidc_debug.sh b/system/loggerd/tests/vidc_debug.sh deleted file mode 100755 index 7471f2ab082cbe..00000000000000 --- a/system/loggerd/tests/vidc_debug.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/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/loggerd/uploader.py b/system/loggerd/uploader.py deleted file mode 100755 index 8ac38b6df6a76d..00000000000000 --- a/system/loggerd/uploader.py +++ /dev/null @@ -1,273 +0,0 @@ -#!/usr/bin/env python3 -import json -import os -import random -import requests -import threading -import time -import traceback -import datetime -from collections.abc import Iterator - -from cereal import log -import cereal.messaging as messaging -from openpilot.common.api import Api -from openpilot.common.utils import get_upload_stream -from openpilot.common.params import Params -from openpilot.common.realtime import set_core_affinity -from openpilot.system.hardware.hw import Paths -from openpilot.system.loggerd.xattr_cache import getxattr, setxattr -from openpilot.common.swaglog import cloudlog - -NetworkType = log.DeviceState.NetworkType -UPLOAD_ATTR_NAME = 'user.upload' -UPLOAD_ATTR_VALUE = b'1' - -MAX_UPLOAD_SIZES = { - "qlog": 25*1e6, # can't be too restrictive here since we use qlogs to find - # bugs, including ones that can cause massive log sizes - "qcam": 5*1e6, -} - -allow_sleep = bool(int(os.getenv("UPLOADER_SLEEP", "1"))) -force_wifi = os.getenv("FORCEWIFI") is not None -fake_upload = os.getenv("FAKEUPLOAD") is not None - - -class FakeRequest: - def __init__(self): - self.headers = {"Content-Length": "0"} - - -class FakeResponse: - def __init__(self): - self.status_code = 200 - self.request = FakeRequest() - - -def get_directory_sort(d: str) -> list[str]: - # ensure old format is sorted sooner - o = ["0", ] if d.startswith("2024-") else ["1", ] - return o + [s.rjust(10, '0') for s in d.rsplit('--', 1)] - -def listdir_by_creation(d: str) -> list[str]: - if not os.path.isdir(d): - return [] - - try: - paths = [f for f in os.listdir(d) if os.path.isdir(os.path.join(d, f))] - paths = sorted(paths, key=get_directory_sort) - return paths - except OSError: - cloudlog.exception("listdir_by_creation failed") - return [] - -def clear_locks(root: str) -> None: - for logdir in os.listdir(root): - path = os.path.join(root, logdir) - try: - for fname in os.listdir(path): - if fname.endswith(".lock"): - os.unlink(os.path.join(path, fname)) - except OSError: - cloudlog.exception("clear_locks failed") - - -class Uploader: - def __init__(self, dongle_id: str, root: str): - self.dongle_id = dongle_id - self.api = Api(dongle_id) - self.root = root - - self.params = Params() - - # stats for last successfully uploaded file - self.last_filename = "" - - self.immediate_folders = ["crash/", "boot/"] - self.immediate_priority = {"qlog": 0, "qlog.zst": 0, "qcamera.ts": 1} - - def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]: - r = self.params.get("AthenadRecentlyViewedRoutes") - requested_routes = [] if r is None else [route for route in r.split(",") if route] - - for logdir in listdir_by_creation(self.root): - path = os.path.join(self.root, logdir) - try: - names = os.listdir(path) - except OSError: - continue - - if any(name.endswith(".lock") for name in names): - continue - - for name in sorted(names, key=lambda n: self.immediate_priority.get(n, 1000)): - key = os.path.join(logdir, name) - fn = os.path.join(path, name) - # skip files already uploaded - try: - ctime = os.path.getctime(fn) - is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE - except OSError: - cloudlog.event("uploader_getxattr_failed", key=key, fn=fn) - # deleter could have deleted, so skip - continue - if is_uploaded: - continue - - # limit uploading on metered connections - if metered: - dt = datetime.timedelta(hours=12) - if logdir in self.immediate_folders and (datetime.datetime.now() - datetime.datetime.fromtimestamp(ctime)) < dt: - continue - - if name == "qcamera.ts" and not any(logdir.startswith(r.split('|')[-1]) for r in requested_routes): - continue - - yield name, key, fn - - def next_file_to_upload(self, metered: bool) -> tuple[str, str, str] | None: - upload_files = list(self.list_upload_files(metered)) - - for name, key, fn in upload_files: - if any(f in fn for f in self.immediate_folders): - return name, key, fn - - for name, key, fn in upload_files: - if name in self.immediate_priority: - return name, key, fn - - return None - - def do_upload(self, key: str, fn: str): - url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) - if url_resp.status_code == 412: - return url_resp - - url_resp_json = json.loads(url_resp.text) - url = url_resp_json['url'] - headers = url_resp_json['headers'] - cloudlog.debug("upload_url v1.4 %s %s", url, str(headers)) - - if fake_upload: - return FakeResponse() - - stream = None - try: - compress = key.endswith('.zst') and not fn.endswith('.zst') - stream, _ = get_upload_stream(fn, compress) - response = requests.put(url, data=stream, headers=headers, timeout=10) - return response - finally: - if stream: - stream.close() - - def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) -> bool: - try: - sz = os.path.getsize(fn) - except OSError: - cloudlog.exception("upload: getsize failed") - return False - - cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) - - if sz == 0: - # tag files of 0 size as uploaded - success = True - elif name in MAX_UPLOAD_SIZES and sz > MAX_UPLOAD_SIZES[name]: - cloudlog.event("uploader_too_large", key=key, fn=fn, sz=sz) - success = True - else: - start_time = time.monotonic() - - stat = None - last_exc = None - try: - stat = self.do_upload(key, fn) - except Exception as e: - last_exc = (e, traceback.format_exc()) - - if stat is not None and stat.status_code in (200, 201, 401, 403, 412): - self.last_filename = fn - dt = time.monotonic() - start_time - if stat.status_code == 412: - cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) - else: - content_length = int(stat.request.headers.get("Content-Length", 0)) - speed = (content_length / 1e6) / dt - cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length, - network_type=network_type, metered=metered, speed=speed) - success = True - else: - success = False - cloudlog.event("upload_failed", stat=stat, exc=last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered) - - if success: - # tag file as uploaded - try: - setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) - except OSError: - cloudlog.event("uploader_setxattr_failed", exc=last_exc, key=key, fn=fn, sz=sz) - - return success - - - def step(self, network_type: int, metered: bool) -> bool | None: - d = self.next_file_to_upload(metered) - if d is None: - return None - - name, key, fn = d - - # qlogs and bootlogs need to be compressed before uploading - if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.zst')): - key += ".zst" - - return self.upload(name, key, fn, network_type, metered) - - -def main(exit_event: threading.Event | None = None) -> None: - if exit_event is None: - exit_event = threading.Event() - - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("failed to set core affinity") - - clear_locks(Paths.log_root()) - - params = Params() - dongle_id = params.get("DongleId") - - if dongle_id is None: - cloudlog.info("uploader missing dongle_id") - raise Exception("uploader can't start without dongle id") - - sm = messaging.SubMaster(['deviceState']) - uploader = Uploader(dongle_id, Paths.log_root()) - - backoff = 0.1 - while not exit_event.is_set(): - sm.update(0) - offroad = params.get_bool("IsOffroad") - network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi - if network_type == NetworkType.none: - if allow_sleep: - time.sleep(60 if offroad else 5) - continue - - success = uploader.step(sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered) - if success is None: - backoff = 60 if offroad else 5 - elif success: - backoff = 0.1 - else: - cloudlog.info("upload backoff %r", backoff) - backoff = min(backoff*2, 120) - if allow_sleep: - time.sleep(backoff + random.uniform(0, backoff)) - - -if __name__ == "__main__": - main() diff --git a/system/loggerd/video_writer.cc b/system/loggerd/video_writer.cc deleted file mode 100644 index 1b47a8fceba613..00000000000000 --- a/system/loggerd/video_writer.cc +++ /dev/null @@ -1,234 +0,0 @@ -#include - -#include "system/loggerd/video_writer.h" -#include "common/swaglog.h" -#include "common/util.h" - -VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec) - : remuxing(remuxing) { - vid_path = util::string_format("%s/%s", path, filename); - lock_path = util::string_format("%s/%s.lock", path, filename); - - int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664)); - assert(lock_fd >= 0); - close(lock_fd); - - LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing); - if (this->remuxing) { - bool raw = (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS); - avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str()); - assert(this->ofmt_ctx); - - // set codec correctly. needed? - assert(codec != cereal::EncodeIndex::Type::FULL_H_E_V_C); - const AVCodec *avcodec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264); - assert(avcodec); - - this->codec_ctx = avcodec_alloc_context3(avcodec); - assert(this->codec_ctx); - this->codec_ctx->width = width; - this->codec_ctx->height = height; - this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; - this->codec_ctx->time_base = (AVRational){ 1, fps }; - - if (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS) { - // without this, there's just noise - int err = avcodec_open2(this->codec_ctx, avcodec, NULL); - assert(err >= 0); - } - - this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? avcodec : NULL); - assert(this->out_stream); - - int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE); - assert(err >= 0); - - } else { - this->of = util::safe_fopen(this->vid_path.c_str(), "wb"); - assert(this->of); - } -} - -void VideoWriter::initialize_audio(int sample_rate) { - assert(this->ofmt_ctx->oformat->audio_codec != AV_CODEC_ID_NONE); // check output format supports audio streams - const AVCodec *audio_avcodec = avcodec_find_encoder(AV_CODEC_ID_AAC); - assert(audio_avcodec); - this->audio_codec_ctx = avcodec_alloc_context3(audio_avcodec); - assert(this->audio_codec_ctx); - this->audio_codec_ctx->sample_fmt = AV_SAMPLE_FMT_FLTP; - this->audio_codec_ctx->sample_rate = sample_rate; - #if LIBAVUTIL_VERSION_INT >= AV_VERSION_INT(57, 28, 100) // FFmpeg 5.1+ - av_channel_layout_default(&this->audio_codec_ctx->ch_layout, 1); - #else - this->audio_codec_ctx->channel_layout = AV_CH_LAYOUT_MONO; - #endif - this->audio_codec_ctx->bit_rate = 32000; - this->audio_codec_ctx->flags |= AV_CODEC_FLAG_GLOBAL_HEADER; - this->audio_codec_ctx->time_base = (AVRational){1, audio_codec_ctx->sample_rate}; - int err = avcodec_open2(this->audio_codec_ctx, audio_avcodec, NULL); - assert(err >= 0); - av_log_set_level(AV_LOG_WARNING); // hide "QAvg" info msgs at the end of every segment - - this->audio_stream = avformat_new_stream(this->ofmt_ctx, NULL); - assert(this->audio_stream); - err = avcodec_parameters_from_context(this->audio_stream->codecpar, this->audio_codec_ctx); - assert(err >= 0); - - this->audio_frame = av_frame_alloc(); - assert(this->audio_frame); - this->audio_frame->format = this->audio_codec_ctx->sample_fmt; - #if LIBAVUTIL_VERSION_INT >= AV_VERSION_INT(57, 28, 100) // FFmpeg 5.1+ - av_channel_layout_copy(&this->audio_frame->ch_layout, &this->audio_codec_ctx->ch_layout); - #else - this->audio_frame->channel_layout = this->audio_codec_ctx->channel_layout; - #endif - this->audio_frame->sample_rate = this->audio_codec_ctx->sample_rate; - this->audio_frame->nb_samples = this->audio_codec_ctx->frame_size; - err = av_frame_get_buffer(this->audio_frame, 0); - assert(err >= 0); -} - -void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe) { - if (of && data) { - size_t written = util::safe_fwrite(data, 1, len, of); - if (written != len) { - LOGE("failed to write file.errno=%d", errno); - } - } - - if (remuxing) { - if (codecconfig) { - if (len > 0) { - codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE); - codec_ctx->extradata_size = len; - memcpy(codec_ctx->extradata, data, len); - } - int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx); - assert(err >= 0); - // if there is an audio stream, it must be initialized before this point - err = avformat_write_header(ofmt_ctx, NULL); - assert(err >= 0); - header_written = true; - } else { - // input timestamps are in microseconds - AVRational in_timebase = {1, 1000000}; - - AVPacket pkt = {}; - pkt.data = data; - pkt.size = len; - pkt.stream_index = this->out_stream->index; - - enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); - pkt.pts = pkt.dts = av_rescale_q_rnd(timestamp, in_timebase, ofmt_ctx->streams[0]->time_base, rnd); - pkt.duration = av_rescale_q(50*1000, in_timebase, ofmt_ctx->streams[0]->time_base); - - if (keyframe) { - pkt.flags |= AV_PKT_FLAG_KEY; - } - - // TODO: can use av_write_frame for non raw? - int err = av_interleaved_write_frame(ofmt_ctx, &pkt); - if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lld", len, timestamp); } - - av_packet_unref(&pkt); - } - } -} - -void VideoWriter::write_audio(uint8_t *data, int len, long long timestamp, int sample_rate) { - if (!remuxing) return; - if (!audio_initialized) { - initialize_audio(sample_rate); - audio_initialized = true; - } - if (!audio_codec_ctx) return; - // sync logMonoTime of first audio packet with the timestampEof of first video packet - if (audio_pts == 0) { - audio_pts = (timestamp * audio_codec_ctx->sample_rate) / 1000000ULL; - } - - // convert s16le samples to fltp and add to buffer - const int16_t *raw_samples = reinterpret_cast(data); - int sample_count = len / sizeof(int16_t); - constexpr float normalizer = 1.0f / 32768.0f; - - const size_t max_buffer_size = sample_rate * 10; // 10 seconds - if (audio_buffer.size() + sample_count > max_buffer_size) { - size_t samples_to_drop = (audio_buffer.size() + sample_count) - max_buffer_size; - LOGE("Audio buffer overflow, dropping %zu oldest samples", samples_to_drop); - audio_buffer.erase(audio_buffer.begin(), audio_buffer.begin() + samples_to_drop); - audio_pts += samples_to_drop; - } - - // Add new samples to the buffer - const size_t original_size = audio_buffer.size(); - audio_buffer.resize(original_size + sample_count); - std::transform(raw_samples, raw_samples + sample_count, audio_buffer.begin() + original_size, - [](int16_t sample) { return sample * normalizer; }); - - if (!header_written) return; // header not written yet, process audio frame after header is written - while (audio_buffer.size() >= audio_codec_ctx->frame_size) { - audio_frame->pts = audio_pts; - float *f_samples = reinterpret_cast(audio_frame->data[0]); - std::copy(audio_buffer.begin(), audio_buffer.begin() + audio_codec_ctx->frame_size, f_samples); - audio_buffer.erase(audio_buffer.begin(), audio_buffer.begin() + audio_codec_ctx->frame_size); - encode_and_write_audio_frame(audio_frame); - } -} - -void VideoWriter::encode_and_write_audio_frame(AVFrame* frame) { - if (!remuxing || !audio_codec_ctx) return; - int send_result = avcodec_send_frame(audio_codec_ctx, frame); // encode frame - if (send_result >= 0) { - AVPacket *pkt = av_packet_alloc(); - while (avcodec_receive_packet(audio_codec_ctx, pkt) == 0) { - av_packet_rescale_ts(pkt, audio_codec_ctx->time_base, audio_stream->time_base); - pkt->stream_index = audio_stream->index; - - int err = av_interleaved_write_frame(ofmt_ctx, pkt); // write encoded frame - if (err < 0) { - LOGW("AUDIO: Write frame failed - error: %d", err); - } - av_packet_unref(pkt); - } - av_packet_free(&pkt); - } else { - LOGW("AUDIO: Failed to send audio frame to encoder: %d", send_result); - } - audio_pts += audio_codec_ctx->frame_size; -} - -void VideoWriter::process_remaining_audio() { - // Process remaining audio samples by padding with silence - if (audio_buffer.size() > 0 && audio_buffer.size() < audio_codec_ctx->frame_size) { - audio_buffer.resize(audio_codec_ctx->frame_size, 0.0f); - - // Encode final frame - audio_frame->pts = audio_pts; - float *f_samples = reinterpret_cast(audio_frame->data[0]); - std::copy(audio_buffer.begin(), audio_buffer.end(), f_samples); - encode_and_write_audio_frame(audio_frame); - } -} - -VideoWriter::~VideoWriter() { - if (this->remuxing) { - if (this->audio_codec_ctx) { - process_remaining_audio(); - encode_and_write_audio_frame(NULL); // flush encoder - avcodec_free_context(&this->audio_codec_ctx); - } - int err = av_write_trailer(this->ofmt_ctx); - if (err != 0) LOGE("av_write_trailer failed %d", err); - avcodec_free_context(&this->codec_ctx); - if (this->audio_frame) av_frame_free(&this->audio_frame); - err = avio_closep(&this->ofmt_ctx->pb); - if (err != 0) LOGE("avio_closep failed %d", err); - avformat_free_context(this->ofmt_ctx); - } else { - util::safe_fflush(this->of); - fclose(this->of); - this->of = nullptr; - } - unlink(this->lock_path.c_str()); -} diff --git a/system/loggerd/video_writer.h b/system/loggerd/video_writer.h deleted file mode 100644 index e973c5d8114d58..00000000000000 --- a/system/loggerd/video_writer.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include -#include - -extern "C" { -#include -#include -} - -#include "cereal/messaging/messaging.h" - -class VideoWriter { -public: - VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec); - void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe); - void write_audio(uint8_t *data, int len, long long timestamp, int sample_rate); - - ~VideoWriter(); - -private: - void initialize_audio(int sample_rate); - void encode_and_write_audio_frame(AVFrame* frame); - void process_remaining_audio(); - - std::string vid_path, lock_path; - FILE *of = nullptr; - - AVCodecContext *codec_ctx; - AVFormatContext *ofmt_ctx; - AVStream *out_stream; - - bool audio_initialized = false; - bool header_written = false; - AVStream *audio_stream = nullptr; - AVCodecContext *audio_codec_ctx = nullptr; - AVFrame *audio_frame = nullptr; - uint64_t audio_pts = 0; - std::deque audio_buffer; - - bool remuxing; -}; diff --git a/system/loggerd/xattr_cache.py b/system/loggerd/xattr_cache.py deleted file mode 100644 index 39bb1720598cc4..00000000000000 --- a/system/loggerd/xattr_cache.py +++ /dev/null @@ -1,23 +0,0 @@ -import errno - -import xattr - -_cached_attributes: dict[tuple, bytes | None] = {} - -def getxattr(path: str, attr_name: str) -> bytes | None: - key = (path, attr_name) - if key not in _cached_attributes: - try: - response = xattr.getxattr(path, attr_name) - except OSError as e: - # ENODATA (Linux) or ENOATTR (macOS) means attribute hasn't been set - if e.errno == errno.ENODATA or (hasattr(errno, 'ENOATTR') and e.errno == errno.ENOATTR): - response = None - else: - raise - _cached_attributes[key] = response - return _cached_attributes[key] - -def setxattr(path: str, attr_name: str, attr_value: bytes) -> None: - _cached_attributes.pop((path, attr_name), None) - xattr.setxattr(path, attr_name, attr_value) diff --git a/system/loggerd/zstd_writer.cc b/system/loggerd/zstd_writer.cc deleted file mode 100644 index 69ca64479edf05..00000000000000 --- a/system/loggerd/zstd_writer.cc +++ /dev/null @@ -1,65 +0,0 @@ - -#include "system/loggerd/zstd_writer.h" - -#include - -#include "common/util.h" - -// Constructor: Initializes compression stream and opens file -ZstdFileWriter::ZstdFileWriter(const std::string& filename, int compression_level) { - // Create the compression stream - cstream_ = ZSTD_createCStream(); - assert(cstream_); - - size_t initResult = ZSTD_initCStream(cstream_, compression_level); - assert(!ZSTD_isError(initResult)); - - input_cache_capacity_ = ZSTD_CStreamInSize(); - input_cache_.reserve(input_cache_capacity_); - output_buffer_.resize(ZSTD_CStreamOutSize()); - - file_ = util::safe_fopen(filename.c_str(), "wb"); - assert(file_ != nullptr); -} - -// Destructor: Finalizes compression and closes file -ZstdFileWriter::~ZstdFileWriter() { - flushCache(true); - util::safe_fflush(file_); - - int err = fclose(file_); - assert(err == 0); - - ZSTD_freeCStream(cstream_); -} - -// Compresses and writes data to file -void ZstdFileWriter::write(void* data, size_t size) { - // Add data to the input cache - input_cache_.insert(input_cache_.end(), (uint8_t*)data, (uint8_t*)data + size); - - // If the cache is full, compress and write to the file - if (input_cache_.size() >= input_cache_capacity_) { - flushCache(false); - } -} - -// Compress and flush the input cache to the file -void ZstdFileWriter::flushCache(bool last_chunk) { - ZSTD_inBuffer input = {input_cache_.data(), input_cache_.size(), 0}; - ZSTD_EndDirective mode = !last_chunk ? ZSTD_e_continue : ZSTD_e_end; - int finished = 0; - - do { - ZSTD_outBuffer output = {output_buffer_.data(), output_buffer_.size(), 0}; - size_t remaining = ZSTD_compressStream2(cstream_, &output, &input, mode); - assert(!ZSTD_isError(remaining)); - - size_t written = util::safe_fwrite(output_buffer_.data(), 1, output.pos, file_); - assert(written == output.pos); - - finished = last_chunk ? (remaining == 0) : (input.pos == input.size); - } while (!finished); - - input_cache_.clear(); // Clear cache after compression -} diff --git a/system/loggerd/zstd_writer.h b/system/loggerd/zstd_writer.h deleted file mode 100644 index b11deaab2055b5..00000000000000 --- a/system/loggerd/zstd_writer.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -class ZstdFileWriter { -public: - ZstdFileWriter(const std::string &filename, int compression_level); - ~ZstdFileWriter(); - void write(void* data, size_t size); - inline void write(kj::ArrayPtr array) { write(array.begin(), array.size()); } - -private: - void flushCache(bool last_chunk); - - size_t input_cache_capacity_ = 0; - std::vector input_cache_; - std::vector output_buffer_; - ZSTD_CStream *cstream_; - FILE* file_ = nullptr; -}; diff --git a/system/logmessaged.py b/system/logmessaged.py index c095c261926b8c..280a23cf1d6b40 100755 --- a/system/logmessaged.py +++ b/system/logmessaged.py @@ -3,9 +3,8 @@ from typing import NoReturn import cereal.messaging as messaging -from openpilot.common.logging_extra import SwagLogFileFormatter -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import get_file_handler +from common.logging_extra import SwagLogFileFormatter +from system.swaglog import get_file_handler def main() -> NoReturn: @@ -13,43 +12,31 @@ def main() -> NoReturn: log_handler.setFormatter(SwagLogFileFormatter(None)) log_level = 20 # logging.INFO - ctx = zmq.Context.instance() + ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) - sock.bind(Paths.swaglog_ipc()) + sock.bind("ipc:///tmp/logmessage") # and we publish them log_message_sock = messaging.pub_sock('logMessage') error_log_message_sock = messaging.pub_sock('errorLogMessage') - try: - while True: - dat = b''.join(sock.recv_multipart()) - level = dat[0] - record = dat[1:].decode("utf-8") - if level >= log_level: - log_handler.emit(record) - - if len(record) > 2*1024*1024: - print("WARNING: log too big to publish", len(record)) - print(record[:100]) - continue - - # then we publish them - msg = messaging.new_message(None, valid=True, logMessage=record) - log_message_sock.send(msg.to_bytes()) - - if level >= 40: # logging.ERROR - msg = messaging.new_message(None, valid=True, errorLogMessage=record) - error_log_message_sock.send(msg.to_bytes()) - finally: - sock.close() - ctx.term() - - # can hit this if interrupted during a rollover - try: - log_handler.close() - except ValueError: - pass + while True: + dat = b''.join(sock.recv_multipart()) + level = dat[0] + record = dat[1:].decode("utf-8") + if level >= log_level: + log_handler.emit(record) + + # then we publish them + msg = messaging.new_message() + msg.logMessage = record + log_message_sock.send(msg.to_bytes()) + + if level >= 40: # logging.ERROR + msg = messaging.new_message() + msg.errorLogMessage = record + error_log_message_sock.send(msg.to_bytes()) + if __name__ == "__main__": main() diff --git a/system/manager/build.py b/system/manager/build.py deleted file mode 100755 index d79e7fd2ad906e..00000000000000 --- a/system/manager/build.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 -import os -import subprocess -from pathlib import Path - -# NOTE: Do NOT import anything here that needs be built (e.g. params) -from openpilot.common.basedir import BASEDIR -from openpilot.common.spinner import Spinner -from openpilot.common.text_window import TextWindow -from openpilot.common.swaglog import cloudlog, add_file_handler -from openpilot.system.hardware import HARDWARE, AGNOS -from openpilot.system.version import get_build_metadata - -MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 -CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") - -TOTAL_SCONS_NODES = 2705 -MAX_BUILD_PROGRESS = 100 - -def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: - env = os.environ.copy() - env['SCONS_PROGRESS'] = "1" - nproc = os.cpu_count() - if nproc is None: - nproc = 2 - - extra_args = ["--minimal"] if minimal else [] - - if AGNOS: - HARDWARE.set_power_save(False) - os.sched_setaffinity(0, range(8)) # ensure we can use the isolcpus cores - - # building with all cores can result in using too - # much memory, so retry with less parallelism - compile_output: list[bytes] = [] - for n in (nproc, nproc/2, 1): - compile_output.clear() - scons: subprocess.Popen = subprocess.Popen(["scons", f"-j{int(n)}", "--cache-populate", *extra_args], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) - assert scons.stderr is not None - - # Read progress from stderr and update spinner - while scons.poll() is None: - try: - line = scons.stderr.readline() - if line is None: - continue - line = line.rstrip() - - prefix = b'progress: ' - if line.startswith(prefix): - i = int(line[len(prefix):]) - spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.) - elif len(line): - compile_output.append(line) - print(line.decode('utf8', 'replace')) - except Exception: - pass - - if scons.returncode == 0: - break - - if scons.returncode != 0: - # Read remaining output - if scons.stderr is not None: - compile_output += scons.stderr.read().split(b'\n') - - # Build failed log errors - error_s = b"\n".join(compile_output).decode('utf8', 'replace') - add_file_handler(cloudlog) - cloudlog.error("scons build failed\n" + error_s) - - # Show TextWindow - spinner.close() - if not os.getenv("CI"): - with TextWindow("openpilot failed to build\n \n" + error_s) as t: - t.wait_for_exit() - exit(1) - - # enforce max cache size - cache_files = [f for f in CACHE_DIR.rglob('*') if f.is_file()] - cache_files.sort(key=lambda f: f.stat().st_mtime) - cache_size = sum(f.stat().st_size for f in cache_files) - for f in cache_files: - if cache_size < MAX_CACHE_SIZE: - break - cache_size -= f.stat().st_size - f.unlink() - - -if __name__ == "__main__": - spinner = Spinner() - spinner.update_progress(0, 100) - build_metadata = get_build_metadata() - build(spinner, build_metadata.openpilot.is_dirty, minimal = AGNOS) diff --git a/system/manager/helpers.py b/system/manager/helpers.py deleted file mode 100644 index 047d0ac2d6f881..00000000000000 --- a/system/manager/helpers.py +++ /dev/null @@ -1,67 +0,0 @@ -import errno -import fcntl -import os -import sys -import pathlib -import shutil -import signal -import subprocess -import tempfile -import threading - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params - -def unblock_stdout() -> None: - # get a non-blocking stdout - child_pid, child_pty = os.forkpty() - if child_pid != 0: # parent - - # child is in its own process group, manually pass kill signals - signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT)) - signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM)) - - fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) - - while True: - try: - dat = os.read(child_pty, 4096) - except OSError as e: - if e.errno == errno.EIO: - break - continue - - if not dat: - break - - try: - sys.stdout.write(dat.decode('utf8')) - except (OSError, UnicodeDecodeError): - pass - - # os.wait() returns a tuple with the pid and a 16 bit value - # whose low byte is the signal number and whose high byte is the exit status - exit_status = os.wait()[1] >> 8 - os._exit(exit_status) - - -def write_onroad_params(started, params): - params.put_bool("IsOnroad", started) - params.put_bool("IsOffroad", not started) - - -def save_bootlog(): - # copy current params - tmp = tempfile.mkdtemp() - params_dirname = pathlib.Path(Params().get_param_path()).name - params_dir = os.path.join(tmp, params_dirname) - shutil.copytree(Params().get_param_path(), params_dir, dirs_exist_ok=True) - - def fn(tmpdir): - env = os.environ.copy() - env['PARAMS_COPY_PATH'] = tmpdir - subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"), env=env) - shutil.rmtree(tmpdir) - t = threading.Thread(target=fn, args=(tmp, )) - t.daemon = True - t.start() diff --git a/system/manager/manager.py b/system/manager/manager.py deleted file mode 100755 index 2d80c78ff50072..00000000000000 --- a/system/manager/manager.py +++ /dev/null @@ -1,236 +0,0 @@ -#!/usr/bin/env python3 -import datetime -import os -import signal -import sys -import time -import traceback - -from cereal import log -import cereal.messaging as messaging -import openpilot.system.sentry as sentry -from openpilot.common.utils import atomic_write -from openpilot.common.params import Params, ParamKeyFlag -from openpilot.common.text_window import TextWindow -from openpilot.system.hardware import HARDWARE -from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog -from openpilot.system.manager.process import ensure_running -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.athena.registration import register, UNREGISTERED_DONGLE_ID -from openpilot.common.swaglog import cloudlog, add_file_handler -from openpilot.system.version import get_build_metadata -from openpilot.system.hardware.hw import Paths - - -def manager_init() -> None: - save_bootlog() - - build_metadata = get_build_metadata() - - params = Params() - params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) - params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION) - params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) - params.clear_all(ParamKeyFlag.CLEAR_ON_IGNITION_ON) - if build_metadata.release_channel: - params.clear_all(ParamKeyFlag.DEVELOPMENT_ONLY) - - if params.get_bool("RecordFrontLock"): - params.put_bool("RecordFront", True) - - # set unset params to their default value - for k in params.all_keys(): - default_value = params.get_default_value(k) - if default_value is not None and params.get(k) is None: - params.put(k, default_value) - - # Create folders needed for msgq - try: - os.mkdir(Paths.shm_path()) - except FileExistsError: - pass - except PermissionError: - print(f"WARNING: failed to make {Paths.shm_path()}") - - # set params - serial = HARDWARE.get_serial() - params.put("Version", build_metadata.openpilot.version) - params.put("GitCommit", build_metadata.openpilot.git_commit) - params.put("GitCommitDate", build_metadata.openpilot.git_commit_date) - params.put("GitBranch", build_metadata.channel) - params.put("GitRemote", build_metadata.openpilot.git_origin) - params.put_bool("IsTestedBranch", build_metadata.tested_channel) - params.put_bool("IsReleaseBranch", build_metadata.release_channel) - params.put("HardwareSerial", serial) - - # set dongle id - reg_res = register(show_spinner=True) - if reg_res: - dongle_id = reg_res - else: - raise Exception(f"Registration failed for device {serial}") - os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog - os.environ['GIT_ORIGIN'] = build_metadata.openpilot.git_normalized_origin # Needed for swaglog - os.environ['GIT_BRANCH'] = build_metadata.channel # Needed for swaglog - os.environ['GIT_COMMIT'] = build_metadata.openpilot.git_commit # Needed for swaglog - - if not build_metadata.openpilot.is_dirty: - os.environ['CLEAN'] = '1' - - # init logging - sentry.init(sentry.SentryProject.SELFDRIVE) - cloudlog.bind_global(dongle_id=dongle_id, - version=build_metadata.openpilot.version, - origin=build_metadata.openpilot.git_normalized_origin, - branch=build_metadata.channel, - commit=build_metadata.openpilot.git_commit, - dirty=build_metadata.openpilot.is_dirty, - device=HARDWARE.get_device_type()) - - # preimport all processes - for p in managed_processes.values(): - p.prepare() - - -def manager_cleanup() -> None: - # send signals to kill all procs - for p in managed_processes.values(): - p.stop(block=False) - - # ensure all are killed - for p in managed_processes.values(): - p.stop(block=True) - - cloudlog.info("everything is dead") - - -def manager_thread() -> None: - cloudlog.bind(daemon="manager") - cloudlog.info("manager start") - cloudlog.info({"environ": os.environ}) - - params = Params() - - ignore: list[str] = [] - if params.get("DongleId") in (None, UNREGISTERED_DONGLE_ID): - ignore += ["manage_athenad", "uploader"] - if os.getenv("NOBOARD") is not None: - ignore.append("pandad") - ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] - - sm = messaging.SubMaster(['deviceState', 'carParams', 'pandaStates'], poll='deviceState') - pm = messaging.PubMaster(['managerState']) - - write_onroad_params(False, params) - ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) - - started_prev = False - ignition_prev = False - - while True: - sm.update(1000) - - started = sm['deviceState'].started - - if started and not started_prev: - params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION) - elif not started and started_prev: - params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) - - ignition = any(ps.ignitionLine or ps.ignitionCan for ps in sm['pandaStates'] if ps.pandaType != log.PandaState.PandaType.unknown) - if ignition and not ignition_prev: - params.clear_all(ParamKeyFlag.CLEAR_ON_IGNITION_ON) - - # update onroad params, which drives pandad's safety setter thread - if started != started_prev: - write_onroad_params(started, params) - - started_prev = started - ignition_prev = ignition - - ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) - - running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) - for p in managed_processes.values() if p.proc) - print(running) - cloudlog.debug(running) - - # send managerState - msg = messaging.new_message('managerState', valid=True) - msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] - pm.send('managerState', msg) - - # kick AGNOS power monitoring watchdog - try: - if sm.all_checks(['deviceState']): - with atomic_write("/var/tmp/power_watchdog", "w", overwrite=True) as f: - f.write(str(time.monotonic())) - except Exception: - pass - - # Exit main loop when uninstall/shutdown/reboot is needed - shutdown = False - for param in ("DoUninstall", "DoShutdown", "DoReboot"): - if params.get_bool(param): - shutdown = True - params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}") - cloudlog.warning(f"Shutting down manager - {param} set") - - if shutdown: - break - - -def main() -> None: - manager_init() - if os.getenv("PREPAREONLY") is not None: - return - - # SystemExit on sigterm - signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) - - try: - manager_thread() - except Exception: - traceback.print_exc() - sentry.capture_exception() - finally: - manager_cleanup() - - params = Params() - if params.get_bool("DoUninstall"): - cloudlog.warning("uninstalling") - HARDWARE.uninstall() - elif params.get_bool("DoReboot"): - cloudlog.warning("reboot") - HARDWARE.reboot() - elif params.get_bool("DoShutdown"): - cloudlog.warning("shutdown") - HARDWARE.shutdown() - - -if __name__ == "__main__": - unblock_stdout() - - try: - main() - except KeyboardInterrupt: - print("got CTRL-C, exiting") - except Exception: - add_file_handler(cloudlog) - cloudlog.exception("Manager failed to start") - - try: - managed_processes['ui'].stop() - except Exception: - pass - - # Show last 3 lines of traceback - error = traceback.format_exc(-3) - error = "Manager failed to start\n\n" + error - with TextWindow(error) as t: - t.wait_for_exit() - - raise - - # manual exit because we are forked - sys.exit(0) diff --git a/system/manager/process.py b/system/manager/process.py deleted file mode 100644 index 36e1ba77b286dd..00000000000000 --- a/system/manager/process.py +++ /dev/null @@ -1,267 +0,0 @@ -import importlib -import os -import signal -import time -import subprocess -from collections.abc import Callable, ValuesView -from abc import ABC, abstractmethod -from multiprocessing import Process - -from setproctitle import setproctitle - -from cereal import car, log -import cereal.messaging as messaging -import openpilot.system.sentry as sentry -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog - - -def launcher(proc: str, name: str) -> None: - try: - # import the process - mod = importlib.import_module(proc) - - # rename the process - setproctitle(proc) - - # create new context since we forked - messaging.reset_context() - - # add daemon name tag to logs - cloudlog.bind(daemon=name) - sentry.set_tag("daemon", name) - - # exec the process - mod.main() - except KeyboardInterrupt: - cloudlog.warning(f"child {proc} got SIGINT") - except Exception: - # can't install the crash handler because sys.excepthook doesn't play nice - # with threads, so catch it here. - sentry.capture_exception() - raise - - -def nativelauncher(pargs: list[str], cwd: str, name: str) -> None: - os.environ['MANAGER_DAEMON'] = name - - # exec the process - os.chdir(cwd) - os.execvp(pargs[0], pargs) - - -def join_process(process: Process, timeout: float) -> None: - # Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382 - # We have to poll the exitcode instead - t = time.monotonic() - while time.monotonic() - t < timeout and process.exitcode is None: - time.sleep(0.001) - - -class ManagerProcess(ABC): - daemon = False - sigkill = False - should_run: Callable[[bool, Params, car.CarParams], bool] - proc: Process | None = None - enabled = True - name = "" - shutting_down = False - restart_if_crash = False - - @abstractmethod - def prepare(self) -> None: - pass - - @abstractmethod - def start(self) -> None: - pass - - def restart(self) -> None: - self.stop(sig=signal.SIGKILL) - self.start() - - def stop(self, retry: bool = True, block: bool = True, sig: signal.Signals | None = None) -> int | None: - if self.proc is None: - return None - - if self.proc.exitcode is None: - if not self.shutting_down: - cloudlog.info(f"killing {self.name}") - if sig is None: - sig = signal.SIGKILL if self.sigkill else signal.SIGINT - self.signal(sig) - self.shutting_down = True - - if not block: - return None - - join_process(self.proc, 5) - - # If process failed to die send SIGKILL - if self.proc.exitcode is None and retry: - cloudlog.info(f"killing {self.name} with SIGKILL") - self.signal(signal.SIGKILL) - self.proc.join() - - ret = self.proc.exitcode - cloudlog.info(f"{self.name} is dead with {ret}") - - if self.proc.exitcode is not None: - self.shutting_down = False - self.proc = None - - return ret - - def signal(self, sig: int) -> None: - if self.proc is None: - return - - # Don't signal if already exited - if self.proc.exitcode is not None and self.proc.pid is not None: - return - - # Can't signal if we don't have a pid - if self.proc.pid is None: - return - - cloudlog.info(f"sending signal {sig} to {self.name}") - os.kill(self.proc.pid, sig) - - def get_process_state_msg(self): - state = log.ManagerState.ProcessState.new_message() - state.name = self.name - if self.proc: - state.running = self.proc.is_alive() - state.shouldBeRunning = self.proc is not None and not self.shutting_down - state.pid = self.proc.pid or 0 - state.exitCode = self.proc.exitcode or 0 - return state - - -class NativeProcess(ManagerProcess): - def __init__(self, name, cwd, cmdline, should_run, enabled=True, sigkill=False): - self.name = name - self.cwd = cwd - self.cmdline = cmdline - self.should_run = should_run - self.enabled = enabled - self.sigkill = sigkill - self.launcher = nativelauncher - - def prepare(self) -> None: - pass - - def start(self) -> None: - # In case we only tried a non blocking stop we need to stop it before restarting - if self.shutting_down: - self.stop() - - if self.proc is not None: - return - - cwd = os.path.join(BASEDIR, self.cwd) - cloudlog.info(f"starting process {self.name}") - self.proc = Process(name=self.name, target=self.launcher, args=(self.cmdline, cwd, self.name)) - self.proc.start() - self.shutting_down = False - - -class PythonProcess(ManagerProcess): - def __init__(self, name, module, should_run, enabled=True, sigkill=False, restart_if_crash=False): - self.name = name - self.module = module - self.should_run = should_run - self.enabled = enabled - self.sigkill = sigkill - self.launcher = launcher - self.restart_if_crash = restart_if_crash - - def prepare(self) -> None: - if self.enabled: - cloudlog.info(f"preimporting {self.module}") - importlib.import_module(self.module) - - def start(self) -> None: - # In case we only tried a non blocking stop we need to stop it before restarting - if self.shutting_down: - self.stop() - - if self.proc is not None: - return - - # TODO: this is just a workaround for this tinygrad check: - # https://github.com/tinygrad/tinygrad/blob/ac9c96dae1656dc220ee4acc39cef4dd449aa850/tinygrad/device.py#L26 - name = self.name if "modeld" not in self.name else "MainProcess" - - cloudlog.info(f"starting python {self.module}") - self.proc = Process(name=name, target=self.launcher, args=(self.module, self.name)) - self.proc.start() - self.shutting_down = False - - -class DaemonProcess(ManagerProcess): - """Python process that has to stay running across manager restart. - This is used for athena so you don't lose SSH access when restarting manager.""" - def __init__(self, name, module, param_name, enabled=True): - self.name = name - self.module = module - self.param_name = param_name - self.enabled = enabled - self.params = None - - @staticmethod - def should_run(started, params, CP): - return True - - def prepare(self) -> None: - pass - - def start(self) -> None: - if self.params is None: - self.params = Params() - - pid = self.params.get(self.param_name) - if pid is not None: - try: - os.kill(int(pid), 0) - with open(f'/proc/{pid}/cmdline') as f: - if self.module in f.read(): - # daemon is running - return - except (OSError, FileNotFoundError): - # process is dead - pass - - cloudlog.info(f"starting daemon {self.name}") - proc = subprocess.Popen(['python', '-m', self.module], - stdin=open('/dev/null'), - stdout=open('/dev/null', 'w'), - stderr=open('/dev/null', 'w'), - preexec_fn=os.setpgrp) - - self.params.put(self.param_name, proc.pid) - - def stop(self, retry=True, block=True, sig=None) -> None: - pass - - -def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, - not_run: list[str] | None=None) -> list[ManagerProcess]: - if not_run is None: - not_run = [] - - running = [] - for p in procs: - if p.enabled and p.name not in not_run and p.should_run(started, params, CP): - if p.restart_if_crash and p.proc is not None and not p.proc.is_alive(): - cloudlog.error(f'Restarting {p.name} (exitcode {p.proc.exitcode})') - p.restart() - running.append(p) - else: - p.stop(block=False) - - for p in running: - p.start() - - return running diff --git a/system/manager/process_config.py b/system/manager/process_config.py deleted file mode 100644 index 0b99183193e6e4..00000000000000 --- a/system/manager/process_config.py +++ /dev/null @@ -1,118 +0,0 @@ -import os -import operator -import platform - -from cereal import car -from openpilot.common.params import Params -from openpilot.system.hardware import PC, TICI -from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess - -WEBCAM = os.getenv("USE_WEBCAM") is not None - -def driverview(started: bool, params: Params, CP: car.CarParams) -> bool: - return started or params.get_bool("IsDriverViewEnabled") - -def notcar(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and CP.notCar - -def iscar(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and not CP.notCar - -def logging(started: bool, params: Params, CP: car.CarParams) -> bool: - run = (not CP.notCar) or not params.get_bool("DisableLogging") - return started and run - -def ublox_available() -> bool: - return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') - -def ublox(started: bool, params: Params, CP: car.CarParams) -> bool: - use_ublox = ublox_available() - if use_ublox != params.get_bool("UbloxAvailable"): - params.put_bool("UbloxAvailable", use_ublox) - return started and use_ublox - -def joystick(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and params.get_bool("JoystickDebugMode") - -def not_joystick(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and not params.get_bool("JoystickDebugMode") - -def long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and params.get_bool("LongitudinalManeuverMode") - -def not_long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and not params.get_bool("LongitudinalManeuverMode") - -def qcomgps(started: bool, params: Params, CP: car.CarParams) -> bool: - return started and not ublox_available() - -def always_run(started: bool, params: Params, CP: car.CarParams) -> bool: - return True - -def only_onroad(started: bool, params: Params, CP: car.CarParams) -> bool: - return started - -def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool: - return not started - -def or_(*fns): - return lambda *args: operator.or_(*(fn(*args) for fn in fns)) - -def and_(*fns): - return lambda *args: operator.and_(*(fn(*args) for fn in fns)) - -procs = [ - DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), - - NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging), - NativeProcess("encoderd", "system/loggerd", ["./encoderd"], only_onroad), - NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar), - PythonProcess("logmessaged", "system.logmessaged", always_run), - - NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM), - PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM), - 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), - - PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad), - PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)), - - PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC), - PythonProcess("ui", "selfdrive.ui.ui", always_run, restart_if_crash=True), - PythonProcess("soundd", "selfdrive.ui.soundd", driverview), - PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), - NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), - PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), - PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), - PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)), - PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)), - PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), - PythonProcess("card", "selfdrive.car.card", only_onroad), - PythonProcess("deleter", "system.loggerd.deleter", always_run), - PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(WEBCAM or not PC)), - PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), - PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), - PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), - PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), - 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), - PythonProcess("radard", "selfdrive.controls.radard", only_onroad), - 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("statsd", "system.statsd", always_run), - PythonProcess("feedbackd", "selfdrive.ui.feedback.feedbackd", only_onroad), - - # debug procs - NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), - PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), - PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), - PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)), -] - -managed_processes = {p.name: p for p in procs} diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py deleted file mode 100644 index 34d07c6724cf7b..00000000000000 --- a/system/manager/test/test_manager.py +++ /dev/null @@ -1,81 +0,0 @@ -import os -import pytest -import signal -import time - -from cereal import car -from openpilot.common.params import Params -import openpilot.system.manager.manager as manager -from openpilot.system.manager.process import ensure_running -from openpilot.system.manager.process_config import managed_processes, procs -from openpilot.system.hardware import HARDWARE - -os.environ['FAKEUPLOAD'] = "1" - -MAX_STARTUP_TIME = 3 -BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] - - -class TestManager: - def setup_method(self): - HARDWARE.set_power_save(False) - - # ensure clean CarParams - params = Params() - params.clear_all() - - def teardown_method(self): - manager.manager_cleanup() - - def test_manager_prepare(self): - os.environ['PREPAREONLY'] = '1' - manager.main() - - def test_duplicate_procs(self): - assert len(procs) == len(managed_processes), "Duplicate process names" - - def test_blacklisted_procs(self): - # TODO: ensure there are blacklisted procs until we have a dedicated test - assert len(BLACKLIST_PROCS), "No blacklisted procs to test not_run" - - def test_set_params_with_default_value(self): - params = Params() - params.clear_all() - - os.environ['PREPAREONLY'] = '1' - manager.main() - for k in params.all_keys(): - default_value = params.get_default_value(k) - if default_value is not None: - assert params.get(k) == default_value - assert params.get("OpenpilotEnabledToggle") - assert params.get("RouteCount") == 0 - - @pytest.mark.skip("this test is flaky the way it's currently written, should be moved to test_onroad") - def test_clean_exit(self, subtests): - """ - Ensure all processes exit cleanly when stopped. - """ - HARDWARE.set_power_save(False) - manager.manager_init() - - CP = car.CarParams.new_message() - procs = ensure_running(managed_processes.values(), True, Params(), CP, not_run=BLACKLIST_PROCS) - - time.sleep(10) - - for p in procs: - with subtests.test(proc=p.name): - state = p.get_process_state_msg() - assert state.running, f"{p.name} not running" - exit_code = p.stop(retry=False) - - assert p.name not in BLACKLIST_PROCS, f"{p.name} was started" - - assert exit_code is not None, f"{p.name} failed to exit" - - # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code - exit_codes = [0, 1] - if p.sigkill: - exit_codes = [-signal.SIGKILL] - assert exit_code in exit_codes, f"{p.name} died with {exit_code}" diff --git a/system/micd.py b/system/micd.py deleted file mode 100755 index 9b3ccc8d29a290..00000000000000 --- a/system/micd.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np -from functools import cache -import threading - -from cereal import messaging -from openpilot.common.realtime import Ratekeeper -from openpilot.common.utils import retry -from openpilot.common.swaglog import cloudlog - -RATE = 10 -FFT_SAMPLES = 1600 # 100ms -REFERENCE_SPL = 2e-5 # newtons/m^2 -SAMPLE_RATE = 16000 -SAMPLE_BUFFER = 800 # 50ms - - -@cache -def get_a_weighting_filter(): - # Calculate the A-weighting filter - # https://en.wikipedia.org/wiki/A-weighting - freqs = np.fft.fftfreq(FFT_SAMPLES, d=1 / SAMPLE_RATE) - A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2))) - return A / np.max(A) - - -def calculate_spl(measurements): - # https://www.engineeringtoolbox.com/sound-pressure-d_711.html - sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes - if sound_pressure > 0: - sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB - else: - sound_pressure_level = 0 - return sound_pressure, sound_pressure_level - - -def apply_a_weighting(measurements: np.ndarray) -> np.ndarray: - # Generate a Hanning window of the same length as the audio measurements - measurements_windowed = measurements * np.hanning(len(measurements)) - - # Apply the A-weighting filter to the signal - return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * get_a_weighting_filter())) - - -class Mic: - def __init__(self): - self.rk = Ratekeeper(RATE) - self.pm = messaging.PubMaster(['soundPressure', 'rawAudioData']) - - self.measurements = np.empty(0) - - self.sound_pressure = 0 - self.sound_pressure_weighted = 0 - self.sound_pressure_level_weighted = 0 - - self.lock = threading.Lock() - - def update(self): - with self.lock: - sound_pressure = self.sound_pressure - sound_pressure_weighted = self.sound_pressure_weighted - sound_pressure_level_weighted = self.sound_pressure_level_weighted - - msg = messaging.new_message('soundPressure', valid=True) - msg.soundPressure.soundPressure = float(sound_pressure) - msg.soundPressure.soundPressureWeighted = float(sound_pressure_weighted) - msg.soundPressure.soundPressureWeightedDb = float(sound_pressure_level_weighted) - - self.pm.send('soundPressure', msg) - self.rk.keep_time() - - def callback(self, indata, frames, time, status): - """ - Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level. - Then apply A-weighting to the raw amplitudes and run the same calculations again. - - Logged A-weighted equivalents are rough approximations of the human-perceived loudness. - """ - msg = messaging.new_message('rawAudioData', valid=True) - audio_data_int_16 = (indata[:, 0] * 32767).astype(np.int16) - msg.rawAudioData.data = audio_data_int_16.tobytes() - msg.rawAudioData.sampleRate = SAMPLE_RATE - self.pm.send('rawAudioData', msg) - - with self.lock: - self.measurements = np.concatenate((self.measurements, indata[:, 0])) - - while self.measurements.size >= FFT_SAMPLES: - measurements = self.measurements[:FFT_SAMPLES] - - self.sound_pressure, _ = calculate_spl(measurements) - measurements_weighted = apply_a_weighting(measurements) - self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted) - - self.measurements = self.measurements[FFT_SAMPLES:] - - @retry(attempts=10, delay=3) - def get_stream(self, sd): - # reload sounddevice to reinitialize portaudio - sd._terminate() - sd._initialize() - return sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER) - - def micd_thread(self): - # sounddevice must be imported after forking processes - import sounddevice as sd - - with self.get_stream(sd) as stream: - cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}") - while True: - self.update() - - -def main(): - mic = Mic() - mic.micd_thread() - - -if __name__ == "__main__": - main() diff --git a/system/proclogd.py b/system/proclogd.py deleted file mode 100755 index 3279425b7b3c7f..00000000000000 --- a/system/proclogd.py +++ /dev/null @@ -1,227 +0,0 @@ -#!/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 new file mode 100644 index 00000000000000..1b94a32f1b3634 --- /dev/null +++ b/system/proclogd/SConscript @@ -0,0 +1,6 @@ +Import('env', 'cereal', 'messaging', 'common') +libs = [cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common', 'zmq', 'json11'] +env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs) + +if GetOption('test'): + 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 new file mode 100644 index 00000000000000..c4faa916d93c24 --- /dev/null +++ b/system/proclogd/main.cc @@ -0,0 +1,22 @@ + +#include + +#include "common/util.h" +#include "system/proclogd/proclog.h" + +ExitHandler do_exit; + +int main(int argc, char **argv) { + setpriority(PRIO_PROCESS, 0, -15); + + PubMaster publisher({"procLog"}); + while (!do_exit) { + MessageBuilder msg; + buildProcLogMessage(msg); + publisher.send("procLog", msg); + + util::sleep_for(2000); // 2 secs + } + + return 0; +} diff --git a/system/proclogd/proclog.cc b/system/proclogd/proclog.cc new file mode 100644 index 00000000000000..cbe3b53493ca54 --- /dev/null +++ b/system/proclogd/proclog.cc @@ -0,0 +1,239 @@ +#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 = stoul(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 new file mode 100644 index 00000000000000..9ed53d1bac7686 --- /dev/null +++ b/system/proclogd/proclog.h @@ -0,0 +1,40 @@ +#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; + unsigned long utime, stime, vms, rss; + 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 new file mode 100644 index 00000000000000..5230b1598dfbd2 --- /dev/null +++ b/system/proclogd/tests/.gitignore @@ -0,0 +1 @@ +test_proclog diff --git a/system/proclogd/tests/test_proclog.cc b/system/proclogd/tests/test_proclog.cc new file mode 100644 index 00000000000000..230e855acb0814 --- /dev/null +++ b/system/proclogd/tests/test_proclog.cc @@ -0,0 +1,145 @@ +#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); + int parsed_cnt = 0; + for (int pid : pids) { + if (auto stat = Parser::procStat(util::read_file("/proc/" + std::to_string(pid) + "/stat"))) { + REQUIRE(stat->pid == pid); + REQUIRE(allowed_states.find(stat->state) != std::string::npos); + ++parsed_cnt; + } + } + REQUIRE(parsed_cnt == pids.size()); + } +} + +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("buildProcLogerMessage") { + std::vector current_pids = Parser::pids(); + + 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(); + REQUIRE(procs.size() == current_pids.size()); + + for (auto p : procs) { + REQUIRE_THAT(current_pids, Catch::Matchers::VectorContains(p.getPid())); + 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")); + } + } +} diff --git a/system/qcomgpsd/nmeaport.py b/system/qcomgpsd/nmeaport.py deleted file mode 100644 index 10b8516ed09d7e..00000000000000 --- a/system/qcomgpsd/nmeaport.py +++ /dev/null @@ -1,169 +0,0 @@ -import os -import sys -from dataclasses import dataclass, fields -from subprocess import check_output, CalledProcessError -from time import sleep -from typing import NoReturn - -DEBUG = int(os.environ.get("DEBUG", "0")) - -@dataclass -class GnssClockNmeaPort: - # flags bit mask: - # 0x01 = leap_seconds valid - # 0x02 = time_uncertainty_ns valid - # 0x04 = full_bias_ns valid - # 0x08 = bias_ns valid - # 0x10 = bias_uncertainty_ns valid - # 0x20 = drift_nsps valid - # 0x40 = drift_uncertainty_nsps valid - flags: int - leap_seconds: int - time_ns: int - time_uncertainty_ns: int # 1-sigma - full_bias_ns: int - bias_ns: float - bias_uncertainty_ns: float # 1-sigma - drift_nsps: float - drift_uncertainty_nsps: float # 1-sigma - - def __post_init__(self): - for field in fields(self): - val = getattr(self, field.name) - setattr(self, field.name, field.type(val) if val else None) - -@dataclass -class GnssMeasNmeaPort: - messageCount: int - messageNum: int - svCount: int - # constellation enum: - # 1 = GPS - # 2 = SBAS - # 3 = GLONASS - # 4 = QZSS - # 5 = BEIDOU - # 6 = GALILEO - constellation: int - svId: int - flags: int # always zero - time_offset_ns: int - # state bit mask: - # 0x0001 = CODE LOCK - # 0x0002 = BIT SYNC - # 0x0004 = SUBFRAME SYNC - # 0x0008 = TIME OF WEEK DECODED - # 0x0010 = MSEC AMBIGUOUS - # 0x0020 = SYMBOL SYNC - # 0x0040 = GLONASS STRING SYNC - # 0x0080 = GLONASS TIME OF DAY DECODED - # 0x0100 = BEIDOU D2 BIT SYNC - # 0x0200 = BEIDOU D2 SUBFRAME SYNC - # 0x0400 = GALILEO E1BC CODE LOCK - # 0x0800 = GALILEO E1C 2ND CODE LOCK - # 0x1000 = GALILEO E1B PAGE SYNC - # 0x2000 = GALILEO E1B PAGE SYNC - state: int - time_of_week_ns: int - time_of_week_uncertainty_ns: int # 1-sigma - carrier_to_noise_ratio: float - pseudorange_rate: float - pseudorange_rate_uncertainty: float # 1-sigma - - def __post_init__(self): - for field in fields(self): - val = getattr(self, field.name) - setattr(self, field.name, field.type(val) if val else None) - -def nmea_checksum_ok(s): - checksum = 0 - for i, c in enumerate(s[1:]): - if c == "*": - if i != len(s) - 4: # should be 3rd to last character - print("ERROR: NMEA string does not have checksum delimiter in correct location:", s) - return False - break - checksum ^= ord(c) - else: - print("ERROR: NMEA string does not have checksum delimiter:", s) - return False - - return True - -def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn: - while True: - try: - with open(device) as nmeaport: - for line in nmeaport: - line = line.strip() - if DEBUG: - print(line) - if not line.startswith("$"): # all NMEA messages start with $ - continue - if not nmea_checksum_ok(line): - continue - - fields = line.split(",") - match fields[0]: - case "$GNCLK": - # fields at end are reserved (not used) - gnss_clock = GnssClockNmeaPort(*fields[1:10]) - print(gnss_clock) - case "$GNMEAS": - # fields at end are reserved (not used) - gnss_meas = GnssMeasNmeaPort(*fields[1:14]) - print(gnss_meas) - except Exception as e: - print(e) - sleep(1) - -def main() -> NoReturn: - from openpilot.common.gpio import gpio_init, gpio_set - from openpilot.system.hardware.tici.pins import GPIO - from openpilot.system.qcomgpsd.qcomgpsd import at_cmd - - try: - check_output(["pidof", "qcomgpsd"]) - print("qcomgpsd is running, please kill openpilot before running this script! (aborted)") - sys.exit(1) - except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (pandad not running) - raise e - - print("power up antenna ...") - gpio_init(GPIO.GNSS_PWR_EN, True) - gpio_set(GPIO.GNSS_PWR_EN, True) - - if b"+QGPS: 0" not in (at_cmd("AT+QGPS?") or b""): - print("stop location tracking ...") - at_cmd("AT+QGPSEND") - - if b'+QGPSCFG: "outport",usbnmea' not in (at_cmd('AT+QGPSCFG="outport"') or b""): - print("configure outport ...") - at_cmd('AT+QGPSCFG="outport","usbnmea"') # usbnmea = /dev/ttyUSB1 - - if b'+QGPSCFG: "gnssrawdata",3,0' not in (at_cmd('AT+QGPSCFG="gnssrawdata"') or b""): - print("configure gnssrawdata ...") - # AT+QGPSCFG="gnssrawdata",,' - # values: - # 0x01 = GPS - # 0x02 = GLONASS - # 0x04 = BEIDOU - # 0x08 = GALILEO - # 0x10 = QZSS - # values: - # 0 = NMEA port - # 1 = AT port - at_cmd('AT+QGPSCFG="gnssrawdata",3,0') # enable all constellations, output data to NMEA port - print("rebooting ...") - at_cmd('AT+CFUN=1,1') - print("re-run this script when it is back up") - sys.exit(2) - - print("starting location tracking ...") - at_cmd("AT+QGPS=1") - - process_nmea_port_messages() - -if __name__ == "__main__": - main() diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py deleted file mode 100755 index 59f5ac0b506113..00000000000000 --- a/system/qcomgpsd/qcomgpsd.py +++ /dev/null @@ -1,463 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import signal -import itertools -import math -import time -import requests -import shutil -import subprocess -import datetime -from multiprocessing import Process, Event -from typing import NoReturn -from struct import unpack_from, calcsize, pack - -from cereal import log -import cereal.messaging as messaging -from openpilot.common.gpio import gpio_init, gpio_set -from openpilot.common.utils import retry -from openpilot.common.time_helpers import system_time_valid -from openpilot.system.hardware.tici.pins import GPIO -from openpilot.common.swaglog import cloudlog -from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv -from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist, - gps_measurement_report, gps_measurement_report_sv, - glonass_measurement_report, glonass_measurement_report_sv, - oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report, - LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, - LOG_GNSS_OEMDRE_SVPOLY_REPORT) - -DEBUG = int(os.getenv("DEBUG", "0"))==1 -ASSIST_DATA_FILE = '/tmp/xtra3grc.bin' -ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download' -ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin' - -LOG_TYPES = [ - LOG_GNSS_GPS_MEASUREMENT_REPORT, - LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, - LOG_GNSS_POSITION_REPORT, - LOG_GNSS_OEMDRE_SVPOLY_REPORT, -] - - -miscStatusFields = { - "multipathEstimateIsValid": 0, - "directionIsValid": 1, -} - -measurementStatusFields = { - "subMillisecondIsValid": 0, - "subBitTimeIsKnown": 1, - "satelliteTimeIsKnown": 2, - "bitEdgeConfirmedFromSignal": 3, - "measuredVelocity": 4, - "fineOrCoarseVelocity": 5, - "lockPointValid": 6, - "lockPointPositive": 7, - - "lastUpdateFromDifference": 9, - "lastUpdateFromVelocityDifference": 10, - "strongIndicationOfCrossCorelation": 11, - "tentativeMeasurement": 12, - "measurementNotUsable": 13, - "sirCheckIsNeeded": 14, - "probationMode": 15, - - "multipathIndicator": 24, - "imdJammingIndicator": 25, - "lteB13TxJammingIndicator": 26, - "freshMeasurementIndicator": 27, -} - -measurementStatusGPSFields = { - "gpsRoundRobinRxDiversity": 18, - "gpsRxDiversity": 19, - "gpsLowBandwidthRxDiversityCombined": 20, - "gpsHighBandwidthNu4": 21, - "gpsHighBandwidthNu8": 22, - "gpsHighBandwidthUniform": 23, -} - -measurementStatusGlonassFields = { - "glonassMeanderBitEdgeValid": 16, - "glonassTimeMarkValid": 17 -} - -@retry(attempts=10, delay=1.0) -def try_setup_logs(diag, logs): - return setup_logs(diag, logs) - -@retry(attempts=3, delay=1.0) -def at_cmd(cmd: str) -> str | None: - return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8') - -def gps_enabled() -> bool: - return "QGPS: 1" in at_cmd("AT+QGPS?") - -def download_assistance(): - try: - response = requests.get(ASSISTANCE_URL, timeout=5, stream=True) - - with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp: - for chunk in response.iter_content(chunk_size=8192): - fp.write(chunk) - if fp.tell() > 1e5: - cloudlog.error("Qcom assistance data larger than expected") - return - - os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE) - - except requests.exceptions.RequestException: - cloudlog.exception("Failed to download assistance file") - return - -def downloader_loop(event): - if os.path.exists(ASSIST_DATA_FILE): - os.remove(ASSIST_DATA_FILE) - - alt_path = os.getenv("QCOM_ALT_ASSISTANCE_PATH", None) - if alt_path is not None and os.path.exists(alt_path): - shutil.copyfile(alt_path, ASSIST_DATA_FILE) - - try: - while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set(): - download_assistance() - event.wait(timeout=10) - except KeyboardInterrupt: - pass - -@retry(attempts=5, delay=0.2, ignore_failure=True) -def inject_assistance(): - cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}" - subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True) - cloudlog.info("successfully loaded assistance data") - -@retry(attempts=5, delay=1.0) -def setup_quectel(diag: ModemDiag) -> bool: - ret = False - - # enable OEMDRE in the NV - # TODO: it has to reboot for this to take effect - DIAG_NV_READ_F = 38 - DIAG_NV_WRITE_F = 39 - NV_GNSS_OEM_FEATURE_MASK = 7165 - send_recv(diag, DIAG_NV_WRITE_F, pack(' NoReturn: - unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) - unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) - - unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) - unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) - - unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) - unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) - - unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True) - unpack_position, _ = dict_unpacker(position_report) - - unpack_position, _ = dict_unpacker(position_report) - - wait_for_modem() - - stop_download_event = Event() - assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,)) - assist_fetch_proc.start() - def cleanup(sig, frame): - cloudlog.warning("caught sig disabling quectel gps") - - gpio_set(GPIO.GNSS_PWR_EN, False) - try: - teardown_quectel(diag) - cloudlog.warning("quectel cleanup done") - except NameError: - cloudlog.warning('quectel not yet setup') - - stop_download_event.set() - assist_fetch_proc.kill() - assist_fetch_proc.join() - - sys.exit(0) - signal.signal(signal.SIGINT, cleanup) - signal.signal(signal.SIGTERM, cleanup) - - # connect to modem - diag = ModemDiag() - r = setup_quectel(diag) - want_assistance = not r - cloudlog.warning("quectel setup done") - gpio_init(GPIO.GNSS_PWR_EN, True) - gpio_set(GPIO.GNSS_PWR_EN, True) - - pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) - - while 1: - if os.path.exists(ASSIST_DATA_FILE) and want_assistance: - setup_quectel(diag) - want_assistance = False - - opcode, payload = diag.recv() - if opcode != DIAG_LOG_F: - cloudlog.error(f"Unhandled opcode: {opcode}") - continue - - (pending_msgs, log_outer_length), inner_log_packet = unpack_from(' 0: - cloudlog.debug(f"have {pending_msgs} pending messages") - assert log_outer_length == len(inner_log_packet) - - (log_inner_length, log_type, log_time), log_payload = unpack_from(' 6*SECS_IN_DAY: - epoch.week += 1 - elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY: - epoch.week -= 1 - - poly.gpsWeek = epoch.week - poly.gpsTow = epoch.tow - ''' - pm.send('qcomGnss', msg) - - elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: - msg = messaging.new_message('qcomGnss', valid=True) - - gnss = msg.qcomGnss - gnss.logTs = log_time - gnss.init('measurementReport') - report = gnss.measurementReport - - if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT: - dat = unpack_gps_meas(log_payload) - sats = log_payload[size_gps_meas:] - unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv - report.source = 0 # gps - measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items()) - elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT: - dat = unpack_glonass_meas(log_payload) - sats = log_payload[size_glonass_meas:] - unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv - report.source = 1 # glonass - measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items()) - else: - raise RuntimeError(f"invalid log_type: {log_type}") - - for k,v in dat.items(): - if k == "version": - assert v == 0 - elif k == "week": - report.gpsWeek = v - elif k == "svCount": - pass - else: - setattr(report, k, v) - report.init('sv', dat['svCount']) - if dat['svCount'] > 0: - assert len(sats)//dat['svCount'] == size_meas_sv - for i in range(dat['svCount']): - sv = report.sv[i] - sv.init('measurementStatus') - sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)]) - for k,v in sat.items(): - if k == "parityErrorCount": - sv.gpsParityErrorCount = v - elif k == "frequencyIndex": - sv.glonassFrequencyIndex = v - elif k == "hemmingErrorCount": - sv.glonassHemmingErrorCount = v - elif k == "measurementStatus": - for kk,vv in itertools.chain(*measurement_status_fields): - setattr(sv.measurementStatus, kk, bool(v & (1< None: - pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt]) - - # Requesting both edges as the data ready pulse from the lsm6ds sensor is - # very short (75us) and is mostly detected as falling edge instead of rising. - # So if it is detected as rising the following falling edge is skipped. - fd = gpiochip_get_ro_value_fd("sensord", 0, 84) - - # Configure IRQ affinity - irq_path = "/proc/irq/336/smp_affinity_list" - if not os.path.exists(irq_path): - irq_path = "/proc/irq/335/smp_affinity_list" - if os.path.exists(irq_path): - sudo_write('1\n', irq_path) - - offset = time.time_ns() - time.monotonic_ns() - - poller = select.poll() - poller.register(fd, select.POLLIN | select.POLLPRI) - while not event.is_set(): - events = poller.poll(100) - if not events: - cloudlog.error("poll timed out") - continue - if not (events[0][1] & (select.POLLIN | select.POLLPRI)): - cloudlog.error("no poll events set") - continue - - dat = os.read(fd, ctypes.sizeof(gpioevent_data)*16) - evd = gpioevent_data.from_buffer_copy(dat) - - cur_offset = time.time_ns() - time.monotonic_ns() - if abs(cur_offset - offset) > 10 * 1e6: # ms - cloudlog.warning(f"time jumped: {cur_offset} {offset}") - offset = cur_offset - continue - - ts = evd.timestamp - cur_offset - for sensor, service, interrupt in sensors: - if interrupt: - try: - evt = sensor.get_event(ts) - if not sensor.is_data_valid(): - continue - msg = messaging.new_message(service, valid=True) - setattr(msg, service, evt) - pm.send(service, msg) - except Sensor.DataNotReady: - pass - except Exception: - cloudlog.exception(f"Error processing {service}") - - -def polling_loop(sensor: Sensor, service: str, event: threading.Event) -> None: - pm = messaging.PubMaster([service]) - rk = Ratekeeper(SERVICE_LIST[service].frequency, print_delay_threshold=None) - while not event.is_set(): - try: - evt = sensor.get_event() - if not sensor.is_data_valid(): - continue - msg = messaging.new_message(service, valid=True) - setattr(msg, service, evt) - pm.send(service, msg) - except Exception: - cloudlog.exception(f"Error in {service} polling loop") - rk.keep_time() - -def main() -> None: - config_realtime_process([1, ], 1) - - sensors_cfg = [ - (LSM6DS3_Accel(I2C_BUS_IMU), "accelerometer", True), - (LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True), - (LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False), - ] - if HARDWARE.get_device_type() == "tizi": - sensors_cfg.append( - (MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False), - ) - - # Reset sensors - for sensor, _, _ in sensors_cfg: - try: - sensor.reset() - except Exception: - cloudlog.exception(f"Error initializing {sensor} sensor") - - # Initialize sensors - exit_event = threading.Event() - threads = [ - threading.Thread(target=interrupt_loop, args=(sensors_cfg, exit_event), daemon=True) - ] - for sensor, service, interrupt in sensors_cfg: - try: - sensor.init() - if not interrupt: - # Start polling thread for sensors without interrupts - threads.append(threading.Thread( - target=polling_loop, - args=(sensor, service, exit_event), - daemon=True - )) - except Exception: - cloudlog.exception(f"Error initializing {service} sensor") - - try: - for t in threads: - t.start() - while any(t.is_alive() for t in threads): - time.sleep(1) - except KeyboardInterrupt: - pass - finally: - exit_event.set() - for t in threads: - if t.is_alive(): - t.join() - - for sensor, _, _ in sensors_cfg: - try: - sensor.shutdown() - except Exception: - cloudlog.exception("Error shutting down sensor") - -if __name__ == "__main__": - main() diff --git a/system/sensord/sensors/i2c_sensor.py b/system/sensord/sensors/i2c_sensor.py deleted file mode 100644 index 57edcc52d9056f..00000000000000 --- a/system/sensord/sensors/i2c_sensor.py +++ /dev/null @@ -1,78 +0,0 @@ -import time -import ctypes -from collections.abc import Iterable - -from cereal import log -from openpilot.common.i2c import SMBus - - -class Sensor: - class SensorException(Exception): - pass - - class DataNotReady(SensorException): - pass - - def __init__(self, bus: int) -> None: - self.bus = SMBus(bus) - self.source = log.SensorEventData.SensorSource.velodyne # unknown - self.start_ts = 0. - - def __del__(self): - self.bus.close() - - def read(self, addr: int, length: int) -> bytes: - return bytes(self.bus.read_i2c_block_data(self.device_address, addr, length)) - - def write(self, addr: int, data: int) -> None: - self.bus.write_byte_data(self.device_address, addr, data) - - def writes(self, writes: Iterable[tuple[int, int]]) -> None: - for addr, data in writes: - self.write(addr, data) - - def verify_chip_id(self, address: int, expected_ids: list[int]) -> int: - chip_id = self.read(address, 1)[0] - assert chip_id in expected_ids - return chip_id - - # Abstract methods that must be implemented by subclasses - @property - def device_address(self) -> int: - raise NotImplementedError - - def reset(self) -> None: - # optional. - # not part of init due to shared registers - pass - - def init(self) -> None: - raise NotImplementedError - - def get_event(self, ts: int | None = None) -> log.SensorEventData: - raise NotImplementedError - - def shutdown(self) -> None: - raise NotImplementedError - - def is_data_valid(self) -> bool: - if self.start_ts == 0: - self.start_ts = time.monotonic() - - # unclear whether we need this... - return (time.monotonic() - self.start_ts) > 0.5 - - # *** helpers *** - @staticmethod - def wait(): - # a standard small sleep - time.sleep(0.005) - - @staticmethod - def parse_16bit(lsb: int, msb: int) -> int: - return ctypes.c_int16((msb << 8) | lsb).value - - @staticmethod - def parse_20bit(b2: int, b1: int, b0: int) -> int: - combined = ctypes.c_uint32((b0 << 16) | (b1 << 8) | b2).value - return ctypes.c_int32(combined).value // (1 << 4) diff --git a/system/sensord/sensors/lsm6ds3_accel.py b/system/sensord/sensors/lsm6ds3_accel.py deleted file mode 100644 index 43863daa93640a..00000000000000 --- a/system/sensord/sensors/lsm6ds3_accel.py +++ /dev/null @@ -1,161 +0,0 @@ -import os -import time - -from cereal import log -from openpilot.system.sensord.sensors.i2c_sensor import Sensor - -class LSM6DS3_Accel(Sensor): - LSM6DS3_ACCEL_I2C_REG_DRDY_CFG = 0x0B - LSM6DS3_ACCEL_I2C_REG_INT1_CTRL = 0x0D - LSM6DS3_ACCEL_I2C_REG_CTRL1_XL = 0x10 - LSM6DS3_ACCEL_I2C_REG_CTRL3_C = 0x12 - LSM6DS3_ACCEL_I2C_REG_CTRL5_C = 0x14 - LSM6DS3_ACCEL_I2C_REG_STAT_REG = 0x1E - LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL = 0x28 - - LSM6DS3_ACCEL_ODR_104HZ = (0b0100 << 4) - LSM6DS3_ACCEL_INT1_DRDY_XL = 0b1 - LSM6DS3_ACCEL_DRDY_XLDA = 0b1 - LSM6DS3_ACCEL_DRDY_PULSE_MODE = (1 << 7) - LSM6DS3_ACCEL_IF_INC = 0b00000100 - - LSM6DS3_ACCEL_ODR_52HZ = (0b0011 << 4) - LSM6DS3_ACCEL_FS_4G = (0b10 << 2) - LSM6DS3_ACCEL_IF_INC_BDU = 0b01000100 - LSM6DS3_ACCEL_POSITIVE_TEST = 0b01 - LSM6DS3_ACCEL_NEGATIVE_TEST = 0b10 - LSM6DS3_ACCEL_MIN_ST_LIMIT_mg = 90.0 - LSM6DS3_ACCEL_MAX_ST_LIMIT_mg = 1700.0 - - @property - def device_address(self) -> int: - return 0x6A - - def reset(self): - self.write(0x12, 0x1) - time.sleep(0.1) - - def init(self): - chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) - if chip_id == 0x6A: - self.source = log.SensorEventData.SensorSource.lsm6ds3trc - else: - self.source = log.SensorEventData.SensorSource.lsm6ds3 - - # self-test - if os.getenv("LSM_SELF_TEST") == "1": - self.self_test(self.LSM6DS3_ACCEL_POSITIVE_TEST) - self.self_test(self.LSM6DS3_ACCEL_NEGATIVE_TEST) - - # actual init - int1 = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] - int1 |= self.LSM6DS3_ACCEL_INT1_DRDY_XL - self.writes(( - # Enable continuous update and automatic address increment - (self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC), - # Set ODR to 104 Hz, FS to ±2g (default) - (self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, self.LSM6DS3_ACCEL_ODR_104HZ), - # Configure data ready signal to pulse mode - (self.LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, self.LSM6DS3_ACCEL_DRDY_PULSE_MODE), - # Enable data ready interrupt on INT1 without resetting existing interrupts - (self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, int1), - )) - - def get_event(self, ts: int | None = None) -> log.SensorEventData: - assert ts is not None # must come from the IRQ event - - # Check if data is ready since IRQ is shared with gyro - status_reg = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] - if (status_reg & self.LSM6DS3_ACCEL_DRDY_XLDA) == 0: - raise self.DataNotReady - - scale = 9.81 * 2.0 / (1 << 15) - b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) - x = self.parse_16bit(b[0], b[1]) * scale - y = self.parse_16bit(b[2], b[3]) * scale - z = self.parse_16bit(b[4], b[5]) * scale - - event = log.SensorEventData.new_message() - event.timestamp = ts - event.version = 1 - event.sensor = 1 # SENSOR_ACCELEROMETER - event.type = 1 # SENSOR_TYPE_ACCELEROMETER - event.source = self.source - a = event.init('acceleration') - a.v = [y, -x, z] - a.status = 1 - return event - - def shutdown(self) -> None: - # Disable data ready interrupt on INT1 - value = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] - value &= ~self.LSM6DS3_ACCEL_INT1_DRDY_XL - self.write(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value) - - # Power down by clearing ODR bits - value = self.read(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 1)[0] - value &= 0x0F - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value) - - # *** self-test stuff *** - def _wait_for_data_ready(self): - while True: - drdy = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] - if drdy & self.LSM6DS3_ACCEL_DRDY_XLDA: - break - - def _read_and_avg_data(self, scaling: float) -> list[float]: - out_buf = [0.0, 0.0, 0.0] - for _ in range(5): - self._wait_for_data_ready() - b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) - for j in range(3): - val = self.parse_16bit(b[j*2], b[j*2+1]) * scaling - out_buf[j] += val - return [x / 5.0 for x in out_buf] - - def self_test(self, test_type: int) -> None: - # Prepare sensor for self-test - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC_BDU) - - # Configure ODR and full scale based on sensor type - if self.source == log.SensorEventData.SensorSource.lsm6ds3trc: - odr_fs = self.LSM6DS3_ACCEL_FS_4G | self.LSM6DS3_ACCEL_ODR_52HZ - scaling = 0.122 # mg/LSB for ±4g - else: - odr_fs = self.LSM6DS3_ACCEL_ODR_52HZ - scaling = 0.061 # mg/LSB for ±2g - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, odr_fs) - - # Wait for stable output - time.sleep(0.1) - self._wait_for_data_ready() - val_st_off = self._read_and_avg_data(scaling) - - # Enable self-test - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type) - - # Wait for stable output - time.sleep(0.1) - self._wait_for_data_ready() - val_st_on = self._read_and_avg_data(scaling) - - # Disable sensor and self-test - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0) - self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0) - - # Calculate differences and check limits - test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] - for val in test_val: - if val < self.LSM6DS3_ACCEL_MIN_ST_LIMIT_mg or val > self.LSM6DS3_ACCEL_MAX_ST_LIMIT_mg: - raise self.SensorException(f"Accelerometer self-test failed for test type {test_type}") - -if __name__ == "__main__": - import numpy as np - s = LSM6DS3_Accel(1) - s.init() - time.sleep(0.2) - e = s.get_event(0) - print(e) - print(np.linalg.norm(e.acceleration.v)) - s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_gyro.py b/system/sensord/sensors/lsm6ds3_gyro.py deleted file mode 100644 index 60de2bbe02ec1b..00000000000000 --- a/system/sensord/sensors/lsm6ds3_gyro.py +++ /dev/null @@ -1,145 +0,0 @@ -import os -import math -import time - -from cereal import log -from openpilot.system.sensord.sensors.i2c_sensor import Sensor - -class LSM6DS3_Gyro(Sensor): - LSM6DS3_GYRO_I2C_REG_DRDY_CFG = 0x0B - LSM6DS3_GYRO_I2C_REG_INT1_CTRL = 0x0D - LSM6DS3_GYRO_I2C_REG_CTRL2_G = 0x11 - LSM6DS3_GYRO_I2C_REG_CTRL5_C = 0x14 - LSM6DS3_GYRO_I2C_REG_STAT_REG = 0x1E - LSM6DS3_GYRO_I2C_REG_OUTX_L_G = 0x22 - - LSM6DS3_GYRO_ODR_104HZ = (0b0100 << 4) - LSM6DS3_GYRO_INT1_DRDY_G = 0b10 - LSM6DS3_GYRO_DRDY_GDA = 0b10 - LSM6DS3_GYRO_DRDY_PULSE_MODE = (1 << 7) - - LSM6DS3_GYRO_ODR_208HZ = (0b0101 << 4) - LSM6DS3_GYRO_FS_2000dps = (0b11 << 2) - LSM6DS3_GYRO_POSITIVE_TEST = (0b01 << 2) - LSM6DS3_GYRO_NEGATIVE_TEST = (0b11 << 2) - LSM6DS3_GYRO_MIN_ST_LIMIT_mdps = 150000.0 - LSM6DS3_GYRO_MAX_ST_LIMIT_mdps = 700000.0 - - @property - def device_address(self) -> int: - return 0x6A - - def reset(self): - self.write(0x12, 0x1) - time.sleep(0.1) - - def init(self): - chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) - if chip_id == 0x6A: - self.source = log.SensorEventData.SensorSource.lsm6ds3trc - else: - self.source = log.SensorEventData.SensorSource.lsm6ds3 - - # self-test - if "LSM_SELF_TEST" in os.environ: - self.self_test(self.LSM6DS3_GYRO_POSITIVE_TEST) - self.self_test(self.LSM6DS3_GYRO_NEGATIVE_TEST) - - # actual init - self.writes(( - # TODO: set scale. Default is +- 250 deg/s - (self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_104HZ), - # Configure data ready signal to pulse mode - (self.LSM6DS3_GYRO_I2C_REG_DRDY_CFG, self.LSM6DS3_GYRO_DRDY_PULSE_MODE), - )) - value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] - value |= self.LSM6DS3_GYRO_INT1_DRDY_G - self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) - - def get_event(self, ts: int | None = None) -> log.SensorEventData: - assert ts is not None # must come from the IRQ event - - # Check if gyroscope data is ready, since it's shared with accelerometer - status_reg = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] - if not (status_reg & self.LSM6DS3_GYRO_DRDY_GDA): - raise self.DataNotReady - - b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) - x = self.parse_16bit(b[0], b[1]) - y = self.parse_16bit(b[2], b[3]) - z = self.parse_16bit(b[4], b[5]) - scale = (8.75 / 1000.0) * (math.pi / 180.0) - xyz = [y * scale, -x * scale, z * scale] - - event = log.SensorEventData.new_message() - event.timestamp = ts - event.version = 2 - event.sensor = 5 # SENSOR_GYRO_UNCALIBRATED - event.type = 16 # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED - event.source = self.source - g = event.init('gyroUncalibrated') - g.v = xyz - g.status = 1 - return event - - def shutdown(self) -> None: - # Disable data ready interrupt on INT1 - value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] - value &= ~self.LSM6DS3_GYRO_INT1_DRDY_G - self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) - - # Power down by clearing ODR bits - value = self.read(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 1)[0] - value &= 0x0F - self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, value) - - # *** self-test stuff *** - def _wait_for_data_ready(self): - while True: - drdy = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] - if drdy & self.LSM6DS3_GYRO_DRDY_GDA: - break - - def _read_and_avg_data(self) -> list[float]: - out_buf = [0.0, 0.0, 0.0] - for _ in range(5): - self._wait_for_data_ready() - b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) - for j in range(3): - val = self.parse_16bit(b[j*2], b[j*2+1]) * 70.0 # mdps/LSB for 2000 dps - out_buf[j] += val - return [x / 5.0 for x in out_buf] - - def self_test(self, test_type: int): - # Set ODR to 208Hz, FS to 2000dps - self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_208HZ | self.LSM6DS3_GYRO_FS_2000dps) - - # Wait for stable output - time.sleep(0.15) - self._wait_for_data_ready() - val_st_off = self._read_and_avg_data() - - # Enable self-test - self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type) - - # Wait for stable output - time.sleep(0.05) - self._wait_for_data_ready() - val_st_on = self._read_and_avg_data() - - # Disable sensor and self-test - self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0) - self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0) - - # Calculate differences and check limits - test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] - for val in test_val: - if val < self.LSM6DS3_GYRO_MIN_ST_LIMIT_mdps or val > self.LSM6DS3_GYRO_MAX_ST_LIMIT_mdps: - raise Exception(f"Gyroscope self-test failed for test type {test_type}") - -if __name__ == "__main__": - s = LSM6DS3_Gyro(1) - s.init() - time.sleep(0.1) - print(s.get_event(0)) - s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_temp.py b/system/sensord/sensors/lsm6ds3_temp.py deleted file mode 100644 index b9bb9fe3da5fea..00000000000000 --- a/system/sensord/sensors/lsm6ds3_temp.py +++ /dev/null @@ -1,33 +0,0 @@ -import time - -from cereal import log -from openpilot.system.sensord.sensors.i2c_sensor import Sensor - -# https://content.arduino.cc/assets/st_imu_lsm6ds3_datasheet.pdf -class LSM6DS3_Temp(Sensor): - @property - def device_address(self) -> int: - return 0x6A - - def _read_temperature(self) -> float: - scale = 16.0 if self.source == log.SensorEventData.SensorSource.lsm6ds3 else 256.0 - data = self.read(0x20, 2) - return 25 + (self.parse_16bit(data[0], data[1]) / scale) - - def init(self): - chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) - if chip_id == 0x6A: - self.source = log.SensorEventData.SensorSource.lsm6ds3trc - else: - self.source = log.SensorEventData.SensorSource.lsm6ds3 - - def get_event(self, ts: int | None = None) -> log.SensorEventData: - event = log.SensorEventData.new_message() - event.version = 1 - event.timestamp = int(time.monotonic() * 1e9) - event.source = self.source - event.temperature = self._read_temperature() - return event - - def shutdown(self) -> None: - pass diff --git a/system/sensord/sensors/mmc5603nj_magn.py b/system/sensord/sensors/mmc5603nj_magn.py deleted file mode 100644 index 255e99eb3e322f..00000000000000 --- a/system/sensord/sensors/mmc5603nj_magn.py +++ /dev/null @@ -1,76 +0,0 @@ -import time - -from cereal import log -from openpilot.system.sensord.sensors.i2c_sensor import Sensor - -# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf - -# Register addresses -REG_ODR = 0x1A -REG_INTERNAL_0 = 0x1B -REG_INTERNAL_1 = 0x1C - -# Control register settings -CMM_FREQ_EN = (1 << 7) -AUTO_SR_EN = (1 << 5) -SET = (1 << 3) -RESET = (1 << 4) - -class MMC5603NJ_Magn(Sensor): - @property - def device_address(self) -> int: - return 0x30 - - def init(self): - self.verify_chip_id(0x39, [0x10, ]) - self.writes(( - (REG_ODR, 0), - - # Set BW to 0b01 for 1-150 Hz operation - (REG_INTERNAL_1, 0b01), - )) - - def _read_data(self, cycle) -> list[float]: - # start measurement - self.write(REG_INTERNAL_0, cycle) - self.wait() - - # read out XYZ - scale = 1.0 / 16384.0 - b = self.read(0x00, 9) - return [ - (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0, - (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0, - (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0, - ] - - def get_event(self, ts: int | None = None) -> log.SensorEventData: - ts = time.monotonic_ns() - - # SET - RESET cycle - xyz = self._read_data(SET) - reset_xyz = self._read_data(RESET) - vals = [*xyz, *reset_xyz] - - event = log.SensorEventData.new_message() - event.timestamp = ts - event.version = 1 - event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED - event.type = 14 # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED - event.source = log.SensorEventData.SensorSource.mmc5603nj - - m = event.init('magneticUncalibrated') - m.v = vals - m.status = int(all(int(v) != -32 for v in vals)) - - return event - - def shutdown(self) -> None: - v = self.read(REG_INTERNAL_0, 1)[0] - self.writes(( - # disable auto-reset of measurements - (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))), - - # disable continuous mode - (REG_ODR, 0), - )) diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py deleted file mode 100644 index 20214e12ffadbf..00000000000000 --- a/system/sensord/tests/test_sensord.py +++ /dev/null @@ -1,225 +0,0 @@ -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 - -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] = { - "mici": [LSM, LSM_C], - "tizi": [MMC | LSM, MMC | LSM_C], - "tici": [LSM, LSM_C, MMC | LSM, MMC | LSM_C], -}.get(HARDWARE.get_device_type(), []) - -Sensor = log.SensorEventData.SensorSource -SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max']) -ALL_SENSORS = { - Sensor.lsm6ds3trc: { - SensorConfig("acceleration", 5, 15), - SensorConfig("gyroUncalibrated", 0, .2), - SensorConfig("temperature", 10, 40), # set for max range of our office - }, - - Sensor.mmc5603nj: { - SensorConfig("magneticUncalibrated", 0, 300), - } -} -ALL_SENSORS[Sensor.lsm6ds3] = ALL_SENSORS[Sensor.lsm6ds3trc] - -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', 'temperatureSensor',] - 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/system/sentry.py b/system/sentry.py deleted file mode 100644 index 47d64ba0fde75a..00000000000000 --- a/system/sentry.py +++ /dev/null @@ -1,73 +0,0 @@ -"""Install exception handler for process crash.""" -import sentry_sdk -from enum import Enum -from sentry_sdk.integrations.threading import ThreadingIntegration - -from openpilot.common.params import Params -from openpilot.system.athena.registration import is_registered_device -from openpilot.system.hardware import HARDWARE, PC -from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_build_metadata, get_version - - -class SentryProject(Enum): - # python project - SELFDRIVE = "https://6f3c7076c1e14b2aa10f5dde6dda0cc4@o33823.ingest.sentry.io/77924" - # native project - SELFDRIVE_NATIVE = "https://3e4b586ed21a4479ad5d85083b639bc6@o33823.ingest.sentry.io/157615" - - -def report_tombstone(fn: str, message: str, contents: str) -> None: - cloudlog.error({'tombstone': message}) - - with sentry_sdk.configure_scope() as scope: - scope.set_extra("tombstone_fn", fn) - scope.set_extra("tombstone", contents) - sentry_sdk.capture_message(message=message) - sentry_sdk.flush() - - -def capture_exception(*args, **kwargs) -> None: - cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) - - try: - sentry_sdk.capture_exception(*args, **kwargs) - sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 - except Exception: - cloudlog.exception("sentry exception") - - -def set_tag(key: str, value: str) -> None: - sentry_sdk.set_tag(key, value) - - -def init(project: SentryProject) -> bool: - build_metadata = get_build_metadata() - # forks like to mess with this, so double check - comma_remote = build_metadata.openpilot.comma_remote and "commaai" in build_metadata.openpilot.git_origin - if not comma_remote or not is_registered_device() or PC: - return False - - env = "release" if build_metadata.tested_channel else "master" - dongle_id = Params().get("DongleId") - - integrations = [] - if project == SentryProject.SELFDRIVE: - integrations.append(ThreadingIntegration(propagate_hub=True)) - - sentry_sdk.init(project.value, - default_integrations=False, - release=get_version(), - integrations=integrations, - traces_sample_rate=1.0, - max_value_length=8192, - environment=env) - - sentry_sdk.set_user({"id": dongle_id}) - sentry_sdk.set_tag("dirty", build_metadata.openpilot.is_dirty) - sentry_sdk.set_tag("origin", build_metadata.openpilot.git_origin) - sentry_sdk.set_tag("branch", build_metadata.channel) - sentry_sdk.set_tag("commit", build_metadata.openpilot.git_commit) - sentry_sdk.set_tag("device", HARDWARE.get_device_type()) - - return True diff --git a/system/statsd.py b/system/statsd.py deleted file mode 100755 index 33e9e9912d4a1c..00000000000000 --- a/system/statsd.py +++ /dev/null @@ -1,183 +0,0 @@ -#!/usr/bin/env python3 -import os -import zmq -import time -import uuid -from pathlib import Path -from collections import defaultdict -from datetime import datetime, UTC -from typing import NoReturn - -from openpilot.common.params import Params -from cereal.messaging import SubMaster -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import HARDWARE -from openpilot.common.utils import atomic_write -from openpilot.system.version import get_build_metadata -from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S - - -class METRIC_TYPE: - GAUGE = 'g' - SAMPLE = 'sa' - -class StatLog: - def __init__(self): - self.pid = None - self.zctx = None - self.sock = None - - def connect(self) -> None: - self.zctx = zmq.Context() - self.sock = self.zctx.socket(zmq.PUSH) - self.sock.setsockopt(zmq.LINGER, 10) - self.sock.connect(STATS_SOCKET) - self.pid = os.getpid() - - def __del__(self): - if self.sock is not None: - self.sock.close() - if self.zctx is not None: - self.zctx.term() - - def _send(self, metric: str) -> None: - if os.getpid() != self.pid: - self.connect() - - try: - self.sock.send_string(metric, zmq.NOBLOCK) - except zmq.error.Again: - # drop :/ - pass - - def gauge(self, name: str, value: float) -> None: - self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}") - - # Samples will be recorded in a buffer and at aggregation time, - # statistical properties will be logged (mean, count, percentiles, ...) - def sample(self, name: str, value: float): - self._send(f"{name}:{value}|{METRIC_TYPE.SAMPLE}") - - -def main() -> NoReturn: - dongle_id = Params().get("DongleId") - def get_influxdb_line(measurement: str, value: float | dict[str, float], timestamp: datetime, tags: dict) -> str: - res = f"{measurement}" - for k, v in tags.items(): - res += f",{k}={str(v)}" - res += " " - - if isinstance(value, float): - value = {'value': value} - - for k, v in value.items(): - res += f"{k}={v}," - - res += f"dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n" - return res - - # open statistics socket - ctx = zmq.Context.instance() - sock = ctx.socket(zmq.PULL) - sock.bind(STATS_SOCKET) - - STATS_DIR = Paths.stats_root() - - # initialize stats directory - Path(STATS_DIR).mkdir(parents=True, exist_ok=True) - - build_metadata = get_build_metadata() - - # initialize tags - tags = { - 'started': False, - 'version': build_metadata.openpilot.version, - 'branch': build_metadata.channel, - 'dirty': build_metadata.openpilot.is_dirty, - 'origin': build_metadata.openpilot.git_normalized_origin, - 'deviceType': HARDWARE.get_device_type(), - } - - # subscribe to deviceState for started state - sm = SubMaster(['deviceState']) - - idx = 0 - boot_uid = str(uuid.uuid4())[:8] - last_flush_time = time.monotonic() - gauges = {} - samples: dict[str, list[float]] = defaultdict(list) - try: - while True: - started_prev = sm['deviceState'].started - sm.update() - - # Update metrics - while True: - try: - metric = sock.recv_string(zmq.NOBLOCK) - try: - metric_type = metric.split('|')[1] - metric_name = metric.split(':')[0] - metric_value = float(metric.split('|')[0].split(':')[1]) - - if metric_type == METRIC_TYPE.GAUGE: - gauges[metric_name] = metric_value - elif metric_type == METRIC_TYPE.SAMPLE: - samples[metric_name].append(metric_value) - else: - cloudlog.event("unknown metric type", metric_type=metric_type) - except Exception: - cloudlog.event("malformed metric", metric=metric) - except zmq.error.Again: - break - - # flush when started state changes or after FLUSH_TIME_S - if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev): - result = "" - current_time = datetime.now(UTC) - tags['started'] = sm['deviceState'].started - - for key, value in gauges.items(): - result += get_influxdb_line(f"gauge.{key}", value, current_time, tags) - - for key, values in samples.items(): - values.sort() - sample_count = len(values) - sample_sum = sum(values) - - stats = { - 'count': sample_count, - 'min': values[0], - 'max': values[-1], - 'mean': sample_sum / sample_count, - } - for percentile in [0.05, 0.5, 0.95]: - value = values[int(round(percentile * (sample_count - 1)))] - stats[f"p{int(percentile * 100)}"] = value - - result += get_influxdb_line(f"sample.{key}", stats, current_time, tags) - - # clear intermediate data - gauges.clear() - samples.clear() - last_flush_time = time.monotonic() - - # check that we aren't filling up the drive - if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT: - if len(result) > 0: - stats_path = os.path.join(STATS_DIR, f"{boot_uid}_{idx}") - with atomic_write(stats_path) as f: - f.write(result) - idx += 1 - else: - cloudlog.error("stats dir full") - finally: - sock.close() - ctx.term() - - -if __name__ == "__main__": - main() -else: - statlog = StatLog() diff --git a/common/swaglog.py b/system/swaglog.py similarity index 77% rename from common/swaglog.py rename to system/swaglog.py index d009f00e76177d..68664330a56a66 100644 --- a/common/swaglog.py +++ b/system/swaglog.py @@ -1,19 +1,22 @@ import logging import os import time -import warnings from pathlib import Path from logging.handlers import BaseRotatingHandler import zmq -from openpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter -from openpilot.system.hardware.hw import Paths +from common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter +from system.hardware import PC +if PC: + SWAGLOG_DIR = os.path.join(str(Path.home()), ".comma", "log") +else: + SWAGLOG_DIR = "/data/log/" def get_file_handler(): - Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True) - base_filename = os.path.join(Paths.swaglog_root(), "swaglog") + Path(SWAGLOG_DIR).mkdir(parents=True, exist_ok=True) + base_filename = os.path.join(SWAGLOG_DIR, "swaglog") handler = SwaglogRotatingFileHandler(base_filename) return handler @@ -69,29 +72,15 @@ def __init__(self, formatter): self.setFormatter(formatter) self.pid = None - self.zctx = None - self.sock = None - - def __del__(self): - self.close() - - def close(self): - if self.sock is not None: - self.sock.close() - if self.zctx is not None: - self.zctx.term() - def connect(self): self.zctx = zmq.Context() self.sock = self.zctx.socket(zmq.PUSH) self.sock.setsockopt(zmq.LINGER, 10) - self.sock.connect(Paths.swaglog_ipc()) + self.sock.connect("ipc:///tmp/logmessage") self.pid = os.getpid() def emit(self, record): if os.getpid() != self.pid: - # TODO suppresses warning about forking proc with zmq socket, fix root cause - warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*") self.connect() msg = self.format(record).rstrip('\n') @@ -104,15 +93,6 @@ def emit(self, record): pass -class ForwardingHandler(logging.Handler): - def __init__(self, target_logger): - super().__init__() - self.target_logger = target_logger - - def emit(self, record): - self.target_logger.handle(record) - - def add_file_handler(log): """ Function to add the file log handler to swaglog. @@ -137,8 +117,6 @@ def add_file_handler(log): elif print_level == 'warning': outhandler.setLevel(logging.WARNING) -ipchandler = UnixDomainSocketHandler(SwagFormatter(log)) - log.addHandler(outhandler) # logs are sent through IPC before writing to disk to prevent disk I/O blocking -log.addHandler(ipchandler) +log.addHandler(UnixDomainSocketHandler(SwagFormatter(log))) diff --git a/system/tests/test_logmessaged.py b/system/tests/test_logmessaged.py deleted file mode 100644 index 9ccc8ef53bc25c..00000000000000 --- a/system/tests/test_logmessaged.py +++ /dev/null @@ -1,55 +0,0 @@ -import glob -import os -import time - -import cereal.messaging as messaging -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import cloudlog, ipchandler - - -class TestLogmessaged: - def setup_method(self): - # clear the IPC buffer in case some other tests used cloudlog and filled it - ipchandler.close() - ipchandler.connect() - - managed_processes['logmessaged'].start() - self.sock = messaging.sub_sock("logMessage", timeout=1000, conflate=False) - self.error_sock = messaging.sub_sock("logMessage", timeout=1000, conflate=False) - - # ensure sockets are connected - time.sleep(0.5) - messaging.drain_sock(self.sock) - messaging.drain_sock(self.error_sock) - - def teardown_method(self): - del self.sock - del self.error_sock - managed_processes['logmessaged'].stop(block=True) - - def _get_log_files(self): - return list(glob.glob(os.path.join(Paths.swaglog_root(), "swaglog.*"))) - - def test_simple_log(self): - msgs = [f"abc {i}" for i in range(10)] - for m in msgs: - cloudlog.error(m) - time.sleep(0.5) - m = messaging.drain_sock(self.sock) - assert len(m) == len(msgs) - assert len(self._get_log_files()) >= 1 - - def test_big_log(self): - n = 10 - msg = "a"*3*1024*1024 - for _ in range(n): - cloudlog.info(msg) - time.sleep(0.5) - - msgs = messaging.drain_sock(self.sock) - assert len(msgs) == 0 - - logsize = sum([os.path.getsize(f) for f in self._get_log_files()]) - assert (n*len(msg)) < logsize < (n*(len(msg)+1024)) - diff --git a/system/timed.py b/system/timed.py deleted file mode 100755 index b7131b04c070b7..00000000000000 --- a/system/timed.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python3 -import datetime -import subprocess -import time -from typing import NoReturn - -import cereal.messaging as messaging -from openpilot.common.time_helpers import min_date, system_time_valid -from openpilot.common.swaglog import cloudlog -from openpilot.common.params import Params -from openpilot.common.gps import get_gps_location_service - - -def set_time(new_time): - diff = datetime.datetime.now() - new_time - if abs(diff) < datetime.timedelta(seconds=10): - cloudlog.debug(f"Time diff too small: {diff}") - return - - cloudlog.debug(f"Setting time to {new_time}") - try: - subprocess.run(f"TZ=UTC date -s '{new_time}'", shell=True, check=True) - except subprocess.CalledProcessError: - cloudlog.exception("timed.failed_setting_time") - - -def main() -> NoReturn: - """ - timed has two responsibilities: - - getting the current time from GPS - - publishing the time in the logs - - AGNOS will also use NTP to update the time. - """ - - params = Params() - gps_location_service = get_gps_location_service(params) - - pm = messaging.PubMaster(['clocks']) - sm = messaging.SubMaster([gps_location_service]) - while True: - sm.update(1000) - - msg = messaging.new_message('clocks') - msg.valid = system_time_valid() - msg.clocks.wallTimeNanos = time.time_ns() - pm.send('clocks', msg) - - gps = sm[gps_location_service] - gps_time = datetime.datetime.fromtimestamp(gps.unixTimestampMillis / 1000.) - if not sm.updated[gps_location_service] or (time.monotonic() - sm.logMonoTime[gps_location_service] / 1e9) > 2.0: - continue - if not gps.hasFix: - continue - if gps_time < min_date(): - continue - - set_time(gps_time) - time.sleep(10) - -if __name__ == "__main__": - main() diff --git a/system/timezoned.py b/system/timezoned.py new file mode 100755 index 00000000000000..884a5c38122843 --- /dev/null +++ b/system/timezoned.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +import json +import os +import time +import subprocess +from typing import NoReturn + +import requests +from timezonefinder import TimezoneFinder + +from common.params import Params +from system.hardware import AGNOS +from system.swaglog import cloudlog + + +def set_timezone(valid_timezones, timezone): + if timezone not in valid_timezones: + cloudlog.error(f"Timezone not supported {timezone}") + return + + cloudlog.debug(f"Setting timezone to {timezone}") + try: + if AGNOS: + tzpath = os.path.join("/usr/share/zoneinfo/", timezone) + subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \ + mv /data/etc/tmptime /data/etc/localtime"', shell=True) + subprocess.check_call(f'sudo su -c "echo \"{timezone}\" > /data/etc/timezone"', shell=True) + else: + subprocess.check_call(f'sudo timedatectl set-timezone {timezone}', shell=True) + except subprocess.CalledProcessError: + cloudlog.exception(f"Error setting timezone to {timezone}") + + +def main() -> NoReturn: + params = Params() + tf = TimezoneFinder() + + # Get allowed timezones + valid_timezones = subprocess.check_output('timedatectl list-timezones', shell=True, encoding='utf8').strip().split('\n') + + while True: + time.sleep(60) + + is_onroad = not params.get_bool("IsOffroad") + if is_onroad: + continue + + # Set based on param + timezone = params.get("Timezone", encoding='utf8') + if timezone is not None: + cloudlog.debug("Setting timezone based on param") + set_timezone(valid_timezones, timezone) + continue + + location = params.get("LastGPSPosition", encoding='utf8') + + # Find timezone based on IP geolocation if no gps location is available + if location is None: + cloudlog.debug("Setting timezone based on IP lookup") + try: + r = requests.get("https://ipapi.co/timezone", timeout=10) + if r.status_code == 200: + set_timezone(valid_timezones, r.text) + else: + cloudlog.error(f"Unexpected status code from api {r.status_code}") + + time.sleep(3600) # Don't make too many API requests + except requests.exceptions.RequestException: + cloudlog.exception("Error getting timezone based on IP") + continue + + # Find timezone by reverse geocoding the last known gps location + else: + cloudlog.debug("Setting timezone based on GPS location") + try: + location = json.loads(location) + except Exception: + cloudlog.exception("Error parsing location") + continue + + timezone = tf.timezone_at(lng=location['longitude'], lat=location['latitude']) + if timezone is None: + cloudlog.error(f"No timezone found based on location, {location}") + continue + set_timezone(valid_timezones, timezone) + + +if __name__ == "__main__": + main() diff --git a/system/tombstoned.py b/system/tombstoned.py deleted file mode 100755 index 5bcced266629cf..00000000000000 --- a/system/tombstoned.py +++ /dev/null @@ -1,176 +0,0 @@ -#!/usr/bin/env python3 -import datetime -import os -import re -import shutil -import signal -import subprocess -import time -import glob -from typing import NoReturn - -import openpilot.system.sentry as sentry -from openpilot.system.hardware.hw import Paths -from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_build_metadata - -MAX_SIZE = 1_000_000 * 100 # allow up to 100M -MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") - -TOMBSTONE_DIR = "/data/tombstones/" -APPORT_DIR = "/var/crash/" - - -def safe_fn(s): - extra = ['_'] - return "".join(c for c in s if c.isalnum() or c in extra).rstrip() - - -def clear_apport_folder(): - for f in glob.glob(APPORT_DIR + '*'): - try: - os.remove(f) - except Exception: - pass - - -def get_apport_stacktrace(fn): - try: - cmd = f'apport-retrace -s <(cat <(echo "Package: openpilot") "{fn}")' - return subprocess.check_output(cmd, shell=True, encoding='utf8', timeout=30, executable='/bin/bash') - except subprocess.CalledProcessError: - return "Error getting stacktrace" - except subprocess.TimeoutExpired: - return "Timeout getting stacktrace" - - -def get_tombstones(): - """Returns list of (filename, ctime) for all crashlogs""" - files = [] - if os.path.exists(APPORT_DIR): - with os.scandir(APPORT_DIR) as d: - # Loop over first 1000 directory entries - for _, f in zip(range(1000), d, strict=False): - if f.name.startswith("tombstone"): - files.append((f.path, int(f.stat().st_ctime))) - elif f.name.endswith(".crash") and f.stat().st_mode == 0o100640: - files.append((f.path, int(f.stat().st_ctime))) - return files - - -def report_tombstone_apport(fn): - f_size = os.path.getsize(fn) - if f_size > MAX_SIZE: - cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") - return - - message = "" # One line description of the crash - contents = "" # Full file contents without coredump - path = "" # File path relative to openpilot directory - - proc_maps = False - - with open(fn) as f: - for line in f: - if "CoreDump" in line: - break - elif "ProcMaps" in line: - proc_maps = True - elif "ProcStatus" in line: - proc_maps = False - - if not proc_maps: - contents += line - - if "ExecutablePath" in line: - path = line.strip().split(': ')[-1] - path = path.replace('/data/openpilot/', '') - message += path - elif "Signal" in line: - message += " - " + line.strip() - - try: - sig_num = int(line.strip().split(': ')[-1]) - message += " (" + signal.Signals(sig_num).name + ")" - except ValueError: - pass - - stacktrace = get_apport_stacktrace(fn) - stacktrace_s = stacktrace.split('\n') - crash_function = "No stacktrace" - - if len(stacktrace_s) > 2: - found = False - - # Try to find first entry in openpilot, fall back to first line - for line in stacktrace_s: - if "at selfdrive/" in line: - crash_function = line - found = True - break - - if not found: - crash_function = stacktrace_s[1] - - # Remove arguments that can contain pointers to make sentry one-liner unique - crash_function = " ".join(x for x in crash_function.split(' ')[1:] if not x.startswith('0x')) - crash_function = re.sub(r'\(.*?\)', '', crash_function) - - contents = stacktrace + "\n\n" + contents - message = message + " - " + crash_function - sentry.report_tombstone(fn, message, contents) - - # Copy crashlog to upload folder - clean_path = path.replace('/', '_') - date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") - - build_metadata = get_build_metadata() - - new_fn = f"{date}_{(build_metadata.openpilot.git_commit or 'nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] - - crashlog_dir = os.path.join(Paths.log_root(), "crash") - os.makedirs(crashlog_dir, exist_ok=True) - - # Files could be on different filesystems, copy, then delete - shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) - - try: - os.remove(fn) - except PermissionError: - pass - - -def main() -> NoReturn: - should_report = sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE) - - # Clear apport folder on start, otherwise duplicate crashes won't register - clear_apport_folder() - initial_tombstones = set(get_tombstones()) - - while True: - now_tombstones = set(get_tombstones()) - - for fn, _ in (now_tombstones - initial_tombstones): - # clear logs if we're not interested in them - if not should_report: - try: - os.remove(fn) - except Exception: - pass - continue - - try: - cloudlog.info(f"reporting new tombstone {fn}") - if fn.endswith(".crash"): - report_tombstone_apport(fn) - else: - cloudlog.error(f"unknown crash type: {fn}") - except Exception: - cloudlog.exception(f"Error reporting tombstone {fn}") - - initial_tombstones = now_tombstones - time.sleep(5) - - -if __name__ == "__main__": - main() diff --git a/system/ubloxd/binary_struct.py b/system/ubloxd/binary_struct.py deleted file mode 100644 index 7b229620a2f591..00000000000000 --- a/system/ubloxd/binary_struct.py +++ /dev/null @@ -1,280 +0,0 @@ -""" -Binary struct parsing DSL. - -Defines a declarative schema for binary messages using dataclasses -and type annotations. -""" - -import struct -from enum import Enum -from dataclasses import dataclass, is_dataclass -from typing import Annotated, Any, TypeVar, get_args, get_origin - - -class FieldType: - """Base class for field type descriptors.""" - - -@dataclass(frozen=True) -class IntType(FieldType): - bits: int - signed: bool - big_endian: bool = False - -@dataclass(frozen=True) -class FloatType(FieldType): - bits: int - -@dataclass(frozen=True) -class BitsType(FieldType): - bits: int - -@dataclass(frozen=True) -class BytesType(FieldType): - size: int - -@dataclass(frozen=True) -class ArrayType(FieldType): - element_type: Any - count_field: str - -@dataclass(frozen=True) -class SwitchType(FieldType): - selector: str - cases: dict[Any, Any] - default: Any = None - -@dataclass(frozen=True) -class EnumType(FieldType): - base_type: FieldType - enum_cls: type[Enum] - -@dataclass(frozen=True) -class ConstType(FieldType): - base_type: FieldType - expected: Any - -@dataclass(frozen=True) -class SubstreamType(FieldType): - length_field: str - element_type: Any - -# Common types - little endian -u8 = IntType(8, False) -u16 = IntType(16, False) -u32 = IntType(32, False) -s8 = IntType(8, True) -s16 = IntType(16, True) -s32 = IntType(32, True) -f32 = FloatType(32) -f64 = FloatType(64) -# Big endian variants -u16be = IntType(16, False, big_endian=True) -u32be = IntType(32, False, big_endian=True) -s16be = IntType(16, True, big_endian=True) -s32be = IntType(32, True, big_endian=True) - - -def bits(n: int) -> BitsType: - """Create a bit-level field type.""" - return BitsType(n) - -def bytes_field(size: int) -> BytesType: - """Create a fixed-size bytes field.""" - return BytesType(size) - -def array(element_type: Any, count_field: str) -> ArrayType: - """Create an array/repeated field.""" - return ArrayType(element_type, count_field) - -def switch(selector: str, cases: dict[Any, Any], default: Any = None) -> SwitchType: - """Create a switch-on field.""" - return SwitchType(selector, cases, default) - -def enum(base_type: Any, enum_cls: type[Enum]) -> EnumType: - """Create an enum-wrapped field.""" - field_type = _field_type_from_spec(base_type) - if field_type is None: - raise TypeError(f"Unsupported field type: {base_type!r}") - return EnumType(field_type, enum_cls) - -def const(base_type: Any, expected: Any) -> ConstType: - """Create a constant-value field.""" - field_type = _field_type_from_spec(base_type) - if field_type is None: - raise TypeError(f"Unsupported field type: {base_type!r}") - return ConstType(field_type, expected) - -def substream(length_field: str, element_type: Any) -> SubstreamType: - """Parse a fixed-length substream using an inner schema.""" - return SubstreamType(length_field, element_type) - - -class BinaryReader: - def __init__(self, data: bytes): - self.data = data - self.pos = 0 - self.bit_pos = 0 # 0-7, position within current byte - - def _require(self, n: int) -> None: - if self.pos + n > len(self.data): - raise EOFError("Unexpected end of data") - - def _read_struct(self, fmt: str): - self._align_to_byte() - size = struct.calcsize(fmt) - self._require(size) - value = struct.unpack_from(fmt, self.data, self.pos)[0] - self.pos += size - return value - - def read_bytes(self, n: int) -> bytes: - self._align_to_byte() - self._require(n) - result = self.data[self.pos : self.pos + n] - self.pos += n - return result - - def read_bits_int_be(self, n: int) -> int: - result = 0 - bits_remaining = n - while bits_remaining > 0: - if self.pos >= len(self.data): - raise EOFError("Unexpected end of data while reading bits") - bits_in_byte = 8 - self.bit_pos - bits_to_read = min(bits_remaining, bits_in_byte) - byte_val = self.data[self.pos] - shift = bits_in_byte - bits_to_read - mask = (1 << bits_to_read) - 1 - extracted = (byte_val >> shift) & mask - result = (result << bits_to_read) | extracted - self.bit_pos += bits_to_read - bits_remaining -= bits_to_read - if self.bit_pos >= 8: - self.bit_pos = 0 - self.pos += 1 - return result - - def _align_to_byte(self) -> None: - if self.bit_pos > 0: - self.bit_pos = 0 - self.pos += 1 - - -T = TypeVar('T', bound='BinaryStruct') - - -class BinaryStruct: - """Base class for binary struct definitions.""" - - def __init_subclass__(cls, **kwargs) -> None: - super().__init_subclass__(**kwargs) - if cls is BinaryStruct: - return - if not is_dataclass(cls): - dataclass(init=False)(cls) - fields = list(getattr(cls, '__annotations__', {}).items()) - cls.__binary_fields__ = fields # type: ignore[attr-defined] - - @classmethod - def _read(inner_cls, reader: BinaryReader): - obj = inner_cls.__new__(inner_cls) - for name, spec in inner_cls.__binary_fields__: - value = _parse_field(spec, reader, obj) - setattr(obj, name, value) - return obj - - cls._read = _read # type: ignore[attr-defined] - - @classmethod - def from_bytes(cls: type[T], data: bytes) -> T: - """Parse struct from bytes.""" - reader = BinaryReader(data) - return cls._read(reader) - - @classmethod - def _read(cls: type[T], reader: BinaryReader) -> T: - """Override in subclasses to implement parsing.""" - raise NotImplementedError - - -def _resolve_path(obj: Any, path: str) -> Any: - cur = obj - for part in path.split('.'): - cur = getattr(cur, part) - return cur - -def _unwrap_annotated(spec: Any) -> tuple[Any, ...]: - if get_origin(spec) is Annotated: - return get_args(spec)[1:] - return () - -def _field_type_from_spec(spec: Any) -> FieldType | None: - if isinstance(spec, FieldType): - return spec - for item in _unwrap_annotated(spec): - if isinstance(item, FieldType): - return item - return None - - -def _int_format(field_type: IntType) -> str: - if field_type.bits == 8: - return 'b' if field_type.signed else 'B' - endian = '>' if field_type.big_endian else '<' - if field_type.bits == 16: - code = 'h' if field_type.signed else 'H' - elif field_type.bits == 32: - code = 'i' if field_type.signed else 'I' - else: - raise ValueError(f"Unsupported integer size: {field_type.bits}") - return f"{endian}{code}" - -def _float_format(field_type: FloatType) -> str: - if field_type.bits == 32: - return ' Any: - field_type = _field_type_from_spec(spec) - if field_type is not None: - spec = field_type - if isinstance(spec, ConstType): - value = _parse_field(spec.base_type, reader, obj) - if value != spec.expected: - raise ValueError(f"Invalid constant: expected {spec.expected!r}, got {value!r}") - return value - if isinstance(spec, EnumType): - raw = _parse_field(spec.base_type, reader, obj) - try: - return spec.enum_cls(raw) - except ValueError: - return raw - if isinstance(spec, SwitchType): - key = _resolve_path(obj, spec.selector) - target = spec.cases.get(key, spec.default) - if target is None: - return None - return _parse_field(target, reader, obj) - if isinstance(spec, ArrayType): - count = _resolve_path(obj, spec.count_field) - return [_parse_field(spec.element_type, reader, obj) for _ in range(int(count))] - if isinstance(spec, SubstreamType): - length = _resolve_path(obj, spec.length_field) - data = reader.read_bytes(int(length)) - sub_reader = BinaryReader(data) - return _parse_field(spec.element_type, sub_reader, obj) - if isinstance(spec, IntType): - return reader._read_struct(_int_format(spec)) - if isinstance(spec, FloatType): - return reader._read_struct(_float_format(spec)) - if isinstance(spec, BitsType): - value = reader.read_bits_int_be(spec.bits) - return bool(value) if spec.bits == 1 else value - if isinstance(spec, BytesType): - return reader.read_bytes(spec.size) - if isinstance(spec, type) and issubclass(spec, BinaryStruct): - return spec._read(reader) - raise TypeError(f"Unsupported field spec: {spec!r}") diff --git a/system/ubloxd/glonass.py b/system/ubloxd/glonass.py deleted file mode 100644 index 144ccdde6e244b..00000000000000 --- a/system/ubloxd/glonass.py +++ /dev/null @@ -1,156 +0,0 @@ -""" -Parses GLONASS navigation strings per GLONASS ICD specification. -http://gauss.gge.unb.ca/GLONASS.ICD.pdf -https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf -""" - -from typing import Annotated - -from openpilot.system.ubloxd import binary_struct as bs - - -class Glonass(bs.BinaryStruct): - class String1(bs.BinaryStruct): - not_used: Annotated[int, bs.bits(2)] - p1: Annotated[int, bs.bits(2)] - t_k: Annotated[int, bs.bits(12)] - x_vel_sign: Annotated[bool, bs.bits(1)] - x_vel_value: Annotated[int, bs.bits(23)] - x_accel_sign: Annotated[bool, bs.bits(1)] - x_accel_value: Annotated[int, bs.bits(4)] - x_sign: Annotated[bool, bs.bits(1)] - x_value: Annotated[int, bs.bits(26)] - - @property - def x_vel(self) -> int: - """Computed x_vel from sign-magnitude representation.""" - return (self.x_vel_value * -1) if self.x_vel_sign else self.x_vel_value - - @property - def x_accel(self) -> int: - """Computed x_accel from sign-magnitude representation.""" - return (self.x_accel_value * -1) if self.x_accel_sign else self.x_accel_value - - @property - def x(self) -> int: - """Computed x from sign-magnitude representation.""" - return (self.x_value * -1) if self.x_sign else self.x_value - - class String2(bs.BinaryStruct): - b_n: Annotated[int, bs.bits(3)] - p2: Annotated[bool, bs.bits(1)] - t_b: Annotated[int, bs.bits(7)] - not_used: Annotated[int, bs.bits(5)] - y_vel_sign: Annotated[bool, bs.bits(1)] - y_vel_value: Annotated[int, bs.bits(23)] - y_accel_sign: Annotated[bool, bs.bits(1)] - y_accel_value: Annotated[int, bs.bits(4)] - y_sign: Annotated[bool, bs.bits(1)] - y_value: Annotated[int, bs.bits(26)] - - @property - def y_vel(self) -> int: - """Computed y_vel from sign-magnitude representation.""" - return (self.y_vel_value * -1) if self.y_vel_sign else self.y_vel_value - - @property - def y_accel(self) -> int: - """Computed y_accel from sign-magnitude representation.""" - return (self.y_accel_value * -1) if self.y_accel_sign else self.y_accel_value - - @property - def y(self) -> int: - """Computed y from sign-magnitude representation.""" - return (self.y_value * -1) if self.y_sign else self.y_value - - class String3(bs.BinaryStruct): - p3: Annotated[bool, bs.bits(1)] - gamma_n_sign: Annotated[bool, bs.bits(1)] - gamma_n_value: Annotated[int, bs.bits(10)] - not_used: Annotated[bool, bs.bits(1)] - p: Annotated[int, bs.bits(2)] - l_n: Annotated[bool, bs.bits(1)] - z_vel_sign: Annotated[bool, bs.bits(1)] - z_vel_value: Annotated[int, bs.bits(23)] - z_accel_sign: Annotated[bool, bs.bits(1)] - z_accel_value: Annotated[int, bs.bits(4)] - z_sign: Annotated[bool, bs.bits(1)] - z_value: Annotated[int, bs.bits(26)] - - @property - def gamma_n(self) -> int: - """Computed gamma_n from sign-magnitude representation.""" - return (self.gamma_n_value * -1) if self.gamma_n_sign else self.gamma_n_value - - @property - def z_vel(self) -> int: - """Computed z_vel from sign-magnitude representation.""" - return (self.z_vel_value * -1) if self.z_vel_sign else self.z_vel_value - - @property - def z_accel(self) -> int: - """Computed z_accel from sign-magnitude representation.""" - return (self.z_accel_value * -1) if self.z_accel_sign else self.z_accel_value - - @property - def z(self) -> int: - """Computed z from sign-magnitude representation.""" - return (self.z_value * -1) if self.z_sign else self.z_value - - class String4(bs.BinaryStruct): - tau_n_sign: Annotated[bool, bs.bits(1)] - tau_n_value: Annotated[int, bs.bits(21)] - delta_tau_n_sign: Annotated[bool, bs.bits(1)] - delta_tau_n_value: Annotated[int, bs.bits(4)] - e_n: Annotated[int, bs.bits(5)] - not_used_1: Annotated[int, bs.bits(14)] - p4: Annotated[bool, bs.bits(1)] - f_t: Annotated[int, bs.bits(4)] - not_used_2: Annotated[int, bs.bits(3)] - n_t: Annotated[int, bs.bits(11)] - n: Annotated[int, bs.bits(5)] - m: Annotated[int, bs.bits(2)] - - @property - def tau_n(self) -> int: - """Computed tau_n from sign-magnitude representation.""" - return (self.tau_n_value * -1) if self.tau_n_sign else self.tau_n_value - - @property - def delta_tau_n(self) -> int: - """Computed delta_tau_n from sign-magnitude representation.""" - return (self.delta_tau_n_value * -1) if self.delta_tau_n_sign else self.delta_tau_n_value - - class String5(bs.BinaryStruct): - n_a: Annotated[int, bs.bits(11)] - tau_c: Annotated[int, bs.bits(32)] - not_used: Annotated[bool, bs.bits(1)] - n_4: Annotated[int, bs.bits(5)] - tau_gps: Annotated[int, bs.bits(22)] - l_n: Annotated[bool, bs.bits(1)] - - class StringNonImmediate(bs.BinaryStruct): - data_1: Annotated[int, bs.bits(64)] - data_2: Annotated[int, bs.bits(8)] - - idle_chip: Annotated[bool, bs.bits(1)] - string_number: Annotated[int, bs.bits(4)] - data: Annotated[ - object, - bs.switch( - 'string_number', - { - 1: String1, - 2: String2, - 3: String3, - 4: String4, - 5: String5, - }, - default=StringNonImmediate, - ), - ] - hamming_code: Annotated[int, bs.bits(8)] - pad_1: Annotated[int, bs.bits(11)] - superframe_number: Annotated[int, bs.bits(16)] - pad_2: Annotated[int, bs.bits(8)] - frame_number: Annotated[int, bs.bits(8)] diff --git a/system/ubloxd/gps.py b/system/ubloxd/gps.py deleted file mode 100644 index 1c0833bd92d990..00000000000000 --- a/system/ubloxd/gps.py +++ /dev/null @@ -1,116 +0,0 @@ -""" -Parses GPS navigation subframes per IS-GPS-200E specification. -https://www.gps.gov/technical/icwg/IS-GPS-200E.pdf -""" - -from typing import Annotated - -from openpilot.system.ubloxd import binary_struct as bs - - -class Gps(bs.BinaryStruct): - class Tlm(bs.BinaryStruct): - preamble: Annotated[bytes, bs.const(bs.bytes_field(1), b"\x8b")] - tlm: Annotated[int, bs.bits(14)] - integrity_status: Annotated[bool, bs.bits(1)] - reserved: Annotated[bool, bs.bits(1)] - - class How(bs.BinaryStruct): - tow_count: Annotated[int, bs.bits(17)] - alert: Annotated[bool, bs.bits(1)] - anti_spoof: Annotated[bool, bs.bits(1)] - subframe_id: Annotated[int, bs.bits(3)] - reserved: Annotated[int, bs.bits(2)] - - class Subframe1(bs.BinaryStruct): - week_no: Annotated[int, bs.bits(10)] - code: Annotated[int, bs.bits(2)] - sv_accuracy: Annotated[int, bs.bits(4)] - sv_health: Annotated[int, bs.bits(6)] - iodc_msb: Annotated[int, bs.bits(2)] - l2_p_data_flag: Annotated[bool, bs.bits(1)] - reserved1: Annotated[int, bs.bits(23)] - reserved2: Annotated[int, bs.bits(24)] - reserved3: Annotated[int, bs.bits(24)] - reserved4: Annotated[int, bs.bits(16)] - t_gd: Annotated[int, bs.s8] - iodc_lsb: Annotated[int, bs.u8] - t_oc: Annotated[int, bs.u16be] - af_2: Annotated[int, bs.s8] - af_1: Annotated[int, bs.s16be] - af_0_sign: Annotated[bool, bs.bits(1)] - af_0_value: Annotated[int, bs.bits(21)] - reserved5: Annotated[int, bs.bits(2)] - - @property - def af_0(self) -> int: - """Computed af_0 from sign-magnitude representation.""" - return (self.af_0_value - (1 << 21)) if self.af_0_sign else self.af_0_value - - class Subframe2(bs.BinaryStruct): - iode: Annotated[int, bs.u8] - c_rs: Annotated[int, bs.s16be] - delta_n: Annotated[int, bs.s16be] - m_0: Annotated[int, bs.s32be] - c_uc: Annotated[int, bs.s16be] - e: Annotated[int, bs.s32be] - c_us: Annotated[int, bs.s16be] - sqrt_a: Annotated[int, bs.u32be] - t_oe: Annotated[int, bs.u16be] - fit_interval_flag: Annotated[bool, bs.bits(1)] - aoda: Annotated[int, bs.bits(5)] - reserved: Annotated[int, bs.bits(2)] - - class Subframe3(bs.BinaryStruct): - c_ic: Annotated[int, bs.s16be] - omega_0: Annotated[int, bs.s32be] - c_is: Annotated[int, bs.s16be] - i_0: Annotated[int, bs.s32be] - c_rc: Annotated[int, bs.s16be] - omega: Annotated[int, bs.s32be] - omega_dot_sign: Annotated[bool, bs.bits(1)] - omega_dot_value: Annotated[int, bs.bits(23)] - iode: Annotated[int, bs.u8] - idot_sign: Annotated[bool, bs.bits(1)] - idot_value: Annotated[int, bs.bits(13)] - reserved: Annotated[int, bs.bits(2)] - - @property - def omega_dot(self) -> int: - """Computed omega_dot from sign-magnitude representation.""" - return (self.omega_dot_value - (1 << 23)) if self.omega_dot_sign else self.omega_dot_value - - @property - def idot(self) -> int: - """Computed idot from sign-magnitude representation.""" - return (self.idot_value - (1 << 13)) if self.idot_sign else self.idot_value - - class Subframe4(bs.BinaryStruct): - class IonosphereData(bs.BinaryStruct): - a0: Annotated[int, bs.s8] - a1: Annotated[int, bs.s8] - a2: Annotated[int, bs.s8] - a3: Annotated[int, bs.s8] - b0: Annotated[int, bs.s8] - b1: Annotated[int, bs.s8] - b2: Annotated[int, bs.s8] - b3: Annotated[int, bs.s8] - - data_id: Annotated[int, bs.bits(2)] - page_id: Annotated[int, bs.bits(6)] - body: Annotated[object, bs.switch('page_id', {56: IonosphereData})] - - tlm: Tlm - how: How - body: Annotated[ - object, - bs.switch( - 'how.subframe_id', - { - 1: Subframe1, - 2: Subframe2, - 3: Subframe3, - 4: Subframe4, - }, - ), - ] diff --git a/system/ubloxd/pigeond.py b/system/ubloxd/pigeond.py deleted file mode 100755 index e458a9d65f2373..00000000000000 --- a/system/ubloxd/pigeond.py +++ /dev/null @@ -1,309 +0,0 @@ -#!/usr/bin/env python3 -import sys -import time -import signal -import serial -import struct -import requests -import urllib.parse -from datetime import datetime, UTC - -from cereal import messaging -from openpilot.common.time_helpers import system_time_valid -from openpilot.common.params import Params -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import TICI -from openpilot.common.gpio import gpio_init, gpio_set -from openpilot.system.hardware.tici.pins import GPIO - -UBLOX_TTY = "/dev/ttyHS0" - -UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00" -UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00" -UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00" -UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00" -UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03" -UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00" - -def set_power(enabled: bool) -> None: - gpio_init(GPIO.UBLOX_SAFEBOOT_N, True) - gpio_init(GPIO.GNSS_PWR_EN, True) - gpio_init(GPIO.UBLOX_RST_N, True) - - gpio_set(GPIO.UBLOX_SAFEBOOT_N, True) - gpio_set(GPIO.GNSS_PWR_EN, enabled) - gpio_set(GPIO.UBLOX_RST_N, enabled) - -def add_ubx_checksum(msg: bytes) -> bytes: - A = B = 0 - for b in msg[2:]: - A = (A + b) % 256 - B = (B + A) % 256 - return msg + bytes([A, B]) - -def get_assistnow_messages(token: str) -> list[bytes]: - # make request - # TODO: implement adding the last known location - r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({ - 'token': token, - 'gnss': 'gps,glo', - 'datatype': 'eph,alm,aux', - }, safe=':,'), timeout=5) - assert r.status_code == 200, "Got invalid status code" - dat = r.content - - # split up messages - msgs = [] - while len(dat) > 0: - assert dat[:2] == b"\xB5\x62" - msg_len = 6 + (dat[5] << 8 | dat[4]) + 2 - msgs.append(dat[:msg_len]) - dat = dat[msg_len:] - return msgs - - -class TTYPigeon: - def __init__(self): - self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0) - - def send(self, dat: bytes) -> None: - self.tty.write(dat) - - def receive(self) -> bytes: - dat = b'' - while len(dat) < 0x1000: - d = self.tty.read(0x40) - dat += d - if len(d) == 0: - break - return dat - - def set_baud(self, baud: int) -> None: - self.tty.baudrate = baud - - def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool: - dat = b'' - st = time.monotonic() - while True: - dat += self.receive() - if ack in dat: - cloudlog.debug("Received ACK from ublox") - return True - elif nack in dat: - cloudlog.error("Received NACK from ublox") - return False - elif time.monotonic() - st > timeout: - cloudlog.error("No response from ublox") - raise TimeoutError('No response from ublox') - time.sleep(0.001) - - def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None: - self.send(dat) - self.wait_for_ack(ack, nack) - - def wait_for_backup_restore_status(self, timeout: float = 1.) -> int: - dat = b'' - st = time.monotonic() - while True: - dat += self.receive() - position = dat.find(UBLOX_BACKUP_RESTORE_MSG) - if position >= 0 and len(dat) >= position + 11: - return dat[position + 10] - elif time.monotonic() - st > timeout: - cloudlog.error("No backup restore response from ublox") - raise TimeoutError('No response from ublox') - time.sleep(0.001) - - def reset_device(self) -> bool: - # deleting the backup does not always work on first try (mostly on second try) - for _ in range(5): - # device cold start - self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d") - time.sleep(1) # wait for cold start - init_baudrate(self) - - # clear configuration - self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7") - - # clear flash memory (almanac backup) - self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0") - - # try restoring backup to verify it got deleted - self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") - # 1: failed to restore, 2: could restore, 3: no backup - status = self.wait_for_backup_restore_status() - if status == 1 or status == 3: - return True - return False - -def save_almanac(pigeon: TTYPigeon) -> None: - # store almanac in flash - pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC") - try: - if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK): - cloudlog.info("Done storing almanac") - else: - cloudlog.error("Error storing almanac") - except TimeoutError: - pass - -def init_baudrate(pigeon: TTYPigeon): - # ublox default setting on startup is 9600 baudrate - pigeon.set_baud(9600) - - # $PUBX,41,1,0007,0003,460800,0*15\r\n - pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A") - time.sleep(0.1) - pigeon.set_baud(460800) - - -def init_pigeon(pigeon: TTYPigeon) -> bool: - # try initializing a few times - for _ in range(10): - try: - - # setup port config - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22") - pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24") - - # UBX-CFG-RATE (0x06 0x08) - pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10") - - # UBX-CFG-NAV5 (0x06 0x24) - pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63") - - # UBX-CFG-ODO (0x06 0x1E) - pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37") - pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C") - pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24") - - # UBX-CFG-NAV5 (0x06 0x24) - pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84") - pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81") - pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72") - pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3") - - # UBX-CFG-MSG (set message rate) - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51") - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70") - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C") - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70") - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74") - pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x35\x01\x41\xAD") - cloudlog.debug("pigeon configured") - - # try restoring almanac backup - pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") - restore_status = pigeon.wait_for_backup_restore_status() - if restore_status == 2: - cloudlog.warning("almanac backup restored") - elif restore_status == 3: - cloudlog.warning("no almanac backup found") - else: - cloudlog.error(f"failed to restore almanac backup, status: {restore_status}") - - # sending time to ublox - if system_time_valid(): - t_now = datetime.now(UTC).replace(tzinfo=None) - cloudlog.warning("Sending current time to ublox") - - # UBX-MGA-INI-TIME_UTC - msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack(" None: - # register exit handler - signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon)) - - # power cycle ublox - set_power(False) - time.sleep(0.1) - set_power(True) - time.sleep(0.5) - - init_baudrate(pigeon) - init_pigeon(pigeon) - -def run_receiving(duration: int = 0): - pm = messaging.PubMaster(['ubloxRaw']) - - pigeon = TTYPigeon() - init(pigeon) - - start_time = time.monotonic() - last_almanac_save = time.monotonic() - while (duration == 0) or (time.monotonic() - start_time < duration): - dat = pigeon.receive() - if len(dat) > 0: - if dat[0] == 0x00: - cloudlog.warning("received invalid data from ublox, re-initing!") - init(pigeon) - continue - - # send out to socket - msg = messaging.new_message('ubloxRaw', len(dat), valid=True) - msg.ubloxRaw = dat[:] - pm.send('ubloxRaw', msg) - - # save almanac every 5 minutes - if (time.monotonic() - last_almanac_save) > 60*5: - save_almanac(pigeon) - last_almanac_save = time.monotonic() - else: - # prevent locking up a CPU core if ublox disconnects - time.sleep(0.001) - - -def main(): - assert TICI, "unsupported hardware for pigeond" - run_receiving() - -if __name__ == "__main__": - main() diff --git a/system/ubloxd/tests/test_pigeond.py b/system/ubloxd/tests/test_pigeond.py deleted file mode 100644 index 202820e4125125..00000000000000 --- a/system/ubloxd/tests/test_pigeond.py +++ /dev/null @@ -1,54 +0,0 @@ -import pytest -import time - -import cereal.messaging as messaging -from cereal.services import SERVICE_LIST -from openpilot.common.gpio import gpio_read -from openpilot.selfdrive.test.helpers import with_processes -from openpilot.system.manager.process_config import managed_processes -from openpilot.system.hardware.tici.pins import GPIO - - -# TODO: test TTFF when we have good A-GNSS -@pytest.mark.tici -class TestPigeond: - - def teardown_method(self): - managed_processes['pigeond'].stop() - - @with_processes(['pigeond']) - def test_frequency(self): - sm = messaging.SubMaster(['ubloxRaw']) - - # setup time - for _ in range(int(5 * SERVICE_LIST['ubloxRaw'].frequency)): - sm.update() - - for _ in range(int(10 * SERVICE_LIST['ubloxRaw'].frequency)): - sm.update() - assert sm.all_checks() - - def test_startup_time(self): - for _ in range(5): - sm = messaging.SubMaster(['ubloxRaw']) - managed_processes['pigeond'].start() - - start_time = time.monotonic() - for __ in range(10): - sm.update(1 * 1000) - if sm.updated['ubloxRaw']: - break - assert sm.recv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time" - - et = time.monotonic() - start_time - assert et < 5, f"pigeond took {et:.1f}s to start" - managed_processes['pigeond'].stop() - - def test_turns_off_ublox(self): - for s in (0.1, 0.5, 1, 5): - managed_processes['pigeond'].start() - time.sleep(s) - managed_processes['pigeond'].stop() - - assert gpio_read(GPIO.UBLOX_RST_N) == 0 - assert gpio_read(GPIO.GNSS_PWR_EN) == 0 diff --git a/system/ubloxd/ubloxd.py b/system/ubloxd/ubloxd.py deleted file mode 100755 index e55cadcf78be3e..00000000000000 --- a/system/ubloxd/ubloxd.py +++ /dev/null @@ -1,534 +0,0 @@ -#!/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.ubx import Ubx -from openpilot.system.ubloxd.gps import Gps -from openpilot.system.ubloxd.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 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(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/ubloxd/ubx.py b/system/ubloxd/ubx.py deleted file mode 100644 index 857498ebf1351e..00000000000000 --- a/system/ubloxd/ubx.py +++ /dev/null @@ -1,180 +0,0 @@ -""" -UBX protocol parser -""" - -from enum import IntEnum -from typing import Annotated - -from openpilot.system.ubloxd import binary_struct as bs - - -class GnssType(IntEnum): - gps = 0 - sbas = 1 - galileo = 2 - beidou = 3 - imes = 4 - qzss = 5 - glonass = 6 - - -class Ubx(bs.BinaryStruct): - GnssType = GnssType - - class RxmRawx(bs.BinaryStruct): - class Measurement(bs.BinaryStruct): - pr_mes: Annotated[float, bs.f64] - cp_mes: Annotated[float, bs.f64] - do_mes: Annotated[float, bs.f32] - gnss_id: Annotated[GnssType | int, bs.enum(bs.u8, GnssType)] - sv_id: Annotated[int, bs.u8] - reserved2: Annotated[bytes, bs.bytes_field(1)] - freq_id: Annotated[int, bs.u8] - lock_time: Annotated[int, bs.u16] - cno: Annotated[int, bs.u8] - pr_stdev: Annotated[int, bs.u8] - cp_stdev: Annotated[int, bs.u8] - do_stdev: Annotated[int, bs.u8] - trk_stat: Annotated[int, bs.u8] - reserved3: Annotated[bytes, bs.bytes_field(1)] - - rcv_tow: Annotated[float, bs.f64] - week: Annotated[int, bs.u16] - leap_s: Annotated[int, bs.s8] - num_meas: Annotated[int, bs.u8] - rec_stat: Annotated[int, bs.u8] - reserved1: Annotated[bytes, bs.bytes_field(3)] - meas: Annotated[list[Measurement], bs.array(Measurement, count_field='num_meas')] - - class RxmSfrbx(bs.BinaryStruct): - gnss_id: Annotated[GnssType | int, bs.enum(bs.u8, GnssType)] - sv_id: Annotated[int, bs.u8] - reserved1: Annotated[bytes, bs.bytes_field(1)] - freq_id: Annotated[int, bs.u8] - num_words: Annotated[int, bs.u8] - reserved2: Annotated[bytes, bs.bytes_field(1)] - version: Annotated[int, bs.u8] - reserved3: Annotated[bytes, bs.bytes_field(1)] - body: Annotated[list[int], bs.array(bs.u32, count_field='num_words')] - - class NavSat(bs.BinaryStruct): - class Nav(bs.BinaryStruct): - gnss_id: Annotated[GnssType | int, bs.enum(bs.u8, GnssType)] - sv_id: Annotated[int, bs.u8] - cno: Annotated[int, bs.u8] - elev: Annotated[int, bs.s8] - azim: Annotated[int, bs.s16] - pr_res: Annotated[int, bs.s16] - flags: Annotated[int, bs.u32] - - itow: Annotated[int, bs.u32] - version: Annotated[int, bs.u8] - num_svs: Annotated[int, bs.u8] - reserved: Annotated[bytes, bs.bytes_field(2)] - svs: Annotated[list[Nav], bs.array(Nav, count_field='num_svs')] - - class NavPvt(bs.BinaryStruct): - i_tow: Annotated[int, bs.u32] - year: Annotated[int, bs.u16] - month: Annotated[int, bs.u8] - day: Annotated[int, bs.u8] - hour: Annotated[int, bs.u8] - min: Annotated[int, bs.u8] - sec: Annotated[int, bs.u8] - valid: Annotated[int, bs.u8] - t_acc: Annotated[int, bs.u32] - nano: Annotated[int, bs.s32] - fix_type: Annotated[int, bs.u8] - flags: Annotated[int, bs.u8] - flags2: Annotated[int, bs.u8] - num_sv: Annotated[int, bs.u8] - lon: Annotated[int, bs.s32] - lat: Annotated[int, bs.s32] - height: Annotated[int, bs.s32] - h_msl: Annotated[int, bs.s32] - h_acc: Annotated[int, bs.u32] - v_acc: Annotated[int, bs.u32] - vel_n: Annotated[int, bs.s32] - vel_e: Annotated[int, bs.s32] - vel_d: Annotated[int, bs.s32] - g_speed: Annotated[int, bs.s32] - head_mot: Annotated[int, bs.s32] - s_acc: Annotated[int, bs.s32] - head_acc: Annotated[int, bs.u32] - p_dop: Annotated[int, bs.u16] - flags3: Annotated[int, bs.u8] - reserved1: Annotated[bytes, bs.bytes_field(5)] - head_veh: Annotated[int, bs.s32] - mag_dec: Annotated[int, bs.s16] - mag_acc: Annotated[int, bs.u16] - - class MonHw2(bs.BinaryStruct): - class ConfigSource(IntEnum): - flash = 102 - otp = 111 - config_pins = 112 - rom = 113 - - ofs_i: Annotated[int, bs.s8] - mag_i: Annotated[int, bs.u8] - ofs_q: Annotated[int, bs.s8] - mag_q: Annotated[int, bs.u8] - cfg_source: Annotated[ConfigSource | int, bs.enum(bs.u8, ConfigSource)] - reserved1: Annotated[bytes, bs.bytes_field(3)] - low_lev_cfg: Annotated[int, bs.u32] - reserved2: Annotated[bytes, bs.bytes_field(8)] - post_status: Annotated[int, bs.u32] - reserved3: Annotated[bytes, bs.bytes_field(4)] - - class MonHw(bs.BinaryStruct): - class AntennaStatus(IntEnum): - init = 0 - dontknow = 1 - ok = 2 - short = 3 - open = 4 - - class AntennaPower(IntEnum): - false = 0 - true = 1 - dontknow = 2 - - pin_sel: Annotated[int, bs.u32] - pin_bank: Annotated[int, bs.u32] - pin_dir: Annotated[int, bs.u32] - pin_val: Annotated[int, bs.u32] - noise_per_ms: Annotated[int, bs.u16] - agc_cnt: Annotated[int, bs.u16] - a_status: Annotated[AntennaStatus | int, bs.enum(bs.u8, AntennaStatus)] - a_power: Annotated[AntennaPower | int, bs.enum(bs.u8, AntennaPower)] - flags: Annotated[int, bs.u8] - reserved1: Annotated[bytes, bs.bytes_field(1)] - used_mask: Annotated[int, bs.u32] - vp: Annotated[bytes, bs.bytes_field(17)] - jam_ind: Annotated[int, bs.u8] - reserved2: Annotated[bytes, bs.bytes_field(2)] - pin_irq: Annotated[int, bs.u32] - pull_h: Annotated[int, bs.u32] - pull_l: Annotated[int, bs.u32] - - magic: Annotated[bytes, bs.const(bs.bytes_field(2), b"\xb5\x62")] - msg_type: Annotated[int, bs.u16be] - length: Annotated[int, bs.u16] - body: Annotated[ - object, - bs.substream( - 'length', - bs.switch( - 'msg_type', - { - 0x0107: NavPvt, - 0x0213: RxmSfrbx, - 0x0215: RxmRawx, - 0x0A09: MonHw, - 0x0A0B: MonHw2, - 0x0135: NavSat, - }, - ), - ), - ] - checksum: Annotated[int, bs.u16] diff --git a/system/ui/README.md b/system/ui/README.md deleted file mode 100644 index 79a4dd32ea9240..00000000000000 --- a/system/ui/README.md +++ /dev/null @@ -1,20 +0,0 @@ -# ui - -The user interfaces here are built with [raylib](https://www.raylib.com/). - -Quick start: -* set `BIG=1` to run the comma 3X UI (comma four UI runs by default) -* set `SHOW_FPS=1` to show the FPS -* set `STRICT_MODE=1` to kill the app if it drops too much below 60fps -* set `SCALE=1.5` to scale the entire UI by 1.5x -* set `BURN_IN=1` to get a burn-in heatmap version of the UI -* set `GRID=50` to show a 50-pixel alignment grid overlay -* set `MAGIC_DEBUG=1` to show every dropped frames (only on device) -* set `RECORD=1` to record the screen, output defaults to `output.mp4` but can be set with `RECORD_OUTPUT` -* https://www.raylib.com/cheatsheet/cheatsheet.html -* https://electronstudio.github.io/raylib-python-cffi/README.html#quickstart - -Style guide: -* All graphical elements should subclass [`Widget`](/system/ui/widgets/__init__.py). - * Prefer a stateful widget over a function for easy migration from QT -* All internal class variables and functions should be prefixed with `_` diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py deleted file mode 100644 index da314a394f57e1..00000000000000 --- a/system/ui/lib/application.py +++ /dev/null @@ -1,780 +0,0 @@ -import atexit -import cffi -import os -import queue -import time -import signal -import sys -import pyray as rl -import threading -import platform -import subprocess -from contextlib import contextmanager -from collections.abc import Callable -from collections import deque -from dataclasses import dataclass -from enum import StrEnum -from pathlib import Path -from typing import NamedTuple -from importlib.resources import as_file, files -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import HARDWARE, PC -from openpilot.system.ui.lib.multilang import multilang -from openpilot.common.realtime import Ratekeeper - -_DEFAULT_FPS = int(os.getenv("FPS", {'tizi': 20}.get(HARDWARE.get_device_type(), 60))) -FPS_LOG_INTERVAL = 5 # Seconds between logging FPS drops -FPS_DROP_THRESHOLD = 0.9 # FPS drop threshold for triggering a warning -FPS_CRITICAL_THRESHOLD = 0.5 # Critical threshold for triggering strict actions -MOUSE_THREAD_RATE = 140 # touch controller runs at 140Hz -MAX_TOUCH_SLOTS = 2 -TOUCH_HISTORY_TIMEOUT = 3.0 # Seconds before touch points fade out - -BIG_UI = os.getenv("BIG", "0") == "1" -ENABLE_VSYNC = os.getenv("ENABLE_VSYNC", "0") == "1" -SHOW_FPS = os.getenv("SHOW_FPS") == "1" -SHOW_TOUCHES = os.getenv("SHOW_TOUCHES") == "1" -STRICT_MODE = os.getenv("STRICT_MODE") == "1" -SCALE = float(os.getenv("SCALE", "1.0")) -GRID_SIZE = int(os.getenv("GRID", "0")) -PROFILE_RENDER = int(os.getenv("PROFILE_RENDER", "0")) -PROFILE_STATS = int(os.getenv("PROFILE_STATS", "100")) # Number of functions to show in profile output -RECORD = os.getenv("RECORD") == "1" -RECORD_OUTPUT = str(Path(os.getenv("RECORD_OUTPUT", "output")).with_suffix(".mp4")) -RECORD_BITRATE = os.getenv("RECORD_BITRATE", "") # Target bitrate e.g. "2000k" -RECORD_SPEED = int(os.getenv("RECORD_SPEED", "1")) # Speed multiplier -OFFSCREEN = os.getenv("OFFSCREEN") == "1" # Disable FPS limiting for fast offline rendering - -GL_VERSION = """ -#version 300 es -precision highp float; -""" -if platform.system() == "Darwin": - GL_VERSION = """ - #version 330 core - """ - -BURN_IN_MODE = "BURN_IN" in os.environ -BURN_IN_VERTEX_SHADER = GL_VERSION + """ -in vec3 vertexPosition; -in vec2 vertexTexCoord; -uniform mat4 mvp; -out vec2 fragTexCoord; -void main() { - fragTexCoord = vertexTexCoord; - gl_Position = mvp * vec4(vertexPosition, 1.0); -} -""" -BURN_IN_FRAGMENT_SHADER = GL_VERSION + """ -in vec2 fragTexCoord; -uniform sampler2D texture0; -out vec4 fragColor; -void main() { - vec4 sampled = texture(texture0, fragTexCoord); - float intensity = sampled.b; - // Map blue intensity to green -> yellow -> red to highlight burn-in risk. - vec3 start = vec3(0.0, 1.0, 0.0); - vec3 middle = vec3(1.0, 1.0, 0.0); - vec3 end = vec3(1.0, 0.0, 0.0); - vec3 gradient = mix(start, middle, clamp(intensity * 2.0, 0.0, 1.0)); - gradient = mix(gradient, end, clamp((intensity - 0.5) * 2.0, 0.0, 1.0)); - fragColor = vec4(gradient, sampled.a); -} -""" - -DEFAULT_TEXT_SIZE = 60 -DEFAULT_TEXT_COLOR = rl.Color(255, 255, 255, int(255 * 0.9)) - -# Qt draws fonts accounting for ascent/descent differently, so compensate to match old styles -# The real scales for the fonts below range from 1.212 to 1.266 -FONT_SCALE = 1.242 if BIG_UI else 1.16 - -ASSETS_DIR = files("openpilot.selfdrive").joinpath("assets") -FONT_DIR = ASSETS_DIR.joinpath("fonts") - - -class FontWeight(StrEnum): - LIGHT = "Inter-Light.fnt" - NORMAL = "Inter-Regular.fnt" if BIG_UI else "Inter-Medium.fnt" - MEDIUM = "Inter-Medium.fnt" - BOLD = "Inter-Bold.fnt" - SEMI_BOLD = "Inter-SemiBold.fnt" - UNIFONT = "unifont.fnt" - - # Small UI fonts - DISPLAY_REGULAR = "Inter-Regular.fnt" - ROMAN = "Inter-Regular.fnt" - DISPLAY = "Inter-Bold.fnt" - - -def font_fallback(font: rl.Font) -> rl.Font: - """Fall back to unifont for languages that require it.""" - if multilang.requires_unifont(): - return gui_app.font(FontWeight.UNIFONT) - return font - - -@dataclass -class ModalOverlay: - overlay: object = None - callback: Callable | None = None - - -class MousePos(NamedTuple): - x: float - y: float - - -class MousePosWithTime(NamedTuple): - x: float - y: float - t: float - - -class MouseEvent(NamedTuple): - pos: MousePos - slot: int - left_pressed: bool - left_released: bool - left_down: bool - t: float - - -class MouseState: - def __init__(self, scale: float = 1.0): - self._scale = scale - self._events: deque[MouseEvent] = deque(maxlen=MOUSE_THREAD_RATE) # bound event list - self._prev_mouse_event: list[MouseEvent | None] = [None] * MAX_TOUCH_SLOTS - - self._rk = Ratekeeper(MOUSE_THREAD_RATE, print_delay_threshold=None) - self._lock = threading.Lock() - self._exit_event = threading.Event() - self._thread = None - - def get_events(self) -> list[MouseEvent]: - with self._lock: - events = list(self._events) - self._events.clear() - return events - - def start(self): - self._exit_event.clear() - if self._thread is None or not self._thread.is_alive(): - self._thread = threading.Thread(target=self._run_thread, daemon=True) - self._thread.start() - - def stop(self): - self._exit_event.set() - if self._thread is not None and self._thread.is_alive(): - self._thread.join() - - def _run_thread(self): - while not self._exit_event.is_set(): - rl.poll_input_events() - self._handle_mouse_event() - self._rk.keep_time() - - def _handle_mouse_event(self): - for slot in range(MAX_TOUCH_SLOTS): - mouse_pos = rl.get_touch_position(slot) - x = mouse_pos.x / self._scale if self._scale != 1.0 else mouse_pos.x - y = mouse_pos.y / self._scale if self._scale != 1.0 else mouse_pos.y - ev = MouseEvent( - MousePos(x, y), - slot, - rl.is_mouse_button_pressed(slot), # noqa: TID251 - rl.is_mouse_button_released(slot), # noqa: TID251 - rl.is_mouse_button_down(slot), - time.monotonic(), - ) - # Only add changes - prev = self._prev_mouse_event[slot] - if prev is None or ev[:-1] != prev[:-1]: - with self._lock: - self._events.append(ev) - self._prev_mouse_event[slot] = ev - - -class GuiApplication: - def __init__(self, width: int | None = None, height: int | None = None): - self._set_log_callback() - - self._fonts: dict[FontWeight, rl.Font] = {} - self._width = width if width is not None else GuiApplication._default_width() - self._height = height if height is not None else GuiApplication._default_height() - - if PC and os.getenv("SCALE") is None: - self._scale = self._calculate_auto_scale() - else: - self._scale = SCALE - - # Scale, then ensure dimensions are even - self._scaled_width = int(self._width * self._scale) - self._scaled_height = int(self._height * self._scale) - self._scaled_width += self._scaled_width % 2 - self._scaled_height += self._scaled_height % 2 - - self._render_texture: rl.RenderTexture | None = None - self._burn_in_shader: rl.Shader | None = None - self._ffmpeg_proc: subprocess.Popen | None = None - self._ffmpeg_queue: queue.Queue | None = None - self._ffmpeg_thread: threading.Thread | None = None - self._ffmpeg_stop_event: threading.Event | None = None - self._textures: dict[str, rl.Texture] = {} - self._target_fps: int = _DEFAULT_FPS - self._last_fps_log_time: float = time.monotonic() - self._frame = 0 - self._window_close_requested = False - self._modal_overlay = ModalOverlay() - self._modal_overlay_shown = False - self._modal_overlay_tick: Callable[[], None] | None = None - - self._mouse = MouseState(self._scale) - self._mouse_events: list[MouseEvent] = [] - self._last_mouse_event: MouseEvent = MouseEvent(MousePos(0, 0), 0, False, False, False, 0.0) - - self._should_render = True - - # Debug variables - self._mouse_history: deque[MousePosWithTime] = deque(maxlen=MOUSE_THREAD_RATE) - self._show_touches = SHOW_TOUCHES - self._show_fps = SHOW_FPS - self._grid_size = GRID_SIZE - self._profile_render_frames = PROFILE_RENDER - self._render_profiler = None - self._render_profile_start_time = None - - @property - def frame(self): - return self._frame - - def set_show_touches(self, show: bool): - self._show_touches = show - - def set_show_fps(self, show: bool): - self._show_fps = show - - @property - def target_fps(self): - return self._target_fps - - def request_close(self): - self._window_close_requested = True - - def init_window(self, title: str, fps: int = _DEFAULT_FPS): - with self._startup_profile_context(): - def _close(sig, frame): - self.close() - sys.exit(0) - signal.signal(signal.SIGINT, _close) - atexit.register(self.close) - - flags = rl.ConfigFlags.FLAG_MSAA_4X_HINT - if ENABLE_VSYNC: - flags |= rl.ConfigFlags.FLAG_VSYNC_HINT - rl.set_config_flags(flags) - - rl.init_window(self._scaled_width, self._scaled_height, title) - - needs_render_texture = self._scale != 1.0 or BURN_IN_MODE or RECORD - if self._scale != 1.0: - rl.set_mouse_scale(1 / self._scale, 1 / self._scale) - if needs_render_texture: - self._render_texture = rl.load_render_texture(self._width, self._height) - rl.set_texture_filter(self._render_texture.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR) - - if RECORD: - output_fps = fps * RECORD_SPEED - ffmpeg_args = [ - 'ffmpeg', - '-v', 'warning', # Reduce ffmpeg log spam - '-nostats', # Suppress encoding progress - '-f', 'rawvideo', # Input format - '-pix_fmt', 'rgba', # Input pixel format - '-s', f'{self._width}x{self._height}', # Input resolution - '-r', str(fps), # Input frame rate - '-i', 'pipe:0', # Input from stdin - '-vf', 'vflip,format=yuv420p', # Flip vertically and convert to yuv420p - '-r', str(output_fps), # Output frame rate (for speed multiplier) - '-c:v', 'libx264', - '-preset', 'ultrafast', - ] - if RECORD_BITRATE: - ffmpeg_args += ['-b:v', RECORD_BITRATE, '-maxrate', RECORD_BITRATE, '-bufsize', RECORD_BITRATE] - ffmpeg_args += [ - '-y', # Overwrite existing file - '-f', 'mp4', # Output format - RECORD_OUTPUT, # Output file path - ] - self._ffmpeg_proc = subprocess.Popen(ffmpeg_args, stdin=subprocess.PIPE) - self._ffmpeg_queue = queue.Queue(maxsize=60) # Buffer up to 60 frames - self._ffmpeg_stop_event = threading.Event() - self._ffmpeg_thread = threading.Thread(target=self._ffmpeg_writer_thread, daemon=True) - self._ffmpeg_thread.start() - - # OFFSCREEN disables FPS limiting for fast offline rendering (e.g. clips) - rl.set_target_fps(0 if OFFSCREEN else fps) - - self._target_fps = fps - self._set_styles() - self._load_fonts() - self._patch_text_functions() - if BURN_IN_MODE and self._burn_in_shader is None: - self._burn_in_shader = rl.load_shader_from_memory(BURN_IN_VERTEX_SHADER, BURN_IN_FRAGMENT_SHADER) - - if not PC: - self._mouse.start() - - @contextmanager - def _startup_profile_context(self): - if "PROFILE_STARTUP" not in os.environ: - yield - return - - import cProfile - import io - import pstats - - profiler = cProfile.Profile() - start_time = time.monotonic() - profiler.enable() - - # do the init - yield - - profiler.disable() - elapsed_ms = (time.monotonic() - start_time) * 1e3 - - stats_stream = io.StringIO() - pstats.Stats(profiler, stream=stats_stream).sort_stats("cumtime").print_stats(25) - print("\n=== Startup profile ===") - print(stats_stream.getvalue().rstrip()) - - green = "\033[92m" - reset = "\033[0m" - print(f"{green}UI window ready in {elapsed_ms:.1f} ms{reset}") - sys.exit(0) - - def _ffmpeg_writer_thread(self): - """Background thread that writes frames to ffmpeg.""" - while True: - try: - data = self._ffmpeg_queue.get(timeout=1.0) - if data is None: # Sentinel to stop - break - self._ffmpeg_proc.stdin.write(data) - except queue.Empty: - if self._ffmpeg_stop_event.is_set(): - break - continue - except Exception: - break - - def set_modal_overlay(self, overlay, callback: Callable | None = None): - if self._modal_overlay.overlay is not None: - if hasattr(self._modal_overlay.overlay, 'hide_event'): - self._modal_overlay.overlay.hide_event() - - if self._modal_overlay.callback is not None: - self._modal_overlay.callback(-1) - - self._modal_overlay = ModalOverlay(overlay=overlay, callback=callback) - - def set_modal_overlay_tick(self, tick_function: Callable | None): - self._modal_overlay_tick = tick_function - - def set_should_render(self, should_render: bool): - self._should_render = should_render - - def texture(self, asset_path: str, width: int | None = None, height: int | None = None, - alpha_premultiply=False, keep_aspect_ratio=True): - cache_key = f"{asset_path}_{width}_{height}_{alpha_premultiply}{keep_aspect_ratio}" - if cache_key in self._textures: - return self._textures[cache_key] - - with as_file(ASSETS_DIR.joinpath(asset_path)) as fspath: - image_obj = self._load_image_from_path(fspath.as_posix(), width, height, alpha_premultiply, keep_aspect_ratio) - texture_obj = self._load_texture_from_image(image_obj) - self._textures[cache_key] = texture_obj - return texture_obj - - def _load_image_from_path(self, image_path: str, width: int | None = None, height: int | None = None, - alpha_premultiply: bool = False, keep_aspect_ratio: bool = True) -> rl.Image: - """Load and resize an image, storing it for later automatic unloading.""" - image = rl.load_image(image_path) - - if alpha_premultiply: - rl.image_alpha_premultiply(image) - - if width is not None and height is not None: - same_dimensions = image.width == width and image.height == height - - # Resize with aspect ratio preservation if requested - if not same_dimensions: - if keep_aspect_ratio: - orig_width = image.width - orig_height = image.height - - scale_width = width / orig_width - scale_height = height / orig_height - - # Calculate new dimensions - scale = min(scale_width, scale_height) - new_width = int(orig_width * scale) - new_height = int(orig_height * scale) - - rl.image_resize(image, new_width, new_height) - else: - rl.image_resize(image, width, height) - else: - assert keep_aspect_ratio, "Cannot resize without specifying width and height" - return image - - def _load_texture_from_image(self, image: rl.Image) -> rl.Texture: - """Send image to GPU and unload original image.""" - texture = rl.load_texture_from_image(image) - # Set texture filtering to smooth the result - rl.set_texture_filter(texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR) - # prevent artifacts from wrapping coordinates - rl.set_texture_wrap(texture, rl.TextureWrap.TEXTURE_WRAP_CLAMP) - - rl.unload_image(image) - return texture - - def close_ffmpeg(self): - if self._ffmpeg_thread is not None: - # Signal thread to stop, send sentinel, then wait for it to drain - self._ffmpeg_stop_event.set() - self._ffmpeg_queue.put(None) - self._ffmpeg_thread.join(timeout=30) - - if self._ffmpeg_proc is not None: - self._ffmpeg_proc.stdin.flush() - self._ffmpeg_proc.stdin.close() - try: - self._ffmpeg_proc.wait(timeout=30) - except subprocess.TimeoutExpired: - self._ffmpeg_proc.terminate() - self._ffmpeg_proc.wait() - - def close(self): - if not rl.is_window_ready(): - return - - for texture in self._textures.values(): - rl.unload_texture(texture) - self._textures = {} - - for font in self._fonts.values(): - rl.unload_font(font) - self._fonts = {} - - if self._render_texture is not None: - rl.unload_render_texture(self._render_texture) - self._render_texture = None - - if self._burn_in_shader: - rl.unload_shader(self._burn_in_shader) - self._burn_in_shader = None - - if not PC: - self._mouse.stop() - - self.close_ffmpeg() - - rl.close_window() - - @property - def mouse_events(self) -> list[MouseEvent]: - return self._mouse_events - - @property - def last_mouse_event(self) -> MouseEvent: - return self._last_mouse_event - - def render(self): - try: - if self._profile_render_frames > 0: - import cProfile - self._render_profiler = cProfile.Profile() - self._render_profile_start_time = time.monotonic() - self._render_profiler.enable() - - while not (self._window_close_requested or rl.window_should_close()): - if PC: - # Thread is not used on PC, need to manually add mouse events - self._mouse._handle_mouse_event() - - # Store all mouse events for the current frame - self._mouse_events = self._mouse.get_events() - if len(self._mouse_events) > 0: - self._last_mouse_event = self._mouse_events[-1] - - # Skip rendering when screen is off - if not self._should_render: - if PC: - rl.poll_input_events() - time.sleep(1 / self._target_fps) - yield False - continue - - if self._render_texture: - rl.begin_texture_mode(self._render_texture) - rl.clear_background(rl.BLACK) - else: - rl.begin_drawing() - rl.clear_background(rl.BLACK) - - # Handle modal overlay rendering and input processing - if self._handle_modal_overlay(): - # Allow a Widget to still run a function while overlay is shown - if self._modal_overlay_tick is not None: - self._modal_overlay_tick() - yield False - else: - yield True - - if self._render_texture: - rl.end_texture_mode() - rl.begin_drawing() - rl.clear_background(rl.BLACK) - src_rect = rl.Rectangle(0, 0, float(self._width), -float(self._height)) - dst_rect = rl.Rectangle(0, 0, float(self._scaled_width), float(self._scaled_height)) - texture = self._render_texture.texture - if texture: - if BURN_IN_MODE and self._burn_in_shader: - rl.begin_shader_mode(self._burn_in_shader) - rl.draw_texture_pro(texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - else: - rl.draw_texture_pro(texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - - if self._show_fps: - rl.draw_fps(10, 10) - - if self._show_touches: - self._draw_touch_points() - - if self._grid_size > 0: - self._draw_grid() - - rl.end_drawing() - - if RECORD: - image = rl.load_image_from_texture(self._render_texture.texture) - data_size = image.width * image.height * 4 - data = bytes(rl.ffi.buffer(image.data, data_size)) - self._ffmpeg_queue.put(data) # Async write via background thread - rl.unload_image(image) - - self._monitor_fps() - self._frame += 1 - - if self._profile_render_frames > 0 and self._frame >= self._profile_render_frames: - self._output_render_profile() - except KeyboardInterrupt: - pass - - def font(self, font_weight: FontWeight = FontWeight.NORMAL) -> rl.Font: - return self._fonts[font_weight] - - @property - def width(self): - return self._width - - @property - def height(self): - return self._height - - def _handle_modal_overlay(self) -> bool: - if self._modal_overlay.overlay: - if hasattr(self._modal_overlay.overlay, 'render'): - result = self._modal_overlay.overlay.render(rl.Rectangle(0, 0, self.width, self.height)) - elif callable(self._modal_overlay.overlay): - result = self._modal_overlay.overlay() - else: - raise Exception - - # Send show event to Widget - if not self._modal_overlay_shown and hasattr(self._modal_overlay.overlay, 'show_event'): - self._modal_overlay.overlay.show_event() - self._modal_overlay_shown = True - - if result >= 0: - # Clear the overlay and execute the callback - original_modal = self._modal_overlay - self._modal_overlay = ModalOverlay() - if hasattr(original_modal.overlay, 'hide_event'): - original_modal.overlay.hide_event() - if original_modal.callback is not None: - original_modal.callback(result) - return True - else: - self._modal_overlay_shown = False - return False - - def _load_fonts(self): - for font_weight_file in FontWeight: - with as_file(FONT_DIR) as fspath: - fnt_path = fspath / font_weight_file - font = rl.load_font(fnt_path.as_posix()) - if font_weight_file != FontWeight.UNIFONT: - rl.set_texture_filter(font.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR) - self._fonts[font_weight_file] = font - rl.gui_set_font(self._fonts[FontWeight.NORMAL]) - - def _set_styles(self): - rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.BORDER_WIDTH, 0) - rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_SIZE, DEFAULT_TEXT_SIZE) - rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.BACKGROUND_COLOR, rl.color_to_int(rl.BLACK)) - rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(DEFAULT_TEXT_COLOR)) - rl.gui_set_style(rl.GuiControl.DEFAULT, rl.GuiControlProperty.BASE_COLOR_NORMAL, rl.color_to_int(rl.Color(50, 50, 50, 255))) - - def _patch_text_functions(self): - # Wrap pyray text APIs to apply a global text size scale so our px sizes match Qt - if not hasattr(rl, "_orig_draw_text_ex"): - rl._orig_draw_text_ex = rl.draw_text_ex - - def _draw_text_ex_scaled(font, text, position, font_size, spacing, tint): - font = font_fallback(font) - return rl._orig_draw_text_ex(font, text, position, font_size * FONT_SCALE, spacing, tint) - - rl.draw_text_ex = _draw_text_ex_scaled - - def _set_log_callback(self): - ffi_libc = cffi.FFI() - ffi_libc.cdef(""" - int vasprintf(char **strp, const char *fmt, void *ap); - void free(void *ptr); - """) - libc = ffi_libc.dlopen(None) - - @rl.ffi.callback("void(int, char *, void *)") - def trace_log_callback(log_level, text, args): - try: - text_addr = int(rl.ffi.cast("uintptr_t", text)) - args_addr = int(rl.ffi.cast("uintptr_t", args)) - text_libc = ffi_libc.cast("char *", text_addr) - args_libc = ffi_libc.cast("void *", args_addr) - - out = ffi_libc.new("char **") - if libc.vasprintf(out, text_libc, args_libc) >= 0 and out[0] != ffi_libc.NULL: - text_str = ffi_libc.string(out[0]).decode("utf-8", "replace") - libc.free(out[0]) - else: - text_str = rl.ffi.string(text).decode("utf-8", "replace") - except Exception as e: - text_str = f"[Log decode error: {e}]" - - if log_level == rl.TraceLogLevel.LOG_ERROR: - cloudlog.error(f"raylib: {text_str}") - elif log_level == rl.TraceLogLevel.LOG_WARNING: - cloudlog.warning(f"raylib: {text_str}") - elif log_level == rl.TraceLogLevel.LOG_INFO: - cloudlog.info(f"raylib: {text_str}") - elif log_level == rl.TraceLogLevel.LOG_DEBUG: - cloudlog.debug(f"raylib: {text_str}") - else: - cloudlog.error(f"raylib: Unknown level {log_level}: {text_str}") - - # ensure we get all the logs forwarded to us - rl.set_trace_log_level(rl.TraceLogLevel.LOG_DEBUG) - - # Store callback reference - self._trace_log_callback = trace_log_callback - rl.set_trace_log_callback(self._trace_log_callback) - - def _monitor_fps(self): - fps = rl.get_fps() - - # Log FPS drop below threshold at regular intervals - if fps < self._target_fps * FPS_DROP_THRESHOLD: - current_time = time.monotonic() - if current_time - self._last_fps_log_time >= FPS_LOG_INTERVAL: - cloudlog.warning(f"FPS dropped below {self._target_fps}: {fps}") - self._last_fps_log_time = current_time - - # Strict mode: terminate UI if FPS drops too much - if STRICT_MODE and fps < self._target_fps * FPS_CRITICAL_THRESHOLD: - cloudlog.error(f"FPS dropped critically below {fps}. Shutting down UI.") - self.close_ffmpeg() - os._exit(1) - - def _draw_touch_points(self): - current_time = time.monotonic() - - for mouse_event in self._mouse_events: - if mouse_event.left_pressed: - self._mouse_history.clear() - self._mouse_history.append(MousePosWithTime(mouse_event.pos.x * self._scale, mouse_event.pos.y * self._scale, current_time)) - - # Remove old touch points that exceed the timeout - while self._mouse_history and (current_time - self._mouse_history[0].t) > TOUCH_HISTORY_TIMEOUT: - self._mouse_history.popleft() - - if self._mouse_history: - mouse_pos = self._mouse_history[-1] - rl.draw_circle(int(mouse_pos.x), int(mouse_pos.y), 15, rl.RED) - for idx, mouse_pos in enumerate(self._mouse_history): - perc = idx / len(self._mouse_history) - color = rl.Color(min(int(255 * (1.5 - perc)), 255), int(min(255 * (perc + 0.5), 255)), 50, 255) - rl.draw_circle(int(mouse_pos.x), int(mouse_pos.y), 5, color) - - def _draw_grid(self): - grid_color = rl.Color(60, 60, 60, 255) - # Draw vertical lines - x = 0 - while x <= self._scaled_width: - rl.draw_line(x, 0, x, self._scaled_height, grid_color) - x += self._grid_size - # Draw horizontal lines - y = 0 - while y <= self._scaled_height: - rl.draw_line(0, y, self._scaled_width, y, grid_color) - y += self._grid_size - - def _output_render_profile(self): - import io - import pstats - - self._render_profiler.disable() - elapsed_ms = (time.monotonic() - self._render_profile_start_time) * 1e3 - avg_frame_time = elapsed_ms / self._frame if self._frame > 0 else 0 - - stats_stream = io.StringIO() - pstats.Stats(self._render_profiler, stream=stats_stream).sort_stats("cumtime").print_stats(PROFILE_STATS) - print("\n=== Render loop profile ===") - print(stats_stream.getvalue().rstrip()) - - green = "\033[92m" - reset = "\033[0m" - print(f"\n{green}Rendered {self._frame} frames in {elapsed_ms:.1f} ms{reset}") - print(f"{green}Average frame time: {avg_frame_time:.2f} ms ({1000/avg_frame_time:.1f} FPS){reset}") - sys.exit(0) - - def _calculate_auto_scale(self) -> float: - # Create temporary window to query monitor info - rl.init_window(1, 1, "") - w, h = rl.get_monitor_width(0), rl.get_monitor_height(0) - rl.close_window() - - if w == 0 or h == 0 or (w >= self._width and h >= self._height): - return 1.0 - - # Apply 0.95 factor for window decorations/taskbar margin - return max(0.3, min(w / self._width, h / self._height) * 0.95) - - @staticmethod - def _default_width() -> int: - return 2160 if GuiApplication.big_ui() else 536 - - @staticmethod - def _default_height() -> int: - return 1080 if GuiApplication.big_ui() else 240 - - @staticmethod - def big_ui() -> bool: - return HARDWARE.get_device_type() in ('tici', 'tizi') or BIG_UI - - -gui_app = GuiApplication() diff --git a/system/ui/lib/egl.py b/system/ui/lib/egl.py deleted file mode 100644 index 69236482b0e85b..00000000000000 --- a/system/ui/lib/egl.py +++ /dev/null @@ -1,181 +0,0 @@ -import os -import cffi -from dataclasses import dataclass -from typing import Any -from openpilot.common.swaglog import cloudlog - -# EGL constants -EGL_LINUX_DMA_BUF_EXT = 0x3270 -EGL_WIDTH = 0x3057 -EGL_HEIGHT = 0x3056 -EGL_LINUX_DRM_FOURCC_EXT = 0x3271 -EGL_DMA_BUF_PLANE0_FD_EXT = 0x3272 -EGL_DMA_BUF_PLANE0_OFFSET_EXT = 0x3273 -EGL_DMA_BUF_PLANE0_PITCH_EXT = 0x3274 -EGL_DMA_BUF_PLANE1_FD_EXT = 0x3275 -EGL_DMA_BUF_PLANE1_OFFSET_EXT = 0x3276 -EGL_DMA_BUF_PLANE1_PITCH_EXT = 0x3277 -EGL_NONE = 0x3038 -GL_TEXTURE0 = 0x84C0 -GL_TEXTURE_EXTERNAL_OES = 0x8D65 - -# DRM Format for NV12 -DRM_FORMAT_NV12 = 842094158 - - -@dataclass -class EGLImage: - """Container for EGL image and associated resources""" - - egl_image: Any - fd: int - - -@dataclass -class EGLState: - """Container for all EGL-related state""" - - initialized: bool = False - ffi: Any = None - egl_lib: Any = None - gles_lib: Any = None - - # EGL display connection - shared across all users - display: Any = None - - # Constants - NO_CONTEXT: Any = None - NO_DISPLAY: Any = None - NO_IMAGE_KHR: Any = None - - # Function pointers - get_current_display: Any = None - create_image_khr: Any = None - destroy_image_khr: Any = None - image_target_texture: Any = None - get_error: Any = None - bind_texture: Any = None - active_texture: Any = None - - -# Create a single instance of the state -_egl = EGLState() - - -def init_egl() -> bool: - """Initialize EGL and load necessary functions""" - global _egl - - # Don't re-initialize if already done - if _egl.initialized: - return True - - try: - _egl.ffi = cffi.FFI() - _egl.ffi.cdef(""" - typedef int EGLint; - typedef unsigned int EGLBoolean; - typedef unsigned int EGLenum; - typedef unsigned int GLenum; - typedef void *EGLContext; - typedef void *EGLDisplay; - typedef void *EGLClientBuffer; - typedef void *EGLImageKHR; - typedef void *GLeglImageOES; - - EGLDisplay eglGetCurrentDisplay(void); - EGLint eglGetError(void); - EGLImageKHR eglCreateImageKHR(EGLDisplay dpy, EGLContext ctx, - EGLenum target, EGLClientBuffer buffer, - const EGLint *attrib_list); - EGLBoolean eglDestroyImageKHR(EGLDisplay dpy, EGLImageKHR image); - void glEGLImageTargetTexture2DOES(GLenum target, GLeglImageOES image); - void glBindTexture(GLenum target, unsigned int texture); - void glActiveTexture(GLenum texture); - """) - - # Load libraries - _egl.egl_lib = _egl.ffi.dlopen("libEGL.so") - _egl.gles_lib = _egl.ffi.dlopen("libGLESv2.so") - - # Cast NULL pointers - _egl.NO_CONTEXT = _egl.ffi.cast("void *", 0) - _egl.NO_DISPLAY = _egl.ffi.cast("void *", 0) - _egl.NO_IMAGE_KHR = _egl.ffi.cast("void *", 0) - - # Bind functions - _egl.get_current_display = _egl.egl_lib.eglGetCurrentDisplay - _egl.create_image_khr = _egl.egl_lib.eglCreateImageKHR - _egl.destroy_image_khr = _egl.egl_lib.eglDestroyImageKHR - _egl.image_target_texture = _egl.gles_lib.glEGLImageTargetTexture2DOES - _egl.get_error = _egl.egl_lib.eglGetError - _egl.bind_texture = _egl.gles_lib.glBindTexture - _egl.active_texture = _egl.gles_lib.glActiveTexture - - # Initialize EGL display once here - _egl.display = _egl.get_current_display() - if _egl.display == _egl.NO_DISPLAY: - raise RuntimeError("Failed to get EGL display") - - _egl.initialized = True - return True - except Exception as e: - cloudlog.exception(f"EGL initialization failed: {e}") - _egl.initialized = False - return False - - -def create_egl_image(width: int, height: int, stride: int, fd: int, uv_offset: int) -> EGLImage | None: - assert _egl.initialized, "EGL not initialized" - - try: - # Duplicate fd since EGL needs it - dup_fd = os.dup(fd) - except OSError as e: - cloudlog.exception(f"Failed to duplicate frame fd when creating EGL image: {e}") - return None - - # Create image attributes for EGL - img_attrs = [ - EGL_WIDTH, width, - EGL_HEIGHT, height, - EGL_LINUX_DRM_FOURCC_EXT, DRM_FORMAT_NV12, - EGL_DMA_BUF_PLANE0_FD_EXT, dup_fd, - EGL_DMA_BUF_PLANE0_OFFSET_EXT, 0, - EGL_DMA_BUF_PLANE0_PITCH_EXT, stride, - EGL_DMA_BUF_PLANE1_FD_EXT, dup_fd, - EGL_DMA_BUF_PLANE1_OFFSET_EXT, uv_offset, - EGL_DMA_BUF_PLANE1_PITCH_EXT, stride, - EGL_NONE - ] - - attr_array = _egl.ffi.new("int[]", img_attrs) - egl_image = _egl.create_image_khr(_egl.display, _egl.NO_CONTEXT, EGL_LINUX_DMA_BUF_EXT, _egl.ffi.NULL, attr_array) - - if egl_image == _egl.NO_IMAGE_KHR: - cloudlog.error(f"Failed to create EGL image: {_egl.get_error()}") - os.close(dup_fd) - return None - - return EGLImage(egl_image=egl_image, fd=dup_fd) - - -def destroy_egl_image(egl_image: EGLImage) -> None: - assert _egl.initialized, "EGL not initialized" - - _egl.destroy_image_khr(_egl.display, egl_image.egl_image) - - # Close the duplicated fd we created in create_egl_image() - # We need to handle OSError since the fd might already be closed - try: - os.close(egl_image.fd) - except OSError: - pass - - -def bind_egl_image_to_texture(texture_id: int, egl_image: EGLImage) -> None: - assert _egl.initialized, "EGL not initialized" - - _egl.active_texture(GL_TEXTURE0) - _egl.bind_texture(GL_TEXTURE_EXTERNAL_OES, texture_id) - _egl.image_target_texture(GL_TEXTURE_EXTERNAL_OES, egl_image.egl_image) diff --git a/system/ui/lib/emoji.py b/system/ui/lib/emoji.py deleted file mode 100644 index 37228e2d45fa0b..00000000000000 --- a/system/ui/lib/emoji.py +++ /dev/null @@ -1,55 +0,0 @@ -import io -import re - -from PIL import Image, ImageDraw, ImageFont -import pyray as rl - -from openpilot.system.ui.lib.application import FONT_DIR - -_emoji_font: ImageFont.FreeTypeFont | None = None -_cache: dict[str, rl.Texture] = {} - -EMOJI_REGEX = re.compile( -"""[\U0001F600-\U0001F64F -\U0001F300-\U0001F5FF -\U0001F680-\U0001F6FF -\U0001F1E0-\U0001F1FF -\U00002700-\U000027BF -\U0001F900-\U0001F9FF -\U00002600-\U000026FF -\U00002300-\U000023FF -\U00002B00-\U00002BFF -\U0001FA70-\U0001FAFF -\U0001F700-\U0001F77F -\u2640-\u2642 -\u2600-\u2B55 -\u200d -\u23cf -\u23e9 -\u231a -\ufe0f -\u3030 -]+""".replace("\n", ""), - flags=re.UNICODE -) - -def _load_emoji_font() -> ImageFont.FreeTypeFont | None: - global _emoji_font - if _emoji_font is None: - _emoji_font = ImageFont.truetype(str(FONT_DIR.joinpath("NotoColorEmoji.ttf")), 109) - return _emoji_font - -def find_emoji(text): - return [(m.start(), m.end(), m.group()) for m in EMOJI_REGEX.finditer(text)] - -def emoji_tex(emoji): - if emoji not in _cache: - img = Image.new("RGBA", (128, 128), (0, 0, 0, 0)) - draw = ImageDraw.Draw(img) - draw.text((0, 0), emoji, font=_load_emoji_font(), embedded_color=True) - with io.BytesIO() as buffer: - img.save(buffer, format="PNG") - l = buffer.tell() - buffer.seek(0) - _cache[emoji] = rl.load_texture_from_image(rl.load_image_from_memory(".png", buffer.getvalue(), l)) - return _cache[emoji] diff --git a/system/ui/lib/multilang.py b/system/ui/lib/multilang.py deleted file mode 100644 index 70de1e3d5c8e6d..00000000000000 --- a/system/ui/lib/multilang.py +++ /dev/null @@ -1,88 +0,0 @@ -from importlib.resources import files -import os -import json -import gettext -from openpilot.common.basedir import BASEDIR -from openpilot.common.swaglog import cloudlog - -try: - from openpilot.common.params import Params -except ImportError: - Params = None - -SYSTEM_UI_DIR = os.path.join(BASEDIR, "system", "ui") -UI_DIR = files("openpilot.selfdrive.ui") -TRANSLATIONS_DIR = UI_DIR.joinpath("translations") -LANGUAGES_FILE = TRANSLATIONS_DIR.joinpath("languages.json") - -UNIFONT_LANGUAGES = [ - "ar", - "th", - "zh-CHT", - "zh-CHS", - "ko", - "ja", -] - - -class Multilang: - def __init__(self): - self._params = Params() if Params is not None else None - self._language: str = "en" - self.languages = {} - self.codes = {} - self._translation: gettext.NullTranslations | gettext.GNUTranslations = gettext.NullTranslations() - self._load_languages() - - @property - def language(self) -> str: - return self._language - - def requires_unifont(self) -> bool: - """Certain languages require unifont to render their glyphs.""" - return self._language in UNIFONT_LANGUAGES - - def setup(self): - try: - with TRANSLATIONS_DIR.joinpath(f'app_{self._language}.mo').open('rb') as fh: - translation = gettext.GNUTranslations(fh) - translation.install() - self._translation = translation - cloudlog.debug(f"Loaded translations for language: {self._language}") - except FileNotFoundError: - cloudlog.error(f"No translation file found for language: {self._language}, using default.") - gettext.install('app') - self._translation = gettext.NullTranslations() - - def change_language(self, language_code: str) -> None: - # Reinstall gettext with the selected language - self._params.put("LanguageSetting", language_code) - self._language = language_code - self.setup() - - def tr(self, text: str) -> str: - return self._translation.gettext(text) - - def trn(self, singular: str, plural: str, n: int) -> str: - return self._translation.ngettext(singular, plural, n) - - def _load_languages(self): - with LANGUAGES_FILE.open(encoding='utf-8') as f: - self.languages = json.load(f) - self.codes = {v: k for k, v in self.languages.items()} - - if self._params is not None: - lang = str(self._params.get("LanguageSetting")).removeprefix("main_") - if lang in self.codes: - self._language = lang - - -multilang = Multilang() -multilang.setup() - -tr, trn = multilang.tr, multilang.trn - - -# no-op marker for static strings translated later -def tr_noop(s: str) -> str: - return s diff --git a/system/ui/lib/networkmanager.py b/system/ui/lib/networkmanager.py deleted file mode 100644 index ffa2ff4db9d352..00000000000000 --- a/system/ui/lib/networkmanager.py +++ /dev/null @@ -1,46 +0,0 @@ -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_ACTIVE_CONNECTION_IFACE = 'org.freedesktop.NetworkManager.Connection.Active' -NM_WIRELESS_IFACE = 'org.freedesktop.NetworkManager.Device.Wireless' -NM_PROPERTIES_IFACE = 'org.freedesktop.DBus.Properties' -NM_DEVICE_IFACE = 'org.freedesktop.NetworkManager.Device' -NM_IP4_CONFIG_IFACE = 'org.freedesktop.NetworkManager.IP4Config' - -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/scroll_panel.py b/system/ui/lib/scroll_panel.py deleted file mode 100644 index a5b9fc70d33555..00000000000000 --- a/system/ui/lib/scroll_panel.py +++ /dev/null @@ -1,134 +0,0 @@ -import math -import pyray as rl -from enum import IntEnum -from openpilot.system.ui.lib.application import gui_app, MouseEvent -from openpilot.common.filter_simple import FirstOrderFilter - -# Scroll constants for smooth scrolling behavior -MOUSE_WHEEL_SCROLL_SPEED = 50 -BOUNCE_RETURN_RATE = 5 # ~0.92 at 60fps -MIN_VELOCITY = 2 # px/s, changes from auto scroll to steady state -MIN_VELOCITY_FOR_CLICKING = 2 * 60 # px/s, accepts clicks while auto scrolling below this velocity -DRAG_THRESHOLD = 12 # pixels of movement to consider it a drag, not a click - -DEBUG = False - - -class ScrollState(IntEnum): - IDLE = 0 # Not dragging, content may be bouncing or scrolling with inertia - DRAGGING_CONTENT = 1 # User is actively dragging the content - - -class GuiScrollPanel: - def __init__(self): - self._scroll_state: ScrollState = ScrollState.IDLE - self._last_mouse_y: float = 0.0 - self._start_mouse_y: float = 0.0 # Track the initial mouse position for drag detection - self._offset_filter_y = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - self._velocity_filter_y = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) - self._last_drag_time: float = 0.0 - - def update(self, bounds: rl.Rectangle, content: rl.Rectangle) -> float: - for mouse_event in gui_app.mouse_events: - if mouse_event.slot == 0: - self._handle_mouse_event(mouse_event, bounds, content) - - self._update_state(bounds, content) - - return float(self._offset_filter_y.x) - - def _update_state(self, bounds: rl.Rectangle, content: rl.Rectangle): - if DEBUG: - rl.draw_rectangle_lines(0, 0, abs(int(self._velocity_filter_y.x)), 10, rl.RED) - - # Handle mouse wheel - self._offset_filter_y.x += rl.get_mouse_wheel_move() * MOUSE_WHEEL_SCROLL_SPEED - - max_scroll_distance = max(0, content.height - bounds.height) - if self._scroll_state == ScrollState.IDLE: - above_bounds, below_bounds = self._check_bounds(bounds, content) - - # Decay velocity when idle - if abs(self._velocity_filter_y.x) > MIN_VELOCITY: - # Faster decay if bouncing back from out of bounds - friction = math.exp(-BOUNCE_RETURN_RATE * 1 / gui_app.target_fps) - self._velocity_filter_y.x *= friction ** 2 if (above_bounds or below_bounds) else friction - else: - self._velocity_filter_y.x = 0.0 - - if above_bounds or below_bounds: - if above_bounds: - self._offset_filter_y.update(0) - else: - self._offset_filter_y.update(-max_scroll_distance) - - self._offset_filter_y.x += self._velocity_filter_y.x / gui_app.target_fps - - elif self._scroll_state == ScrollState.DRAGGING_CONTENT: - # Mouse not moving, decay velocity - if not len(gui_app.mouse_events): - self._velocity_filter_y.update(0.0) - - # Settle to exact bounds - if abs(self._offset_filter_y.x) < 1e-2: - self._offset_filter_y.x = 0.0 - elif abs(self._offset_filter_y.x + max_scroll_distance) < 1e-2: - self._offset_filter_y.x = -max_scroll_distance - - def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, content: rl.Rectangle): - if self._scroll_state == ScrollState.IDLE: - if rl.check_collision_point_rec(mouse_event.pos, bounds): - if mouse_event.left_pressed: - self._start_mouse_y = mouse_event.pos.y - # Interrupt scrolling with new drag - # TODO: stop scrolling with any tap, need to fix is_touch_valid - if abs(self._velocity_filter_y.x) > MIN_VELOCITY_FOR_CLICKING: - self._scroll_state = ScrollState.DRAGGING_CONTENT - # Start velocity at initial measurement for more immediate response - self._velocity_filter_y.initialized = False - - if mouse_event.left_down: - if abs(mouse_event.pos.y - self._start_mouse_y) > DRAG_THRESHOLD: - self._scroll_state = ScrollState.DRAGGING_CONTENT - # Start velocity at initial measurement for more immediate response - self._velocity_filter_y.initialized = False - - elif self._scroll_state == ScrollState.DRAGGING_CONTENT: - if mouse_event.left_released: - self._scroll_state = ScrollState.IDLE - else: - delta_y = mouse_event.pos.y - self._last_mouse_y - above_bounds, below_bounds = self._check_bounds(bounds, content) - # Rubber banding effect when out of bands - if above_bounds or below_bounds: - delta_y /= 3 - - self._offset_filter_y.x += delta_y - - # Track velocity for inertia - dt = mouse_event.t - self._last_drag_time - if dt > 0: - drag_velocity = delta_y / dt - self._velocity_filter_y.update(drag_velocity) - - # TODO: just store last mouse event! - self._last_drag_time = mouse_event.t - self._last_mouse_y = mouse_event.pos.y - - def _check_bounds(self, bounds: rl.Rectangle, content: rl.Rectangle) -> tuple[bool, bool]: - max_scroll_distance = max(0, content.height - bounds.height) - above_bounds = self._offset_filter_y.x > 0 - below_bounds = self._offset_filter_y.x < -max_scroll_distance - return above_bounds, below_bounds - - def is_touch_valid(self): - return self._scroll_state == ScrollState.IDLE and abs(self._velocity_filter_y.x) < MIN_VELOCITY_FOR_CLICKING - - def set_offset(self, position: float) -> None: - self._offset_filter_y.x = position - self._velocity_filter_y.x = 0.0 - self._scroll_state = ScrollState.IDLE - - @property - def offset(self) -> float: - return float(self._offset_filter_y.x) diff --git a/system/ui/lib/scroll_panel2.py b/system/ui/lib/scroll_panel2.py deleted file mode 100644 index 0859071dac2052..00000000000000 --- a/system/ui/lib/scroll_panel2.py +++ /dev/null @@ -1,225 +0,0 @@ -import os -import math -import pyray as rl -from collections.abc import Callable -from enum import Enum -from typing import cast -from openpilot.system.ui.lib.application import gui_app, MouseEvent -from openpilot.system.hardware import TICI -from collections import deque - -MIN_VELOCITY = 10 # px/s, changes from auto scroll to steady state -MIN_VELOCITY_FOR_CLICKING = 2 * 60 # px/s, accepts clicks while auto scrolling below this velocity -MIN_DRAG_PIXELS = 12 -AUTO_SCROLL_TC_SNAP = 0.025 -AUTO_SCROLL_TC = 0.18 -BOUNCE_RETURN_RATE = 10.0 -REJECT_DECELERATION_FACTOR = 3 -MAX_SPEED = 10000.0 # px/s - -DEBUG = os.getenv("DEBUG_SCROLL", "0") == "1" - - -# from https://ariya.io/2011/10/flick-list-with-its-momentum-scrolling-and-deceleration -class ScrollState(Enum): - STEADY = 0 - PRESSED = 1 - MANUAL_SCROLL = 2 - AUTO_SCROLL = 3 - - -class GuiScrollPanel2: - def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None: - self._horizontal = horizontal - self._handle_out_of_bounds = handle_out_of_bounds - self._AUTO_SCROLL_TC = AUTO_SCROLL_TC_SNAP if not self._handle_out_of_bounds else AUTO_SCROLL_TC - self._state = ScrollState.STEADY - self._offset: rl.Vector2 = rl.Vector2(0, 0) - self._initial_click_event: MouseEvent | None = None - self._previous_mouse_event: MouseEvent | None = None - self._velocity = 0.0 # pixels per second - self._velocity_buffer: deque[float] = deque(maxlen=12 if TICI else 6) - self._enabled: bool | Callable[[], bool] = True - - def set_enabled(self, enabled: bool | Callable[[], bool]) -> None: - self._enabled = enabled - - @property - def enabled(self) -> bool: - return self._enabled() if callable(self._enabled) else self._enabled - - def update(self, bounds: rl.Rectangle, content_size: float) -> float: - if DEBUG: - print('Old state:', self._state) - - bounds_size = bounds.width if self._horizontal else bounds.height - - for mouse_event in gui_app.mouse_events: - self._handle_mouse_event(mouse_event, bounds, bounds_size, content_size) - self._previous_mouse_event = mouse_event - - self._update_state(bounds_size, content_size) - - if DEBUG: - print('Velocity:', self._velocity) - print('Offset X:', self._offset.x, 'Y:', self._offset.y) - print('New state:', self._state) - print() - return self.get_offset() - - def _get_offset_bounds(self, bounds_size: float, content_size: float) -> tuple[float, float]: - """Returns (max_offset, min_offset) for the given bounds and content size.""" - return 0.0, min(0.0, bounds_size - content_size) - - def _update_state(self, bounds_size: float, content_size: float) -> None: - """Runs per render frame, independent of mouse events. Updates auto-scrolling state and velocity.""" - if self._state == ScrollState.AUTO_SCROLL: - max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size) - # simple exponential return if out of bounds - out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset - if out_of_bounds and self._handle_out_of_bounds: - target = max_offset if self.get_offset() > max_offset else min_offset - - dt = rl.get_frame_time() or 1e-6 - factor = 1.0 - math.exp(-BOUNCE_RETURN_RATE * dt) - - dist = target - self.get_offset() - self.set_offset(self.get_offset() + dist * factor) # ease toward the edge - self._velocity *= (1.0 - factor) # damp any leftover fling - - # Steady once we are close enough to the target - if abs(dist) < 1 and abs(self._velocity) < MIN_VELOCITY: - self.set_offset(target) - self._velocity = 0.0 - self._state = ScrollState.STEADY - - elif abs(self._velocity) < MIN_VELOCITY: - self._velocity = 0.0 - self._state = ScrollState.STEADY - - # Update the offset based on the current velocity - dt = rl.get_frame_time() - self.set_offset(self.get_offset() + self._velocity * dt) # Adjust the offset based on velocity - alpha = 1 - (dt / (self._AUTO_SCROLL_TC + dt)) - self._velocity *= alpha - - def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float, - content_size: float) -> None: - max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size) - # simple exponential return if out of bounds - out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset - if DEBUG: - print('Mouse event:', mouse_event) - - mouse_pos = self._get_mouse_pos(mouse_event) - - if not self.enabled: - # Reset state if not enabled - self._state = ScrollState.STEADY - self._velocity = 0.0 - self._velocity_buffer.clear() - - elif self._state == ScrollState.STEADY: - if rl.check_collision_point_rec(mouse_event.pos, bounds): - if mouse_event.left_pressed: - self._state = ScrollState.PRESSED - self._initial_click_event = mouse_event - - elif self._state == ScrollState.PRESSED: - initial_click_pos = self._get_mouse_pos(cast(MouseEvent, self._initial_click_event)) - diff = abs(mouse_pos - initial_click_pos) - if mouse_event.left_released: - # Special handling for down and up clicks across two frames - # TODO: not sure what that means or if it's accurate anymore - if out_of_bounds: - self._state = ScrollState.AUTO_SCROLL - elif diff <= MIN_DRAG_PIXELS: - self._state = ScrollState.STEADY - else: - self._state = ScrollState.MANUAL_SCROLL - elif diff > MIN_DRAG_PIXELS: - self._state = ScrollState.MANUAL_SCROLL - - elif self._state == ScrollState.MANUAL_SCROLL: - if mouse_event.left_released: - # Touch rejection: when releasing finger after swiping and stopping, panel - # reports a few erroneous touch events with high velocity, try to ignore. - - # If velocity decelerates very quickly, assume user doesn't intend to auto scroll - high_decel = False - if len(self._velocity_buffer) > 2: - # We limit max to first half since final few velocities can surpass first few - abs_velocity_buffer = [(abs(v), i) for i, v in enumerate(self._velocity_buffer)] - max_idx = max(abs_velocity_buffer[:len(abs_velocity_buffer) // 2])[1] - min_idx = min(abs_velocity_buffer)[1] - if DEBUG: - print('min_idx:', min_idx, 'max_idx:', max_idx, 'velocity buffer:', self._velocity_buffer) - if (abs(self._velocity_buffer[min_idx]) * REJECT_DECELERATION_FACTOR < abs(self._velocity_buffer[max_idx]) and - max_idx < min_idx): - if DEBUG: - print('deceleration too high, going to STEADY') - high_decel = True - - # If final velocity is below some threshold, switch to steady state too - low_speed = abs(self._velocity) <= MIN_VELOCITY_FOR_CLICKING * 1.5 # plus some margin - - if out_of_bounds or not (high_decel or low_speed): - self._state = ScrollState.AUTO_SCROLL - else: - # TODO: we should just set velocity and let autoscroll go back to steady. delays one frame but who cares - self._velocity = 0.0 - self._state = ScrollState.STEADY - self._velocity_buffer.clear() - else: - # Update velocity for when we release the mouse button. - # Do not update velocity on the same frame the mouse was released - previous_mouse_pos = self._get_mouse_pos(cast(MouseEvent, self._previous_mouse_event)) - delta_x = mouse_pos - previous_mouse_pos - delta_t = max((mouse_event.t - cast(MouseEvent, self._previous_mouse_event).t), 1e-6) - self._velocity = delta_x / delta_t - self._velocity = max(-MAX_SPEED, min(MAX_SPEED, self._velocity)) - self._velocity_buffer.append(self._velocity) - - # rubber-banding: reduce dragging when out of bounds - # TODO: this drifts when dragging quickly - if out_of_bounds: - delta_x *= 0.25 - - # Update the offset based on the mouse movement - # Use internal _offset directly to preserve precision (don't round via get_offset()) - # TODO: make get_offset return float - current_offset = self._offset.x if self._horizontal else self._offset.y - self.set_offset(current_offset + delta_x) - - elif self._state == ScrollState.AUTO_SCROLL: - if mouse_event.left_pressed: - # Decide whether to click or scroll (block click if moving too fast) - if abs(self._velocity) <= MIN_VELOCITY_FOR_CLICKING: - # Traveling slow enough, click - self._state = ScrollState.PRESSED - self._initial_click_event = mouse_event - else: - # Go straight into manual scrolling to block erroneous input - self._state = ScrollState.MANUAL_SCROLL - # Reset velocity for touch down and up events that happen in back-to-back frames - self._velocity = 0.0 - - def _get_mouse_pos(self, mouse_event: MouseEvent) -> float: - return mouse_event.pos.x if self._horizontal else mouse_event.pos.y - - def get_offset(self) -> float: - return self._offset.x if self._horizontal else self._offset.y - - def set_offset(self, value: float) -> None: - if self._horizontal: - self._offset.x = value - else: - self._offset.y = value - - @property - def state(self) -> ScrollState: - return self._state - - def is_touch_valid(self) -> bool: - # MIN_VELOCITY_FOR_CLICKING is checked in auto-scroll state - return bool(self._state != ScrollState.MANUAL_SCROLL) diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py deleted file mode 100644 index 94af35e157fc48..00000000000000 --- a/system/ui/lib/shader_polygon.py +++ /dev/null @@ -1,238 +0,0 @@ -import pyray as rl -import numpy as np -from dataclasses import dataclass -from typing import Any, Optional, cast -from openpilot.system.ui.lib.application import gui_app, GL_VERSION - -MAX_GRADIENT_COLORS = 20 # includes stops as well - - -@dataclass -class Gradient: - start: tuple[float, float] - end: tuple[float, float] - colors: list[rl.Color] - stops: list[float] - - def __post_init__(self): - if len(self.colors) > MAX_GRADIENT_COLORS: - self.colors = self.colors[:MAX_GRADIENT_COLORS] - print(f"Warning: Gradient colors truncated to {MAX_GRADIENT_COLORS} entries") - - if len(self.stops) > MAX_GRADIENT_COLORS: - self.stops = self.stops[:MAX_GRADIENT_COLORS] - print(f"Warning: Gradient stops truncated to {MAX_GRADIENT_COLORS} entries") - - if not len(self.stops): - color_count = min(len(self.colors), MAX_GRADIENT_COLORS) - self.stops = [i / max(1, color_count - 1) for i in range(color_count)] - - -FRAGMENT_SHADER = GL_VERSION + """ -in vec2 fragTexCoord; -out vec4 finalColor; - -uniform vec4 fillColor; - -// Gradient line defined in *screen pixels* -uniform int useGradient; -uniform vec2 gradientStart; // e.g. vec2(0, 0) -uniform vec2 gradientEnd; // e.g. vec2(0, screenHeight) -uniform vec4 gradientColors[20]; -uniform float gradientStops[20]; -uniform int gradientColorCount; - -vec4 getGradientColor(vec2 p) { - // Compute t from screen-space position - vec2 d = gradientStart - gradientEnd; - float len2 = max(dot(d, d), 1e-6); - float t = clamp(dot(p - gradientEnd, d) / len2, 0.0, 1.0); - - // Clamp to range - float t0 = gradientStops[0]; - float tn = gradientStops[gradientColorCount-1]; - if (t <= t0) return gradientColors[0]; - if (t >= tn) return gradientColors[gradientColorCount-1]; - - for (int i = 0; i < gradientColorCount - 1; i++) { - float a = gradientStops[i]; - float b = gradientStops[i+1]; - if (t >= a && t <= b) { - float k = (t - a) / max(b - a, 1e-6); - return mix(gradientColors[i], gradientColors[i+1], k); - } - } - - return gradientColors[gradientColorCount-1]; -} - -void main() { - // TODO: do proper antialiasing - finalColor = useGradient == 1 ? getGradientColor(gl_FragCoord.xy) : fillColor; -} -""" - -# Default vertex shader -VERTEX_SHADER = GL_VERSION + """ -in vec3 vertexPosition; -in vec2 vertexTexCoord; -out vec2 fragTexCoord; -uniform mat4 mvp; - -void main() { - fragTexCoord = vertexTexCoord; - gl_Position = mvp * vec4(vertexPosition, 1.0); -} -""" - -UNIFORM_INT = rl.ShaderUniformDataType.SHADER_UNIFORM_INT -UNIFORM_FLOAT = rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT -UNIFORM_VEC2 = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2 -UNIFORM_VEC4 = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4 - - -class ShaderState: - _instance: Any = None - - @classmethod - def get_instance(cls): - if cls._instance is None: - cls._instance = cls() - return cls._instance - - def __init__(self): - if ShaderState._instance is not None: - raise Exception("This class is a singleton. Use get_instance() instead.") - - self.initialized = False - self.shader = None - - # Shader uniform locations - self.locations = { - 'fillColor': None, - 'useGradient': None, - 'gradientStart': None, - 'gradientEnd': None, - 'gradientColors': None, - 'gradientStops': None, - 'gradientColorCount': None, - 'mvp': None, - } - - # Pre-allocated FFI objects - self.fill_color_ptr = rl.ffi.new("float[]", [0.0, 0.0, 0.0, 0.0]) - self.use_gradient_ptr = rl.ffi.new("int[]", [0]) - self.color_count_ptr = rl.ffi.new("int[]", [0]) - self.gradient_colors_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS * 4) - self.gradient_stops_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS) - - def initialize(self): - if self.initialized: - return - - self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAGMENT_SHADER) - - # Cache all uniform locations - for uniform in self.locations.keys(): - self.locations[uniform] = rl.get_shader_location(self.shader, uniform) - - # Orthographic MVP (origin top-left) - proj = rl.matrix_ortho(0, gui_app.width, gui_app.height, 0, -1, 1) - rl.set_shader_value_matrix(self.shader, self.locations['mvp'], proj) - - self.initialized = True - - def cleanup(self): - if not self.initialized: - return - if self.shader: - rl.unload_shader(self.shader) - self.shader = None - - self.initialized = False - - -def _configure_shader_color(state: ShaderState, color: Optional[rl.Color], - gradient: Gradient | None, origin_rect: rl.Rectangle): - assert (color is not None) != (gradient is not None), "Either color or gradient must be provided" - - use_gradient = 1 if (gradient is not None and len(gradient.colors) >= 1) else 0 - state.use_gradient_ptr[0] = use_gradient - rl.set_shader_value(state.shader, state.locations['useGradient'], state.use_gradient_ptr, UNIFORM_INT) - - if use_gradient: - gradient = cast(Gradient, gradient) - state.color_count_ptr[0] = len(gradient.colors) - for i in range(len(gradient.colors)): - c = gradient.colors[i] - base = i * 4 - state.gradient_colors_ptr[base:base + 4] = [c.r / 255.0, c.g / 255.0, c.b / 255.0, c.a / 255.0] - rl.set_shader_value_v(state.shader, state.locations['gradientColors'], state.gradient_colors_ptr, UNIFORM_VEC4, len(gradient.colors)) - - for i in range(len(gradient.stops)): - s = float(gradient.stops[i]) - state.gradient_stops_ptr[i] = 0.0 if s < 0.0 else 1.0 if s > 1.0 else s - rl.set_shader_value_v(state.shader, state.locations['gradientStops'], state.gradient_stops_ptr, UNIFORM_FLOAT, len(gradient.stops)) - rl.set_shader_value(state.shader, state.locations['gradientColorCount'], state.color_count_ptr, UNIFORM_INT) - - # Map normalized start/end to screen pixels - start_vec = rl.Vector2(origin_rect.x + gradient.start[0] * origin_rect.width, origin_rect.y + gradient.start[1] * origin_rect.height) - end_vec = rl.Vector2(origin_rect.x + gradient.end[0] * origin_rect.width, origin_rect.y + gradient.end[1] * origin_rect.height) - rl.set_shader_value(state.shader, state.locations['gradientStart'], start_vec, UNIFORM_VEC2) - rl.set_shader_value(state.shader, state.locations['gradientEnd'], end_vec, UNIFORM_VEC2) - else: - color = color or rl.WHITE - state.fill_color_ptr[0:4] = [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0] - rl.set_shader_value(state.shader, state.locations['fillColor'], state.fill_color_ptr, UNIFORM_VEC4) - - -def triangulate(pts: np.ndarray) -> list[tuple[float, float]]: - """Only supports simple polygons with two chains (ribbon).""" - - # TODO: consider deduping close screenspace points - # interleave points to produce a triangle strip - # assert len(pts) % 2 == 0, "Interleaving expects even number of points" - if len(pts) % 2 != 0: - pts = pts[:-1] - - tri_strip = [] - for i in range(len(pts) // 2): - tri_strip.append(pts[i]) - tri_strip.append(pts[-i - 1]) - - return cast(list, np.array(tri_strip).tolist()) - - -def draw_polygon(origin_rect: rl.Rectangle, points: np.ndarray, - color: Optional[rl.Color] = None, gradient: Gradient | None = None): - - """ - Draw a ribbon polygon (two chains) with a triangle strip and gradient. - - Input must be [L0..Lk-1, Rk-1..R0], even count, no crossings/holes. - """ - if len(points) < 3: - return - - # Initialize shader on-demand - state = ShaderState.get_instance() - state.initialize() - - # Ensure (N,2) float32 contiguous array - pts = np.ascontiguousarray(points, dtype=np.float32) - assert pts.ndim == 2 and pts.shape[1] == 2, "points must be (N,2)" - - # Configure gradient shader - _configure_shader_color(state, color, gradient, origin_rect) - - # Triangulate via interleaving - tri_strip = triangulate(pts) - - # Draw strip, color here doesn't matter - rl.begin_shader_mode(state.shader) - rl.draw_triangle_strip(tri_strip, len(tri_strip), rl.WHITE) - rl.end_shader_mode() - - -def cleanup_shader_resources(): - state = ShaderState.get_instance() - state.cleanup() diff --git a/system/ui/lib/text_measure.py b/system/ui/lib/text_measure.py deleted file mode 100644 index dee4b419ffc1b5..00000000000000 --- a/system/ui/lib/text_measure.py +++ /dev/null @@ -1,36 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.application import FONT_SCALE, font_fallback -from openpilot.system.ui.lib.emoji import find_emoji - -_cache: dict[int, rl.Vector2] = {} - - -def measure_text_cached(font: rl.Font, text: str, font_size: int, spacing: float = 0) -> rl.Vector2: - """Caches text measurements to avoid redundant calculations.""" - font = font_fallback(font) - spacing = round(spacing, 4) - key = hash((font.texture.id, text, font_size, spacing)) - if key in _cache: - return _cache[key] - - # Measure normal characters without emojis, then add standard width for each found emoji - emoji = find_emoji(text) - if emoji: - non_emoji_text = "" - last_index = 0 - for start, end, _ in emoji: - non_emoji_text += text[last_index:start] - last_index = end - non_emoji_text += text[last_index:] - else: - non_emoji_text = text - - result = rl.measure_text_ex(font, non_emoji_text, font_size * FONT_SCALE, spacing) # noqa: TID251 - if emoji: - result.x += len(emoji) * font_size * FONT_SCALE - # If just emoji assume a single line height - if result.y == 0: - result.y = font_size * FONT_SCALE - - _cache[key] = result - return result diff --git a/system/ui/lib/utils.py b/system/ui/lib/utils.py deleted file mode 100644 index 77035d0da08145..00000000000000 --- a/system/ui/lib/utils.py +++ /dev/null @@ -1,18 +0,0 @@ -import pyray as rl - - -class GuiStyleContext: - def __init__(self, styles: list[tuple[int, int, int]]): - """styles is a list of tuples (control, prop, new_value)""" - self.styles = styles - self.prev_styles: list[tuple[int, int, int]] = [] - - def __enter__(self): - for control, prop, new_value in self.styles: - prev_value = rl.gui_get_style(control, prop) - self.prev_styles.append((control, prop, prev_value)) - rl.gui_set_style(control, prop, new_value) - - def __exit__(self, exc_type, exc_value, traceback): - for control, prop, prev_value in self.prev_styles: - rl.gui_set_style(control, prop, prev_value) diff --git a/system/ui/lib/wifi_manager.py b/system/ui/lib/wifi_manager.py deleted file mode 100644 index bd66b8e03ab81d..00000000000000 --- a/system/ui/lib/wifi_manager.py +++ /dev/null @@ -1,762 +0,0 @@ -import atexit -import threading -import time -import uuid -import subprocess -from collections.abc import Callable -from dataclasses import dataclass -from enum import IntEnum -from typing import Any - -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 - -from openpilot.common.swaglog import cloudlog -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_TYPE_MODEM, NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT, - NM_DEVICE_STATE_REASON_NEW_ACTIVATION, NM_ACTIVE_CONNECTION_IFACE, - NM_IP4_CONFIG_IFACE, NMDeviceState) - -try: - from openpilot.common.params import Params -except Exception: - Params = None - -TETHERING_IP_ADDRESS = "192.168.43.1" -DEFAULT_TETHERING_PASSWORD = "swagswagcomma" -SIGNAL_QUEUE_SIZE = 10 -SCAN_PERIOD_SECONDS = 5 - - -class SecurityType(IntEnum): - OPEN = 0 - WPA = 1 - WPA2 = 2 - WPA3 = 3 - UNSUPPORTED = 4 - - -class MeteredType(IntEnum): - UNKNOWN = 0 - YES = 1 - NO = 2 - - -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 - is_saved: bool - ip_address: str = "" # TODO: implement - - @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 - 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]) - - 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): - self._networks: list[Network] = [] # a network can be comprised of multiple APs - self._active = True # used to not run when not in settings - self._exit = False - - # DBus connections - try: - 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._router_main = None - self._conn_monitor = None - self._exit = True - - # Store wifi device path - self._wifi_device: str | None = None - - # State - self._connecting_to_ssid: str = "" - self._ipv4_address: str = "" - self._current_network_metered: MeteredType = MeteredType.UNKNOWN - self._tethering_password: str = "" - self._ipv4_forward = False - - self._last_network_update: float = 0.0 - self._callback_queue: list[Callable] = [] - - self._tethering_ssid = "weedle" - if Params is not None: - dongle_id = Params().get("DongleId") - if dongle_id: - self._tethering_ssid += "-" + dongle_id[:4] - - # Callbacks - self._need_auth: list[Callable[[str], None]] = [] - self._activated: list[Callable[[], None]] = [] - self._forgotten: list[Callable[[], None]] = [] - self._networks_updated: list[Callable[[list[Network]], None]] = [] - self._disconnected: list[Callable[[], None]] = [] - - self._lock = threading.Lock() - self._scan_thread = threading.Thread(target=self._network_scanner, daemon=True) - self._state_thread = threading.Thread(target=self._monitor_state, daemon=True) - self._initialize() - atexit.register(self.stop) - - def _initialize(self): - def worker(): - self._wait_for_wifi_device() - - self._scan_thread.start() - self._state_thread.start() - - if Params is not None and self._tethering_ssid not in self._get_connections(): - self._add_tethering_connection() - - self._tethering_password = self._get_tethering_password() - cloudlog.debug("WifiManager initialized") - - threading.Thread(target=worker, daemon=True).start() - - def add_callbacks(self, need_auth: Callable[[str], None] | None = None, - activated: Callable[[], None] | None = None, - forgotten: Callable[[], None] | None = None, - networks_updated: Callable[[list[Network]], None] | None = None, - disconnected: Callable[[], None] | None = None): - if need_auth is not None: - self._need_auth.append(need_auth) - if activated is not None: - self._activated.append(activated) - if forgotten is not None: - self._forgotten.append(forgotten) - if networks_updated is not None: - self._networks_updated.append(networks_updated) - if disconnected is not None: - self._disconnected.append(disconnected) - - @property - def ipv4_address(self) -> str: - return self._ipv4_address - - @property - def current_network_metered(self) -> MeteredType: - return self._current_network_metered - - @property - def tethering_password(self) -> str: - return self._tethering_password - - def _enqueue_callbacks(self, cbs: list[Callable], *args): - for cb in cbs: - self._callback_queue.append(lambda _cb=cb: _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): - rule = MatchRule( - type="signal", - interface=NM_DEVICE_IFACE, - member="StateChanged", - path=self._wifi_device, - ) - - # 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) - self._enqueue_callbacks(self._need_auth, self._connecting_to_ssid) - self._connecting_to_ssid = "" - - elif new_state == NMDeviceState.ACTIVATED: - if len(self._activated): - self._update_networks() - self._enqueue_callbacks(self._activated) - self._connecting_to_ssid = "" - - elif new_state == NMDeviceState.DISCONNECTED and change_reason != NM_DEVICE_STATE_REASON_NEW_ACTIVATION: - self._connecting_to_ssid = "" - self._enqueue_callbacks(self._forgotten) - - def _network_scanner(self): - 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): - while not self._exit: - device_path = self._get_adapter(NM_DEVICE_TYPE_WIFI) - if device_path is not None: - self._wifi_device = device_path - break - time.sleep(1) - - def _get_adapter(self, adapter_type: int) -> str | None: - # Return the first NetworkManager device path matching adapter_type - try: - 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] - if dev_type == adapter_type: - return str(device_path) - except Exception as e: - cloudlog.exception(f"Error getting adapter type {adapter_type}: {e}") - return None - - 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: - settings = self._get_connection_settings(conn_path) - - if len(settings) == 0: - cloudlog.warning(f'Failed to get connection settings for {conn_path}') - continue - - 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 _get_active_connections(self): - return self._router_main.send_and_get_reply(Properties(self._nm).get('ActiveConnections')).body[0][1] - - def _get_connection_settings(self, conn_path: str) -> dict: - 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')) - if reply.header.message_type == MessageType.error: - cloudlog.warning(f'Failed to get connection settings: {reply}') - return {} - return dict(reply.body[0]) - - def _add_tethering_connection(self): - connection = { - 'connection': { - 'type': ('s', '802-11-wireless'), - 'uuid': ('s', str(uuid.uuid4())), - 'id': ('s', 'Hotspot'), - 'autoconnect-retries': ('i', 0), - 'interface-name': ('s', 'wlan0'), - 'autoconnect': ('b', False), - }, - '802-11-wireless': { - 'band': ('s', 'bg'), - 'mode': ('s', 'ap'), - 'ssid': ('ay', self._tethering_ssid.encode("utf-8")), - }, - '802-11-wireless-security': { - 'group': ('as', ['ccmp']), - 'key-mgmt': ('s', 'wpa-psk'), - 'pairwise': ('as', ['ccmp']), - 'proto': ('as', ['rsn']), - 'psk': ('s', DEFAULT_TETHERING_PASSWORD), - }, - 'ipv4': { - 'method': ('s', 'shared'), - 'address-data': ('aa{sv}', [[ - ('address', ('s', TETHERING_IP_ADDRESS)), - ('prefix', ('u', 24)), - ]]), - 'gateway': ('s', TETHERING_IP_ADDRESS), - 'never-default': ('b', True), - }, - 'ipv6': {'method': ('s', 'ignore')}, - } - - 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,))) - - def connect_to_network(self, ssid: str, password: str, hidden: bool = False): - 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) - - connection = { - 'connection': { - 'type': ('s', '802-11-wireless'), - 'uuid': ('s', str(uuid.uuid4())), - 'id': ('s', f'openpilot connection {ssid}'), - 'autoconnect-retries': ('i', 0), - }, - '802-11-wireless': { - 'ssid': ('ay', ssid.encode("utf-8")), - 'hidden': ('b', hidden), - 'mode': ('s', 'infrastructure'), - }, - 'ipv4': { - 'method': ('s', 'auto'), - 'dns-priority': ('i', 600), - }, - 'ipv6': {'method': ('s', 'ignore')}, - } - - if password: - connection['802-11-wireless-security'] = { - 'key-mgmt': ('s', 'wpa-psk'), - 'auth-alg': ('s', 'open'), - 'psk': ('s', password), - } - - 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) - - threading.Thread(target=worker, daemon=True).start() - - 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')) - - if len(self._forgotten): - self._update_networks() - self._enqueue_callbacks(self._forgotten) - - if block: - worker() - else: - threading.Thread(target=worker, daemon=True).start() - - 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 - - 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 _deactivate_connection(self, ssid: str): - for conn_path in self._get_active_connections(): - conn_addr = DBusAddress(conn_path, bus_name=NM, interface=NM_ACTIVE_CONNECTION_IFACE) - specific_obj_path = self._router_main.send_and_get_reply(Properties(conn_addr).get('SpecificObject')).body[0][1] - - if specific_obj_path != "/": - ap_addr = DBusAddress(specific_obj_path, bus_name=NM, interface=NM_ACCESS_POINT_IFACE) - ap_ssid = bytes(self._router_main.send_and_get_reply(Properties(ap_addr).get('Ssid')).body[0][1]).decode("utf-8", "replace") - - if ap_ssid == ssid: - self._router_main.send_and_get_reply(new_method_call(self._nm, 'DeactivateConnection', 'o', (conn_path,))) - return - - def is_tethering_active(self) -> bool: - for network in self._networks: - if network.is_connected: - return bool(network.ssid == self._tethering_ssid) - return False - - def set_tethering_password(self, password: str): - def worker(): - conn_path = self._get_connections().get(self._tethering_ssid, None) - if conn_path is None: - cloudlog.warning('No tethering connection found') - return - - settings = self._get_connection_settings(conn_path) - if len(settings) == 0: - cloudlog.warning(f'Failed to get tethering settings for {conn_path}') - return - - settings['802-11-wireless-security']['psk'] = ('s', password) - - 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, 'Update', 'a{sa{sv}}', (settings,))) - if reply.header.message_type == MessageType.error: - cloudlog.warning(f'Failed to update tethering settings: {reply}') - return - - self._tethering_password = password - if self.is_tethering_active(): - self.activate_connection(self._tethering_ssid, block=True) - - threading.Thread(target=worker, daemon=True).start() - - def _get_tethering_password(self) -> str: - conn_path = self._get_connections().get(self._tethering_ssid, None) - if conn_path is None: - cloudlog.warning('No tethering connection found') - return '' - - reply = self._router_main.send_and_get_reply(new_method_call( - DBusAddress(conn_path, bus_name=NM, interface=NM_CONNECTION_IFACE), - 'GetSecrets', 's', ('802-11-wireless-security',) - )) - - if reply.header.message_type == MessageType.error: - cloudlog.warning(f'Failed to get tethering password: {reply}') - return '' - - secrets = reply.body[0] - if '802-11-wireless-security' not in secrets: - return '' - - return str(secrets['802-11-wireless-security'].get('psk', ('s', ''))[1]) - - def set_ipv4_forward(self, enabled: bool): - self._ipv4_forward = enabled - - def set_tethering_active(self, active: bool): - def worker(): - if active: - self.activate_connection(self._tethering_ssid, block=True) - - if not self._ipv4_forward: - time.sleep(5) - cloudlog.warning("net.ipv4.ip_forward = 0") - subprocess.run(["sudo", "sysctl", "net.ipv4.ip_forward=0"], check=False) - else: - self._deactivate_connection(self._tethering_ssid) - - threading.Thread(target=worker, daemon=True).start() - - def _update_current_network_metered(self) -> None: - if self._wifi_device is None: - cloudlog.warning("No WiFi device found") - return - - self._current_network_metered = MeteredType.UNKNOWN - for active_conn in self._get_active_connections(): - conn_addr = DBusAddress(active_conn, bus_name=NM, interface=NM_ACTIVE_CONNECTION_IFACE) - conn_type = self._router_main.send_and_get_reply(Properties(conn_addr).get('Type')).body[0][1] - - if conn_type == '802-11-wireless': - conn_path = self._router_main.send_and_get_reply(Properties(conn_addr).get('Connection')).body[0][1] - if conn_path == "/": - continue - - settings = self._get_connection_settings(conn_path) - - if len(settings) == 0: - cloudlog.warning(f'Failed to get connection settings for {conn_path}') - continue - - metered_prop = settings['connection'].get('metered', ('i', 0))[1] - if metered_prop == MeteredType.YES: - self._current_network_metered = MeteredType.YES - elif metered_prop == MeteredType.NO: - self._current_network_metered = MeteredType.NO - return - - def set_current_network_metered(self, metered: MeteredType): - def worker(): - for active_conn in self._get_active_connections(): - conn_addr = DBusAddress(active_conn, bus_name=NM, interface=NM_ACTIVE_CONNECTION_IFACE) - conn_type = self._router_main.send_and_get_reply(Properties(conn_addr).get('Type')).body[0][1] - - if conn_type == '802-11-wireless' and not self.is_tethering_active(): - conn_path = self._router_main.send_and_get_reply(Properties(conn_addr).get('Connection')).body[0][1] - if conn_path == "/": - continue - - settings = self._get_connection_settings(conn_path) - - if len(settings) == 0: - cloudlog.warning(f'Failed to get connection settings for {conn_path}') - return - - settings['connection']['metered'] = ('i', int(metered)) - - 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, 'Update', 'a{sa{sv}}', (settings,))) - if reply.header.message_type == MessageType.error: - cloudlog.warning(f'Failed to update tethering settings: {reply}') - return - - 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 - - try: - ap = AccessPoint.from_dbus(ap_props.body[0], ap_path, active_ap_path) - if ap.ssid == "": - continue - - if ap.ssid not in aps: - aps[ap.ssid] = [] - - aps[ap.ssid].append(ap) - except Exception: - # catch all for parsing errors - cloudlog.exception(f"Failed to parse AP properties for {ap_path}") - - known_connections = self._get_connections() - networks = [Network.from_dbus(ssid, ap_list, ssid in known_connections) for ssid, ap_list in aps.items()] - # sort with quantized strength to reduce jumping - networks.sort(key=lambda n: (-n.is_connected, -round(n.strength / 100 * 2), n.ssid.lower())) - self._networks = networks - - self._update_ipv4_address() - self._update_current_network_metered() - - self._enqueue_callbacks(self._networks_updated, self._networks) - - def _update_ipv4_address(self): - if self._wifi_device is None: - cloudlog.warning("No WiFi device found") - return - - self._ipv4_address = "" - - for conn_path in self._get_active_connections(): - conn_addr = DBusAddress(conn_path, bus_name=NM, interface=NM_ACTIVE_CONNECTION_IFACE) - conn_type = self._router_main.send_and_get_reply(Properties(conn_addr).get('Type')).body[0][1] - if conn_type == '802-11-wireless': - ip4config_path = self._router_main.send_and_get_reply(Properties(conn_addr).get('Ip4Config')).body[0][1] - - if ip4config_path != "/": - ip4config_addr = DBusAddress(ip4config_path, bus_name=NM, interface=NM_IP4_CONFIG_IFACE) - address_data = self._router_main.send_and_get_reply(Properties(ip4config_addr).get('AddressData')).body[0][1] - - for entry in address_data: - if 'address' in entry: - self._ipv4_address = entry['address'][1] - return - - def __del__(self): - self.stop() - - def update_gsm_settings(self, roaming: bool, apn: str, metered: bool): - """Update GSM settings for cellular connection""" - - def worker(): - try: - lte_connection_path = self._get_lte_connection_path() - if not lte_connection_path: - cloudlog.warning("No LTE connection found") - return - - settings = self._get_connection_settings(lte_connection_path) - - if len(settings) == 0: - cloudlog.warning(f"Failed to get connection settings for {lte_connection_path}") - return - - # Ensure dicts exist - if 'gsm' not in settings: - settings['gsm'] = {} - if 'connection' not in settings: - settings['connection'] = {} - - changes = False - auto_config = apn == "" - - if settings['gsm'].get('auto-config', ('b', False))[1] != auto_config: - cloudlog.warning(f'Changing gsm.auto-config to {auto_config}') - settings['gsm']['auto-config'] = ('b', auto_config) - changes = True - - if settings['gsm'].get('apn', ('s', ''))[1] != apn: - cloudlog.warning(f'Changing gsm.apn to {apn}') - settings['gsm']['apn'] = ('s', apn) - changes = True - - if settings['gsm'].get('home-only', ('b', False))[1] == roaming: - cloudlog.warning(f'Changing gsm.home-only to {not roaming}') - settings['gsm']['home-only'] = ('b', not roaming) - changes = True - - # Unknown means NetworkManager decides - metered_int = int(MeteredType.UNKNOWN if metered else MeteredType.NO) - if settings['connection'].get('metered', ('i', 0))[1] != metered_int: - cloudlog.warning(f'Changing connection.metered to {metered_int}') - settings['connection']['metered'] = ('i', metered_int) - changes = True - - if changes: - # Update the connection settings (temporary update) - conn_addr = DBusAddress(lte_connection_path, bus_name=NM, interface=NM_CONNECTION_IFACE) - reply = self._router_main.send_and_get_reply(new_method_call(conn_addr, 'UpdateUnsaved', 'a{sa{sv}}', (settings,))) - - if reply.header.message_type == MessageType.error: - cloudlog.warning(f"Failed to update GSM settings: {reply}") - return - - self._activate_modem_connection(lte_connection_path) - except Exception as e: - cloudlog.exception(f"Error updating GSM settings: {e}") - - threading.Thread(target=worker, daemon=True).start() - - def _get_lte_connection_path(self) -> str | None: - try: - 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] - - for conn_path in known_connections: - settings = self._get_connection_settings(conn_path) - if settings and settings.get('connection', {}).get('id', ('s', ''))[1] == 'lte': - return str(conn_path) - except Exception as e: - cloudlog.exception(f"Error finding LTE connection: {e}") - return None - - def _activate_modem_connection(self, connection_path: str): - try: - modem_device = self._get_adapter(NM_DEVICE_TYPE_MODEM) - if modem_device and connection_path: - self._router_main.send_and_get_reply(new_method_call(self._nm, 'ActivateConnection', 'ooo', (connection_path, modem_device, "/"))) - except Exception as e: - cloudlog.exception(f"Error activating modem connection: {e}") - - def stop(self): - if not self._exit: - self._exit = True - if self._scan_thread.is_alive(): - self._scan_thread.join() - if self._state_thread.is_alive(): - self._state_thread.join() - - if self._router_main is not None: - self._router_main.close() - self._router_main.conn.close() - if self._conn_monitor is not None: - self._conn_monitor.close() diff --git a/system/ui/lib/wrap_text.py b/system/ui/lib/wrap_text.py deleted file mode 100644 index 3fabfbb66bd385..00000000000000 --- a/system/ui/lib/wrap_text.py +++ /dev/null @@ -1,107 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.application import font_fallback - - -def _break_long_word(font: rl.Font, word: str, font_size: int, max_width: int, spacing: float = 0) -> list[str]: - if not word: - return [] - - parts = [] - remaining = word - - while remaining: - if measure_text_cached(font, remaining, font_size, spacing).x <= max_width: - parts.append(remaining) - break - - # Binary search for the longest substring that fits - left, right = 1, len(remaining) - best_fit = 1 - - while left <= right: - mid = (left + right) // 2 - substring = remaining[:mid] - width = measure_text_cached(font, substring, font_size, spacing).x - - if width <= max_width: - best_fit = mid - left = mid + 1 - else: - right = mid - 1 - - # Add the part that fits - parts.append(remaining[:best_fit]) - remaining = remaining[best_fit:] - - return parts - - -_cache: dict[int, list[str]] = {} - - -def wrap_text(font: rl.Font, text: str, font_size: int, max_width: int, spacing: float = 0) -> list[str]: - font = font_fallback(font) - spacing = round(spacing, 4) - key = hash((font.texture.id, text, font_size, max_width, spacing)) - if key in _cache: - return _cache[key] - - if not text or max_width <= 0: - return [] - - # Split text by newlines first to preserve explicit line breaks - paragraphs = text.split('\n') - all_lines: list[str] = [] - - for paragraph in paragraphs: - # Handle empty paragraphs (preserve empty lines) - if not paragraph.strip(): - all_lines.append("") - continue - - # Process each paragraph separately - words = paragraph.split() - if not words: - all_lines.append("") - continue - - lines: list[str] = [] - current_line: list[str] = [] - - for word in words: - word_width = measure_text_cached(font, word, font_size, spacing).x - - # Check if word alone exceeds max width (need to break the word) - if word_width > max_width: - # Finish current line if it has content - if current_line: - lines.append(" ".join(current_line)) - current_line = [] - - # Break the long word into parts - lines.extend(_break_long_word(font, word, font_size, max_width, spacing)) - continue - - # Measure the actual joined string to get accurate width (accounts for kerning, etc.) - test_line = " ".join(current_line + [word]) if current_line else word - test_width = measure_text_cached(font, test_line, font_size, spacing).x - - # Check if word fits on current line - if test_width <= max_width: - current_line.append(word) - else: - # Start new line with this word - if current_line: - lines.append(" ".join(current_line)) - current_line = [word] - - # Add remaining words - if current_line: - lines.append(" ".join(current_line)) - - # Add all lines from this paragraph - all_lines.extend(lines) - - _cache[key] = all_lines - return all_lines diff --git a/system/ui/mici_reset.py b/system/ui/mici_reset.py deleted file mode 100755 index 925afd7d10a9a6..00000000000000 --- a/system/ui/mici_reset.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import threading -import time -from enum import IntEnum - -import pyray as rl - -from openpilot.system.hardware import PC -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.slider import SmallSlider -from openpilot.system.ui.widgets.button import SmallButton, FullRoundedButton -from openpilot.system.ui.widgets.label import gui_label, gui_text_box - -USERDATA = "/dev/disk/by-partlabel/userdata" -TIMEOUT = 3*60 - - -class ResetMode(IntEnum): - USER_RESET = 0 # user initiated a factory reset from openpilot - RECOVER = 1 # userdata is corrupt for some reason, give a chance to recover - FORMAT = 2 # finish up a factory reset from a tool that doesn't flash an empty partition to userdata - - -class ResetState(IntEnum): - NONE = 0 - RESETTING = 1 - FAILED = 2 - - -class Reset(Widget): - def __init__(self, mode): - super().__init__() - self._mode = mode - self._previous_reset_state = None - self._reset_state = ResetState.NONE - - self._cancel_button = SmallButton("cancel") - self._cancel_button.set_click_callback(self._cancel_callback) - - self._reboot_button = FullRoundedButton("reboot") - self._reboot_button.set_click_callback(self._do_reboot) - - self._confirm_slider = SmallSlider("reset", self._confirm) - - self._render_status = True - - def _cancel_callback(self): - self._render_status = False - - def _do_reboot(self): - if PC: - return - - os.system("sudo reboot") - - def _do_erase(self): - if PC: - return - - # Removing data and formatting - rm = os.system("sudo rm -rf /data/*") - os.system(f"sudo umount {USERDATA}") - fmt = os.system(f"yes | sudo mkfs.ext4 {USERDATA}") - - if rm == 0 or fmt == 0: - os.system("sudo reboot") - else: - self._reset_state = ResetState.FAILED - - def start_reset(self): - self._reset_state = ResetState.RESETTING - threading.Timer(0.1, self._do_erase).start() - - def _update_state(self): - if self._reset_state != self._previous_reset_state: - self._previous_reset_state = self._reset_state - self._timeout_st = time.monotonic() - elif self._reset_state != ResetState.RESETTING and (time.monotonic() - self._timeout_st) > TIMEOUT: - exit(0) - - def _render(self, rect: rl.Rectangle): - label_rect = rl.Rectangle(rect.x + 8, rect.y + 8, rect.width, 50) - gui_label(label_rect, "factory reset", 48, font_weight=FontWeight.BOLD, - color=rl.Color(255, 255, 255, int(255 * 0.9))) - - text_rect = rl.Rectangle(rect.x + 8, rect.y + 56, rect.width - 8 * 2, rect.height - 80) - gui_text_box(text_rect, self._get_body_text(), 36, font_weight=FontWeight.ROMAN, line_scale=0.9) - - if self._reset_state != ResetState.RESETTING: - # fade out cancel button as slider is moved, set visible to prevent pressing invisible cancel - self._cancel_button.set_opacity(1.0 - self._confirm_slider.slider_percentage) - self._cancel_button.set_visible(self._confirm_slider.slider_percentage < 0.8) - - if self._mode == ResetMode.RECOVER: - self._cancel_button.set_text("reboot") - self._cancel_button.render(rl.Rectangle( - rect.x + 8, - rect.y + rect.height - self._cancel_button.rect.height, - self._cancel_button.rect.width, - self._cancel_button.rect.height)) - elif self._mode == ResetMode.USER_RESET and self._reset_state != ResetState.FAILED: - self._cancel_button.render(rl.Rectangle( - rect.x + 8, - rect.y + rect.height - self._cancel_button.rect.height, - self._cancel_button.rect.width, - self._cancel_button.rect.height)) - - if self._reset_state != ResetState.FAILED: - self._confirm_slider.render(rl.Rectangle( - rect.x + rect.width - self._confirm_slider.rect.width, - rect.y + rect.height - self._confirm_slider.rect.height, - self._confirm_slider.rect.width, - self._confirm_slider.rect.height)) - else: - self._reboot_button.render(rl.Rectangle( - rect.x + 8, - rect.y + rect.height - self._reboot_button.rect.height, - self._reboot_button.rect.width, - self._reboot_button.rect.height)) - - return self._render_status - - def _confirm(self): - self.start_reset() - - def _get_body_text(self): - if self._reset_state == ResetState.RESETTING: - return "Resetting device... This may take up to a minute." - if self._reset_state == ResetState.FAILED: - return "Reset failed. Reboot to try again." - if self._mode == ResetMode.RECOVER: - return "Unable to mount data partition. It may be corrupted." - return "All content and settings will be erased." - - -def main(): - mode = ResetMode.USER_RESET - if len(sys.argv) > 1: - if sys.argv[1] == '--recover': - mode = ResetMode.RECOVER - elif sys.argv[1] == "--format": - mode = ResetMode.FORMAT - - gui_app.init_window("System Reset") - reset = Reset(mode) - - if mode == ResetMode.FORMAT: - reset.start_reset() - - for should_render in gui_app.render(): - if should_render: - if not reset.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)): - break - - -if __name__ == "__main__": - main() diff --git a/system/ui/mici_setup.py b/system/ui/mici_setup.py deleted file mode 100755 index fac26f06eac718..00000000000000 --- a/system/ui/mici_setup.py +++ /dev/null @@ -1,756 +0,0 @@ -#!/usr/bin/env python3 -from abc import abstractmethod -import os -import re -import threading -import time -import urllib.request -import urllib.error -from urllib.parse import urlparse -from enum import IntEnum -import shutil -from collections.abc import Callable - -import pyray as rl - -from cereal import log -from openpilot.common.utils import run_cmd -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.wifi_manager import WifiManager -from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2 -from openpilot.system.ui.widgets import Widget, DialogResult -from openpilot.system.ui.widgets.button import (IconButton, SmallButton, WideRoundedButton, SmallerRoundedButton, - SmallCircleIconButton, WidishRoundedButton, SmallRedPillButton, - FullRoundedButton) -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.system.ui.widgets.slider import LargerSlider, SmallSlider -from openpilot.selfdrive.ui.mici.layouts.settings.network import WifiUIMici -from openpilot.selfdrive.ui.mici.widgets.dialog import BigInputDialog - -NetworkType = log.DeviceState.NetworkType - -OPENPILOT_URL = "https://openpilot.comma.ai" -USER_AGENT = f"AGNOSSetup-{HARDWARE.get_os_version()}" - -CONTINUE_PATH = "/data/continue.sh" -TMP_CONTINUE_PATH = "/data/continue.sh.new" -INSTALL_PATH = "/data/openpilot" -VALID_CACHE_PATH = "/data/.openpilot_cache" -INSTALLER_SOURCE_PATH = "/usr/comma/installer" -INSTALLER_DESTINATION_PATH = "/tmp/installer" -INSTALLER_URL_PATH = "/tmp/installer_url" - -CONTINUE = """#!/usr/bin/env bash - -cd /data/openpilot -exec ./launch_openpilot.sh -""" - - -class NetworkConnectivityMonitor: - def __init__(self, should_check: Callable[[], bool] | None = None, check_interval: float = 1.0): - self.network_connected = threading.Event() - self.wifi_connected = threading.Event() - self._should_check = should_check or (lambda: True) - self._check_interval = check_interval - self._stop_event = threading.Event() - self._thread: threading.Thread | None = None - - def start(self): - self._stop_event.clear() - if self._thread is None or not self._thread.is_alive(): - self._thread = threading.Thread(target=self._run, daemon=True) - self._thread.start() - - def stop(self): - if self._thread is not None: - self._stop_event.set() - self._thread.join() - self._thread = None - - def reset(self): - self.network_connected.clear() - self.wifi_connected.clear() - - def _run(self): - while not self._stop_event.is_set(): - if self._should_check(): - try: - request = urllib.request.Request(OPENPILOT_URL, method="HEAD") - urllib.request.urlopen(request, timeout=1.0) - self.network_connected.set() - if HARDWARE.get_network_type() == NetworkType.wifi: - self.wifi_connected.set() - except Exception: - self.reset() - else: - self.reset() - - if self._stop_event.wait(timeout=self._check_interval): - break - - -class SetupState(IntEnum): - GETTING_STARTED = 0 - NETWORK_SETUP = 1 - NETWORK_SETUP_CUSTOM_SOFTWARE = 8 - SOFTWARE_SELECTION = 2 - CUSTOM_SOFTWARE = 3 - DOWNLOADING = 4 - DOWNLOAD_FAILED = 5 - CUSTOM_SOFTWARE_WARNING = 6 - - -class StartPage(Widget): - def __init__(self): - super().__init__() - - self._title = UnifiedLabel("start", 64, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - self._start_bg_txt = gui_app.texture("icons_mici/setup/green_button.png", 520, 224) - self._start_bg_pressed_txt = gui_app.texture("icons_mici/setup/green_button_pressed.png", 520, 224) - - def _render(self, rect: rl.Rectangle): - draw_x = rect.x + (rect.width - self._start_bg_txt.width) / 2 - draw_y = rect.y + (rect.height - self._start_bg_txt.height) / 2 - texture = self._start_bg_pressed_txt if self.is_pressed else self._start_bg_txt - rl.draw_texture(texture, int(draw_x), int(draw_y), rl.WHITE) - - self._title.render(rect) - - -class SoftwareSelectionPage(Widget): - def __init__(self, use_openpilot_callback: Callable, - use_custom_software_callback: Callable): - super().__init__() - - self._openpilot_slider = LargerSlider("slide to use\nopenpilot", use_openpilot_callback) - self._custom_software_slider = LargerSlider("slide to use\ncustom software", use_custom_software_callback, green=False) - - def reset(self): - self._openpilot_slider.reset() - self._custom_software_slider.reset() - - def _render(self, rect: rl.Rectangle): - self._openpilot_slider.set_opacity(1.0 - self._custom_software_slider.slider_percentage) - self._custom_software_slider.set_opacity(1.0 - self._openpilot_slider.slider_percentage) - - openpilot_rect = rl.Rectangle( - rect.x + (rect.width - self._openpilot_slider.rect.width) / 2, - rect.y, - self._openpilot_slider.rect.width, - rect.height / 2, - ) - self._openpilot_slider.render(openpilot_rect) - - custom_software_rect = rl.Rectangle( - rect.x + (rect.width - self._custom_software_slider.rect.width) / 2, - rect.y + rect.height / 2, - self._custom_software_slider.rect.width, - rect.height / 2, - ) - self._custom_software_slider.render(custom_software_rect) - - -class TermsHeader(Widget): - def __init__(self, text: str, icon_texture: rl.Texture): - super().__init__() - - self._title = UnifiedLabel(text, 36, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.BOLD, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, - line_height=0.8) - self._icon_texture = icon_texture - - self.set_rect(rl.Rectangle(0, 0, gui_app.width - 16 * 2, self._icon_texture.height)) - - def set_title(self, text: str): - self._title.set_text(text) - - def set_icon(self, icon_texture: rl.Texture): - self._icon_texture = icon_texture - - def _render(self, _): - rl.draw_texture_ex(self._icon_texture, rl.Vector2(self._rect.x, self._rect.y), - 0.0, 1.0, rl.WHITE) - - # May expand outside parent rect - title_content_height = self._title.get_content_height(int(self._rect.width - self._icon_texture.width - 16)) - title_rect = rl.Rectangle( - self._rect.x + self._icon_texture.width + 16, - self._rect.y + (self._rect.height - title_content_height) / 2, - self._rect.width - self._icon_texture.width - 16, - title_content_height, - ) - self._title.render(title_rect) - - -class TermsPage(Widget): - ITEM_SPACING = 20 - - def __init__(self, continue_callback: Callable, back_callback: Callable | None = None, - back_text: str = "back", continue_text: str = "accept"): - super().__init__() - - # TODO: use Scroller - self._scroll_panel = GuiScrollPanel2(horizontal=False) - - self._continue_text = continue_text - self._continue_slider: bool = continue_text in ("reboot", "power off") - self._continue_button: WideRoundedButton | FullRoundedButton | SmallSlider - if self._continue_slider: - self._continue_button = SmallSlider(continue_text, confirm_callback=continue_callback) - self._scroll_panel.set_enabled(lambda: not self._continue_button.is_pressed) - elif back_callback is not None: - self._continue_button = WideRoundedButton(continue_text) - else: - self._continue_button = FullRoundedButton(continue_text) - self._continue_button.set_enabled(False) - self._continue_button.set_opacity(0.0) - self._continue_button.set_touch_valid_callback(self._scroll_panel.is_touch_valid) - if not self._continue_slider: - self._continue_button.set_click_callback(continue_callback) - - self._enable_back = back_callback is not None - self._back_button = SmallButton(back_text) - self._back_button.set_opacity(0.0) - self._back_button.set_touch_valid_callback(self._scroll_panel.is_touch_valid) - self._back_button.set_click_callback(back_callback) - - self._scroll_down_indicator = IconButton(gui_app.texture("icons_mici/setup/scroll_down_indicator.png", 64, 78)) - self._scroll_down_indicator.set_enabled(False) - - def reset(self): - self._scroll_panel.set_offset(0) - self._continue_button.set_enabled(False) - self._continue_button.set_opacity(0.0) - self._back_button.set_enabled(False) - self._back_button.set_opacity(0.0) - self._scroll_down_indicator.set_opacity(1.0) - - def show_event(self): - super().show_event() - self.reset() - - @property - @abstractmethod - def _content_height(self): - pass - - @property - def _scrolled_down_offset(self): - return -self._content_height + (self._continue_button.rect.height + 16 + 30) - - @abstractmethod - def _render_content(self, scroll_offset): - pass - - def _render(self, _): - scroll_offset = round(self._scroll_panel.update(self._rect, self._content_height + self._continue_button.rect.height + 16)) - - if scroll_offset <= self._scrolled_down_offset: - # don't show back if not enabled - if self._enable_back: - self._back_button.set_enabled(True) - self._back_button.set_opacity(1.0, smooth=True) - self._continue_button.set_enabled(True) - self._continue_button.set_opacity(1.0, smooth=True) - self._scroll_down_indicator.set_opacity(0.0, smooth=True) - else: - self._back_button.set_enabled(False) - self._back_button.set_opacity(0.0, smooth=True) - self._continue_button.set_enabled(False) - self._continue_button.set_opacity(0.0, smooth=True) - self._scroll_down_indicator.set_opacity(1.0, smooth=True) - - # Render content - self._render_content(scroll_offset) - - # black gradient at top and bottom for scrolling content - rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y), - int(self._rect.width), 20, rl.BLACK, rl.BLANK) - rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y + self._rect.height - 20), - int(self._rect.width), 20, rl.BLANK, rl.BLACK) - - # fade out back button as slider is moved - if self._continue_slider and scroll_offset <= self._scrolled_down_offset: - self._back_button.set_opacity(1.0 - self._continue_button.slider_percentage) - self._back_button.set_visible(self._continue_button.slider_percentage < 0.99) - - self._back_button.render(rl.Rectangle( - self._rect.x + 8, - self._rect.y + self._rect.height - self._back_button.rect.height, - self._back_button.rect.width, - self._back_button.rect.height, - )) - - continue_x = self._rect.x + 8 - if self._enable_back: - continue_x = self._rect.x + self._rect.width - self._continue_button.rect.width - 8 - if self._continue_slider: - continue_x += 8 - self._continue_button.render(rl.Rectangle( - continue_x, - self._rect.y + self._rect.height - self._continue_button.rect.height, - self._continue_button.rect.width, - self._continue_button.rect.height, - )) - - self._scroll_down_indicator.render(rl.Rectangle( - self._rect.x + self._rect.width - self._scroll_down_indicator.rect.width - 8, - self._rect.y + self._rect.height - self._scroll_down_indicator.rect.height - 8, - self._scroll_down_indicator.rect.width, - self._scroll_down_indicator.rect.height, - )) - - -class CustomSoftwareWarningPage(TermsPage): - def __init__(self, continue_callback: Callable, back_callback: Callable): - super().__init__(continue_callback, back_callback) - - self._title_header = TermsHeader("use caution installing\n3rd party software", - gui_app.texture("icons_mici/setup/warning.png", 66, 60)) - self._body = UnifiedLabel("• It has not been tested by comma.\n" + - "• It may not comply with relevant safety standards.\n" + - "• It may cause damage to your device and/or vehicle.\n", 36, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.ROMAN) - - self._restore_header = TermsHeader("how to backup &\nrestore", gui_app.texture("icons_mici/setup/restore.png", 60, 60)) - self._restore_body = UnifiedLabel("To restore your device to a factory state later, use https://flash.comma.ai", - 36, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.ROMAN) - - @property - def _content_height(self): - return self._restore_body.rect.y + self._restore_body.rect.height - self._scroll_panel.get_offset() - - def _render_content(self, scroll_offset): - self._title_header.set_position(self._rect.x + 16, self._rect.y + 8 + scroll_offset) - self._title_header.render() - - body_rect = rl.Rectangle( - self._rect.x + 8, - self._title_header.rect.y + self._title_header.rect.height + self.ITEM_SPACING, - self._rect.width - 50, - self._body.get_content_height(int(self._rect.width - 50)), - ) - self._body.render(body_rect) - - self._restore_header.set_position(self._rect.x + 16, self._body.rect.y + self._body.rect.height + self.ITEM_SPACING) - self._restore_header.render() - - self._restore_body.render(rl.Rectangle( - self._rect.x + 8, - self._restore_header.rect.y + self._restore_header.rect.height + self.ITEM_SPACING, - self._rect.width - 50, - self._restore_body.get_content_height(int(self._rect.width - 50)), - )) - - -class DownloadingPage(Widget): - def __init__(self): - super().__init__() - - self._title_label = UnifiedLabel("downloading", 64, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.DISPLAY) - self._progress_label = UnifiedLabel("", 128, text_color=rl.Color(255, 255, 255, int(255 * 0.9 * 0.35)), - font_weight=FontWeight.ROMAN, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM) - self._progress = 0 - - def set_progress(self, progress: int): - self._progress = progress - self._progress_label.set_text(f"{progress}%") - - def _render(self, rect: rl.Rectangle): - self._title_label.render(rl.Rectangle( - rect.x + 20, - rect.y + 10, - rect.width, - 64, - )) - - self._progress_label.render(rl.Rectangle( - rect.x + 20, - rect.y + 20, - rect.width, - rect.height, - )) - - -class FailedPage(Widget): - def __init__(self, reboot_callback: Callable, retry_callback: Callable, title: str = "download failed"): - super().__init__() - - self._title_label = UnifiedLabel(title, 64, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.DISPLAY) - self._reason_label = UnifiedLabel("", 36, text_color=rl.Color(255, 255, 255, int(255 * 0.9 * 0.65)), - font_weight=FontWeight.ROMAN) - - self._reboot_button = SmallRedPillButton("reboot") - self._reboot_button.set_click_callback(reboot_callback) - - self._retry_button = WideRoundedButton("retry") - self._retry_button.set_click_callback(retry_callback) - - def set_reason(self, reason: str): - self._reason_label.set_text(reason) - - def _render(self, rect: rl.Rectangle): - self._title_label.render(rl.Rectangle( - rect.x + 8, - rect.y + 10, - rect.width, - 64, - )) - - self._reason_label.render(rl.Rectangle( - rect.x + 8, - rect.y + 10 + 64, - rect.width, - 36, - )) - - self._reboot_button.render(rl.Rectangle( - rect.x + 8, - rect.y + rect.height - self._reboot_button.rect.height, - self._reboot_button.rect.width, - self._reboot_button.rect.height, - )) - - self._retry_button.render(rl.Rectangle( - rect.x + 8 + self._reboot_button.rect.width + 8, - rect.y + rect.height - self._retry_button.rect.height, - self._retry_button.rect.width, - self._retry_button.rect.height, - )) - - -class NetworkSetupState(IntEnum): - MAIN = 0 - WIFI_PANEL = 1 - - -class NetworkSetupPage(Widget): - def __init__(self, wifi_manager, continue_callback: Callable, back_callback: Callable): - super().__init__() - self._wifi_ui = WifiUIMici(wifi_manager, back_callback=lambda: self.set_state(NetworkSetupState.MAIN)) - - self._no_wifi_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_slash.png", 58, 50) - self._wifi_full_txt = gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 58, 50) - self._waiting_text = "waiting for internet..." - self._network_header = TermsHeader(self._waiting_text, self._no_wifi_txt) - - back_txt = gui_app.texture("icons_mici/setup/back_new.png", 37, 32) - self._back_button = SmallCircleIconButton(back_txt) - self._back_button.set_click_callback(back_callback) - - self._wifi_button = SmallerRoundedButton("wifi") - self._wifi_button.set_click_callback(lambda: self.set_state(NetworkSetupState.WIFI_PANEL)) - - self._continue_button = WidishRoundedButton("continue") - self._continue_button.set_enabled(False) - self._continue_button.set_click_callback(continue_callback) - - self._state = NetworkSetupState.MAIN - self._prev_has_internet = False - - def set_state(self, state: NetworkSetupState): - self._state = state - if state == NetworkSetupState.WIFI_PANEL: - self._wifi_ui.show_event() - - def set_has_internet(self, has_internet: bool): - if has_internet: - self._network_header.set_title("connected to internet") - self._network_header.set_icon(self._wifi_full_txt) - self._continue_button.set_enabled(True) - else: - self._network_header.set_title(self._waiting_text) - self._network_header.set_icon(self._no_wifi_txt) - self._continue_button.set_enabled(False) - - if has_internet and not self._prev_has_internet: - self.set_state(NetworkSetupState.MAIN) - self._prev_has_internet = has_internet - - def show_event(self): - super().show_event() - self._state = NetworkSetupState.MAIN - self._wifi_ui.show_event() - - def hide_event(self): - super().hide_event() - self._wifi_ui.hide_event() - - def _render(self, _): - if self._state == NetworkSetupState.MAIN: - self._network_header.render(rl.Rectangle( - self._rect.x + 16, - self._rect.y + 16, - self._rect.width - 32, - self._network_header.rect.height, - )) - - self._back_button.render(rl.Rectangle( - self._rect.x + 8, - self._rect.y + self._rect.height - self._back_button.rect.height, - self._back_button.rect.width, - self._back_button.rect.height, - )) - - self._wifi_button.render(rl.Rectangle( - self._rect.x + 8 + self._back_button.rect.width + 10, - self._rect.y + self._rect.height - self._wifi_button.rect.height, - self._wifi_button.rect.width, - self._wifi_button.rect.height, - )) - - self._continue_button.render(rl.Rectangle( - self._rect.x + self._rect.width - self._continue_button.rect.width - 8, - self._rect.y + self._rect.height - self._continue_button.rect.height, - self._continue_button.rect.width, - self._continue_button.rect.height, - )) - else: - self._wifi_ui.render(self._rect) - - -class Setup(Widget): - def __init__(self): - super().__init__() - self.state = SetupState.GETTING_STARTED - self.failed_url = "" - self.failed_reason = "" - self.download_url = "" - self.download_progress = 0 - self.download_thread = None - self._wifi_manager = WifiManager() - self._wifi_manager.set_active(True) - self._network_monitor = NetworkConnectivityMonitor() - self._network_monitor.start() - self._prev_has_internet = False - gui_app.set_modal_overlay_tick(self._modal_overlay_tick) - - self._start_page = StartPage() - self._start_page.set_click_callback(self._getting_started_button_callback) - - self._network_setup_page = NetworkSetupPage(self._wifi_manager, self._network_setup_continue_button_callback, - self._network_setup_back_button_callback) - - self._software_selection_page = SoftwareSelectionPage(self._software_selection_continue_button_callback, - self._software_selection_custom_software_button_callback) - - self._download_failed_page = FailedPage(HARDWARE.reboot, self._download_failed_startover_button_callback) - - self._custom_software_warning_page = CustomSoftwareWarningPage(self._software_selection_custom_software_continue, - self._custom_software_warning_back_button_callback) - - self._downloading_page = DownloadingPage() - - def _modal_overlay_tick(self): - has_internet = self._network_monitor.network_connected.is_set() - if has_internet and not self._prev_has_internet: - gui_app.set_modal_overlay(None) - self._prev_has_internet = has_internet - - def _update_state(self): - self._wifi_manager.process_callbacks() - - def _set_state(self, state: SetupState): - self.state = state - if self.state == SetupState.SOFTWARE_SELECTION: - self._software_selection_page.reset() - elif self.state == SetupState.CUSTOM_SOFTWARE_WARNING: - self._custom_software_warning_page.reset() - - if self.state in (SetupState.NETWORK_SETUP, SetupState.NETWORK_SETUP_CUSTOM_SOFTWARE): - self._network_setup_page.show_event() - self._network_monitor.reset() - else: - self._network_setup_page.hide_event() - - def _render(self, rect: rl.Rectangle): - if self.state == SetupState.GETTING_STARTED: - self._start_page.render(rect) - elif self.state in (SetupState.NETWORK_SETUP, SetupState.NETWORK_SETUP_CUSTOM_SOFTWARE): - self.render_network_setup(rect) - elif self.state == SetupState.SOFTWARE_SELECTION: - self._software_selection_page.render(rect) - elif self.state == SetupState.CUSTOM_SOFTWARE_WARNING: - self._custom_software_warning_page.render(rect) - elif self.state == SetupState.CUSTOM_SOFTWARE: - self.render_custom_software() - elif self.state == SetupState.DOWNLOADING: - self.render_downloading(rect) - elif self.state == SetupState.DOWNLOAD_FAILED: - self._download_failed_page.render(rect) - - def _custom_software_warning_back_button_callback(self): - self._set_state(SetupState.SOFTWARE_SELECTION) - - def _custom_software_warning_continue_button_callback(self): - self._set_state(SetupState.CUSTOM_SOFTWARE) - - def _getting_started_button_callback(self): - self._set_state(SetupState.SOFTWARE_SELECTION) - - def _software_selection_back_button_callback(self): - self._set_state(SetupState.GETTING_STARTED) - - def _software_selection_continue_button_callback(self): - self.use_openpilot() - - def _software_selection_custom_software_button_callback(self): - self._set_state(SetupState.CUSTOM_SOFTWARE_WARNING) - - def _software_selection_custom_software_continue(self): - self._set_state(SetupState.NETWORK_SETUP_CUSTOM_SOFTWARE) - - def _download_failed_startover_button_callback(self): - self._set_state(SetupState.GETTING_STARTED) - - def _network_setup_back_button_callback(self): - self._set_state(SetupState.SOFTWARE_SELECTION) - - def _network_setup_continue_button_callback(self): - if self.state == SetupState.NETWORK_SETUP: - self.download(OPENPILOT_URL) - elif self.state == SetupState.NETWORK_SETUP_CUSTOM_SOFTWARE: - self._set_state(SetupState.CUSTOM_SOFTWARE) - - def close(self): - self._network_monitor.stop() - - def render_network_setup(self, rect: rl.Rectangle): - has_internet = self._network_monitor.network_connected.is_set() - self._prev_has_internet = has_internet - self._network_setup_page.set_has_internet(has_internet) - self._network_setup_page.render(rect) - - def render_downloading(self, rect: rl.Rectangle): - self._downloading_page.set_progress(self.download_progress) - self._downloading_page.render(rect) - - def render_custom_software(self): - def handle_keyboard_result(text): - url = text.strip() - if url: - self.download(url) - - def handle_keyboard_exit(result): - if result == DialogResult.CANCEL: - self._set_state(SetupState.SOFTWARE_SELECTION) - - keyboard = BigInputDialog("custom software URL", confirm_callback=handle_keyboard_result) - gui_app.set_modal_overlay(keyboard, callback=handle_keyboard_exit) - - def use_openpilot(self): - if os.path.isdir(INSTALL_PATH) and os.path.isfile(VALID_CACHE_PATH): - os.remove(VALID_CACHE_PATH) - with open(TMP_CONTINUE_PATH, "w") as f: - f.write(CONTINUE) - run_cmd(["chmod", "+x", TMP_CONTINUE_PATH]) - shutil.move(TMP_CONTINUE_PATH, CONTINUE_PATH) - shutil.copyfile(INSTALLER_SOURCE_PATH, INSTALLER_DESTINATION_PATH) - - # give time for installer UI to take over - time.sleep(0.1) - gui_app.request_close() - else: - self._set_state(SetupState.NETWORK_SETUP) - - def download(self, url: str): - # autocomplete incomplete URLs - if re.match("^([^/.]+)/([^/]+)$", url): - url = f"https://installer.comma.ai/{url}" - - parsed = urlparse(url, scheme='https') - self.download_url = (urlparse(f"https://{url}") if not parsed.netloc else parsed).geturl() - - self._set_state(SetupState.DOWNLOADING) - - self.download_thread = threading.Thread(target=self._download_thread, daemon=True) - self.download_thread.start() - - def _download_thread(self): - try: - import tempfile - - fd, tmpfile = tempfile.mkstemp(prefix="installer_") - - headers = {"User-Agent": USER_AGENT, - "X-openpilot-serial": HARDWARE.get_serial(), - "X-openpilot-device-type": HARDWARE.get_device_type()} - req = urllib.request.Request(self.download_url, headers=headers) - - with open(tmpfile, 'wb') as f, urllib.request.urlopen(req, timeout=30) as response: - total_size = int(response.headers.get('content-length', 0)) - downloaded = 0 - block_size = 8192 - - while True: - buffer = response.read(block_size) - if not buffer: - break - - downloaded += len(buffer) - f.write(buffer) - - if total_size: - self.download_progress = int(downloaded * 100 / total_size) - self._downloading_page.set_progress(self.download_progress) - - is_elf = False - with open(tmpfile, 'rb') as f: - header = f.read(4) - is_elf = header == b'\x7fELF' - - if not is_elf: - self.download_failed(self.download_url, "No custom software found at this URL.") - return - - # AGNOS might try to execute the installer before this process exits. - # Therefore, important to close the fd before renaming the installer. - os.close(fd) - os.rename(tmpfile, INSTALLER_DESTINATION_PATH) - - with open(INSTALLER_URL_PATH, "w") as f: - f.write(self.download_url) - - # give time for installer UI to take over - time.sleep(0.1) - gui_app.request_close() - - except urllib.error.HTTPError as e: - if e.code == 409: - error_msg = "Incompatible openpilot version" - self.download_failed(self.download_url, error_msg) - except Exception: - error_msg = "Invalid URL" - self.download_failed(self.download_url, error_msg) - - def download_failed(self, url: str, reason: str): - self.failed_url = url - self.failed_reason = reason - self._download_failed_page.set_reason(reason) - self._set_state(SetupState.DOWNLOAD_FAILED) - - -def main(): - try: - gui_app.init_window("Setup") - setup = Setup() - for should_render in gui_app.render(): - if should_render: - setup.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - setup.close() - except Exception as e: - print(f"Setup error: {e}") - finally: - gui_app.close() - - -if __name__ == "__main__": - main() diff --git a/system/ui/mici_updater.py b/system/ui/mici_updater.py deleted file mode 100755 index 7ebb4262ff9501..00000000000000 --- a/system/ui/mici_updater.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python3 -import sys -import subprocess -import threading -import pyray as rl -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.text_measure import measure_text_cached -from openpilot.system.ui.lib.wifi_manager import WifiManager, Network -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import gui_text_box, gui_label, UnifiedLabel -from openpilot.system.ui.widgets.button import FullRoundedButton -from openpilot.system.ui.mici_setup import NetworkSetupPage, FailedPage, NetworkConnectivityMonitor - - -class Screen(IntEnum): - PROMPT = 0 - WIFI = 1 - PROGRESS = 2 - FAILED = 3 - - -class Updater(Widget): - def __init__(self, updater_path, manifest_path): - super().__init__() - self.updater = updater_path - self.manifest = manifest_path - self.current_screen = Screen.PROMPT - self._current_network_strength = -1 - - self.progress_value = 0 - self.progress_text = "loading" - self.process = None - self.update_thread = None - self._wifi_manager = WifiManager() - self._wifi_manager.set_active(True) - - self._network_setup_page = NetworkSetupPage(self._wifi_manager, self._network_setup_continue_callback, - self._network_setup_back_callback) - - self._wifi_manager.add_callbacks(networks_updated=self._on_network_updated) - self._network_monitor = NetworkConnectivityMonitor() - self._network_monitor.start() - - # Buttons - self._continue_button = FullRoundedButton("continue") - self._continue_button.set_click_callback(lambda: self.set_current_screen(Screen.WIFI)) - - self._title_label = UnifiedLabel("update required", 48, text_color=rl.Color(255, 115, 0, 255), - font_weight=FontWeight.DISPLAY) - self._subtitle_label = UnifiedLabel("The download size is approximately 1GB.", 36, - text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - font_weight=FontWeight.ROMAN) - - self._update_failed_page = FailedPage(HARDWARE.reboot, self._update_failed_retry_callback, - title="update failed") - - def _network_setup_back_callback(self): - self.set_current_screen(Screen.PROMPT) - - def _network_setup_continue_callback(self): - self.install_update() - - def _update_failed_retry_callback(self): - self.set_current_screen(Screen.PROMPT) - - def _on_network_updated(self, networks: list[Network]): - self._current_network_strength = next((net.strength for net in networks if net.is_connected), -1) - - def set_current_screen(self, screen: Screen): - if self.current_screen != screen: - if screen == Screen.PROGRESS: - if self._network_setup_page: - self._network_setup_page.hide_event() - elif screen == Screen.WIFI: - if self._network_setup_page: - self._network_setup_page.show_event() - elif screen == Screen.PROMPT: - if self._network_setup_page: - self._network_setup_page.hide_event() - elif screen == Screen.FAILED: - if self._network_setup_page: - self._network_setup_page.hide_event() - - self.current_screen = screen - - def install_update(self): - self.set_current_screen(Screen.PROGRESS) - self.progress_value = 0 - self.progress_text = "downloading" - - # Start the update process in a separate thread - self.update_thread = threading.Thread(target=self._run_update_process) - self.update_thread.daemon = True - self.update_thread.start() - - def _run_update_process(self): - # TODO: just import it and run in a thread without a subprocess - cmd = [self.updater, "--swap", self.manifest] - self.process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, - text=True, bufsize=1, universal_newlines=True) - - if self.process.stdout is not None: - for line in self.process.stdout: - parts = line.strip().split(":") - if len(parts) == 2: - self.progress_text = parts[0].lower() - try: - self.progress_value = int(float(parts[1])) - except ValueError: - pass - - exit_code = self.process.wait() - if exit_code == 0: - HARDWARE.reboot() - else: - self.set_current_screen(Screen.FAILED) - - def render_prompt_screen(self, rect: rl.Rectangle): - self._title_label.render(rl.Rectangle( - rect.x + 8, - rect.y - 5, - rect.width, - 48, - )) - - subtitle_width = rect.width - 16 - subtitle_height = self._subtitle_label.get_content_height(int(subtitle_width)) - self._subtitle_label.render(rl.Rectangle( - rect.x + 8, - rect.y + 48, - subtitle_width, - subtitle_height, - )) - - self._continue_button.render(rl.Rectangle( - rect.x + 8, - rect.y + rect.height - self._continue_button.rect.height, - self._continue_button.rect.width, - self._continue_button.rect.height, - )) - - def render_progress_screen(self, rect: rl.Rectangle): - title_rect = rl.Rectangle(self._rect.x + 6, self._rect.y - 5, self._rect.width - 12, self._rect.height - 8) - if ' ' in self.progress_text: - font_size = 62 - else: - font_size = 82 - gui_text_box(title_rect, self.progress_text, font_size, font_weight=FontWeight.DISPLAY, - color=rl.Color(255, 255, 255, int(255 * 0.9))) - - progress_value = f"{self.progress_value}%" - text_height = measure_text_cached(gui_app.font(FontWeight.ROMAN), progress_value, 128).y - progress_rect = rl.Rectangle(self._rect.x + 6, self._rect.y + self._rect.height - text_height + 18, - self._rect.width - 12, text_height) - gui_label(progress_rect, progress_value, 128, font_weight=FontWeight.ROMAN, - color=rl.Color(255, 255, 255, int(255 * 0.9 * 0.35))) - - def _update_state(self): - self._wifi_manager.process_callbacks() - - def _render(self, rect: rl.Rectangle): - if self.current_screen == Screen.PROMPT: - self.render_prompt_screen(rect) - elif self.current_screen == Screen.WIFI: - self._network_setup_page.set_has_internet(self._network_monitor.network_connected.is_set()) - self._network_setup_page.render(rect) - elif self.current_screen == Screen.PROGRESS: - self.render_progress_screen(rect) - elif self.current_screen == Screen.FAILED: - self._update_failed_page.render(rect) - - def close(self): - self._network_monitor.stop() - - -def main(): - if len(sys.argv) < 3: - print("Usage: updater.py ") - sys.exit(1) - - updater_path = sys.argv[1] - manifest_path = sys.argv[2] - - try: - gui_app.init_window("System Update") - updater = Updater(updater_path, manifest_path) - for should_render in gui_app.render(): - if should_render: - updater.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - updater.close() - except Exception as e: - print(f"Updater error: {e}") - finally: - gui_app.close() - - -if __name__ == "__main__": - main() diff --git a/system/ui/reset.py b/system/ui/reset.py deleted file mode 100755 index c32504a5b8451b..00000000000000 --- a/system/ui/reset.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python3 -from openpilot.system.ui.lib.application import gui_app -import openpilot.system.ui.tici_reset as tici_reset -import openpilot.system.ui.mici_reset as mici_reset - - -def main(): - if gui_app.big_ui(): - tici_reset.main() - else: - mici_reset.main() - - -if __name__ == "__main__": - main() diff --git a/system/ui/setup.py b/system/ui/setup.py deleted file mode 100755 index 23ffc26aa2b765..00000000000000 --- a/system/ui/setup.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python3 -from openpilot.system.ui.lib.application import gui_app -import openpilot.system.ui.tici_setup as tici_setup -import openpilot.system.ui.mici_setup as mici_setup - - -def main(): - if gui_app.big_ui(): - tici_setup.main() - else: - mici_setup.main() - - -if __name__ == "__main__": - main() diff --git a/system/ui/spinner.py b/system/ui/spinner.py deleted file mode 100755 index 2a48b3889b90c5..00000000000000 --- a/system/ui/spinner.py +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env python3 -import pyray as rl -import select -import sys - -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.text import wrap_text -from openpilot.system.ui.widgets import Widget - -# Constants -if gui_app.big_ui(): - PROGRESS_BAR_WIDTH = 1000 - PROGRESS_BAR_HEIGHT = 20 - TEXTURE_SIZE = 360 - WRAPPED_SPACING = 50 - CENTERED_SPACING = 150 -else: - PROGRESS_BAR_WIDTH = 268 - PROGRESS_BAR_HEIGHT = 10 - TEXTURE_SIZE = 140 - WRAPPED_SPACING = 10 - CENTERED_SPACING = 20 -DEGREES_PER_SECOND = 360.0 # one full rotation per second -MARGIN_H = 100 -FONT_SIZE = 96 -LINE_HEIGHT = 104 -DARKGRAY = (55, 55, 55, 255) - - -def clamp(value, min_value, max_value): - return max(min(value, max_value), min_value) - - -class Spinner(Widget): - def __init__(self): - super().__init__() - self._comma_texture = gui_app.texture("images/spinner_comma.png", TEXTURE_SIZE, TEXTURE_SIZE) - self._spinner_texture = gui_app.texture("images/spinner_track.png", TEXTURE_SIZE, TEXTURE_SIZE, alpha_premultiply=True) - self._rotation = 0.0 - self._progress: int | None = None - self._wrapped_lines: list[str] = [] - - def set_text(self, text: str) -> None: - if text.isdigit(): - self._progress = clamp(int(text), 0, 100) - self._wrapped_lines = [] - else: - self._progress = None - self._wrapped_lines = wrap_text(text, FONT_SIZE, gui_app.width - MARGIN_H) - - def _render(self, rect: rl.Rectangle): - if self._wrapped_lines: - # Calculate total height required for spinner and text - spacing = WRAPPED_SPACING - total_height = TEXTURE_SIZE + spacing + len(self._wrapped_lines) * LINE_HEIGHT - center_y = (rect.height - total_height) / 2.0 + TEXTURE_SIZE / 2.0 - else: - # Center spinner vertically - spacing = CENTERED_SPACING - center_y = rect.height / 2.0 - y_pos = center_y + TEXTURE_SIZE / 2.0 + spacing - - center = rl.Vector2(rect.width / 2.0, center_y) - spinner_origin = rl.Vector2(TEXTURE_SIZE / 2.0, TEXTURE_SIZE / 2.0) - comma_position = rl.Vector2(center.x - TEXTURE_SIZE / 2.0, center.y - TEXTURE_SIZE / 2.0) - - delta_time = rl.get_frame_time() - self._rotation = (self._rotation + DEGREES_PER_SECOND * delta_time) % 360.0 - - # Draw rotating spinner and static comma logo - rl.draw_texture_pro(self._spinner_texture, rl.Rectangle(0, 0, TEXTURE_SIZE, TEXTURE_SIZE), - rl.Rectangle(center.x, center.y, TEXTURE_SIZE, TEXTURE_SIZE), - spinner_origin, self._rotation, rl.WHITE) - rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) - - # Display the progress bar or text based on user input - if self._progress is not None: - bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) - rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) - - bar.width *= self._progress / 100.0 - rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) - elif self._wrapped_lines: - for i, line in enumerate(self._wrapped_lines): - text_size = measure_text_cached(gui_app.font(), line, FONT_SIZE) - rl.draw_text_ex(gui_app.font(), line, rl.Vector2(center.x - text_size.x / 2, y_pos + i * LINE_HEIGHT), - FONT_SIZE, 0.0, rl.WHITE) - - -def _read_stdin(): - """Non-blocking read of available lines from stdin.""" - lines = [] - while True: - rlist, _, _ = select.select([sys.stdin], [], [], 0.0) - if not rlist: - break - line = sys.stdin.readline().strip() - if line == "": - break - lines.append(line) - return lines - - -def main(): - gui_app.init_window("Spinner") - spinner = Spinner() - for _ in gui_app.render(): - text_list = _read_stdin() - if text_list: - spinner.set_text(text_list[-1]) - - spinner.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - - -if __name__ == "__main__": - main() diff --git a/system/ui/text.py b/system/ui/text.py deleted file mode 100755 index 17e8a507cb0b2a..00000000000000 --- a/system/ui/text.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 -import re -import sys -import pyray as rl -from openpilot.system.hardware import HARDWARE, PC -from openpilot.system.ui.lib.application import BIG_UI, gui_app -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import Button, ButtonStyle - -if BIG_UI: - MARGIN = 50 - SPACING = 40 - FONT_SIZE = 72 - LINE_HEIGHT = 80 - BUTTON_SIZE = rl.Vector2(310, 160) -else: - MARGIN = 20 - SPACING = 30 - FONT_SIZE = 25 - LINE_HEIGHT = 25 - BUTTON_SIZE = rl.Vector2(150, 80) - -DEMO_TEXT = """This is a sample text that will be wrapped and scrolled if necessary. - The text is long enough to demonstrate scrolling and word wrapping.""" * 30 - - -def wrap_text(text, font_size, max_width): - lines = [] - font = gui_app.font() - - for paragraph in text.split("\n"): - if not paragraph.strip(): - # Don't add empty lines first, ensuring wrap_text("") returns [] - if lines: - lines.append("") - continue - indent = re.match(r"^\s*", paragraph).group() - current_line = indent - words = re.split(r"(\s+|-)", paragraph[len(indent):]) - while len(words): - word = words.pop(0) - test_line = current_line + word + (words.pop(0) if words else "") - if measure_text_cached(font, test_line, font_size).x <= max_width: - current_line = test_line - else: - lines.append(current_line) - current_line = word + " " - current_line = current_line.rstrip() - if current_line: - lines.append(current_line) - - return lines - - -class TextWindow(Widget): - def __init__(self, text: str): - super().__init__() - self._textarea_rect = rl.Rectangle(MARGIN, MARGIN, gui_app.width - MARGIN * 2, gui_app.height - MARGIN * 2) - self._wrapped_lines = wrap_text(text, FONT_SIZE, self._textarea_rect.width - 20) - self._content_rect = rl.Rectangle(0, 0, self._textarea_rect.width - 20, len(self._wrapped_lines) * LINE_HEIGHT) - self._scroll_panel = GuiScrollPanel() - self._scroll_panel._offset_filter_y.x = -max(self._content_rect.height - self._textarea_rect.height, 0) - - button_text = "Exit" if PC else "Reboot" - self._button = Button(button_text, click_callback=self._on_button_clicked, button_style=ButtonStyle.TRANSPARENT_WHITE_BORDER, font_size=FONT_SIZE) - - @staticmethod - def _on_button_clicked(): - gui_app.request_close() - if not PC: - HARDWARE.reboot() - - def _render(self, rect: rl.Rectangle): - scroll = self._scroll_panel.update(self._textarea_rect, self._content_rect) - rl.begin_scissor_mode(int(self._textarea_rect.x), int(self._textarea_rect.y), int(self._textarea_rect.width), int(self._textarea_rect.height)) - for i, line in enumerate(self._wrapped_lines): - position = rl.Vector2(self._textarea_rect.x, self._textarea_rect.y + scroll + i * LINE_HEIGHT) - if position.y + LINE_HEIGHT < self._textarea_rect.y or position.y > self._textarea_rect.y + self._textarea_rect.height: - continue - rl.draw_text_ex(gui_app.font(), line, position, FONT_SIZE, 0, rl.WHITE) - rl.end_scissor_mode() - - button_bounds = rl.Rectangle(rect.width - MARGIN - BUTTON_SIZE.x - SPACING, rect.height - MARGIN - BUTTON_SIZE.y, BUTTON_SIZE.x, BUTTON_SIZE.y) - self._button.render(button_bounds) - - -if __name__ == "__main__": - text = sys.argv[1] if len(sys.argv) > 1 else DEMO_TEXT - gui_app.init_window("Text Viewer") - text_window = TextWindow(text) - for _ in gui_app.render(): - text_window.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) diff --git a/system/ui/tici_reset.py b/system/ui/tici_reset.py deleted file mode 100755 index 3922c27aac6b6d..00000000000000 --- a/system/ui/tici_reset.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import threading -import time -from enum import IntEnum - -import pyray as rl - -from openpilot.system.hardware import PC -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -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 - -USERDATA = "/dev/disk/by-partlabel/userdata" -TIMEOUT = 3*60 - - -class ResetMode(IntEnum): - USER_RESET = 0 # user initiated a factory reset from openpilot - RECOVER = 1 # userdata is corrupt for some reason, give a chance to recover - FORMAT = 2 # finish up a factory reset from a tool that doesn't flash an empty partition to userdata - - -class ResetState(IntEnum): - NONE = 0 - CONFIRM = 1 - RESETTING = 2 - FAILED = 3 - - -class Reset(Widget): - def __init__(self, mode): - super().__init__() - self._mode = mode - self._previous_reset_state = None - self._reset_state = ResetState.NONE - self._cancel_button = Button("Cancel", self._cancel_callback) - self._confirm_button = Button("Confirm", self._confirm, button_style=ButtonStyle.PRIMARY) - self._reboot_button = Button("Reboot", lambda: os.system("sudo reboot")) - self._render_status = True - - def _cancel_callback(self): - self._render_status = False - - def _do_erase(self): - if PC: - return - - # Removing data and formatting - rm = os.system("sudo rm -rf /data/*") - os.system(f"sudo umount {USERDATA}") - fmt = os.system(f"yes | sudo mkfs.ext4 {USERDATA}") - - if rm == 0 or fmt == 0: - os.system("sudo reboot") - else: - self._reset_state = ResetState.FAILED - - def start_reset(self): - self._reset_state = ResetState.RESETTING - threading.Timer(0.1, self._do_erase).start() - - def _update_state(self): - if self._reset_state != self._previous_reset_state: - self._previous_reset_state = self._reset_state - self._timeout_st = time.monotonic() - elif self._reset_state != ResetState.RESETTING and (time.monotonic() - self._timeout_st) > TIMEOUT: - exit(0) - - def _render(self, rect: rl.Rectangle): - label_rect = rl.Rectangle(rect.x + 140, rect.y, rect.width - 280, 100 * FONT_SCALE) - gui_label(label_rect, "System Reset", 100, font_weight=FontWeight.BOLD) - - text_rect = rl.Rectangle(rect.x + 140, rect.y + 140, rect.width - 280, rect.height - 90 - 100 * FONT_SCALE) - gui_text_box(text_rect, self._get_body_text(), 90) - - button_height = 160 - button_spacing = 50 - button_top = rect.y + rect.height - button_height - button_width = (rect.width - button_spacing) / 2.0 - - if self._reset_state != ResetState.RESETTING: - if self._mode == ResetMode.RECOVER: - self._reboot_button.render(rl.Rectangle(rect.x, button_top, button_width, button_height)) - elif self._mode == ResetMode.USER_RESET: - self._cancel_button.render(rl.Rectangle(rect.x, button_top, button_width, button_height)) - - if self._reset_state != ResetState.FAILED: - self._confirm_button.render(rl.Rectangle(rect.x + button_width + 50, button_top, button_width, button_height)) - else: - self._reboot_button.render(rl.Rectangle(rect.x, button_top, rect.width, button_height)) - - return self._render_status - - def _confirm(self): - if self._reset_state == ResetState.CONFIRM: - self.start_reset() - else: - self._reset_state = ResetState.CONFIRM - - def _get_body_text(self): - if self._reset_state == ResetState.CONFIRM: - return "Are you sure you want to reset your device?" - if self._reset_state == ResetState.RESETTING: - return "Resetting device...\nThis may take up to a minute." - if self._reset_state == ResetState.FAILED: - return "Reset failed. Reboot to try again." - if self._mode == ResetMode.RECOVER: - return "Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device." - return "System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot." - - -def main(): - mode = ResetMode.USER_RESET - if len(sys.argv) > 1: - if sys.argv[1] == '--recover': - mode = ResetMode.RECOVER - elif sys.argv[1] == "--format": - mode = ResetMode.FORMAT - - gui_app.init_window("System Reset", 20) - reset = Reset(mode) - - if mode == ResetMode.FORMAT: - reset.start_reset() - - for should_render in gui_app.render(): - if should_render: - if not reset.render(rl.Rectangle(45, 200, gui_app.width - 90, gui_app.height - 245)): - break - - -if __name__ == "__main__": - main() diff --git a/system/ui/tici_setup.py b/system/ui/tici_setup.py deleted file mode 100755 index bf64361bed44e3..00000000000000 --- a/system/ui/tici_setup.py +++ /dev/null @@ -1,451 +0,0 @@ -#!/usr/bin/env python3 -import os -import re -import threading -import time -import urllib.request -import urllib.error -from urllib.parse import urlparse -from enum import IntEnum -import shutil - -import pyray as rl - -from cereal import log -from openpilot.common.utils import run_cmd -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -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 -from openpilot.system.ui.widgets.network import WifiManagerUI, WifiManager - -NetworkType = log.DeviceState.NetworkType - -MARGIN = 50 -TITLE_FONT_SIZE = 90 -TITLE_FONT_WEIGHT = FontWeight.MEDIUM -NEXT_BUTTON_WIDTH = 310 -BODY_FONT_SIZE = 80 -BUTTON_HEIGHT = 160 -BUTTON_SPACING = 50 - -OPENPILOT_URL = "https://openpilot.comma.ai" -USER_AGENT = f"AGNOSSetup-{HARDWARE.get_os_version()}" - -CONTINUE_PATH = "/data/continue.sh" -TMP_CONTINUE_PATH = "/data/continue.sh.new" -INSTALL_PATH = "/data/openpilot" -VALID_CACHE_PATH = "/data/.openpilot_cache" -INSTALLER_SOURCE_PATH = "/usr/comma/installer" -INSTALLER_DESTINATION_PATH = "/tmp/installer" -INSTALLER_URL_PATH = "/tmp/installer_url" - -CONTINUE = """#!/usr/bin/env bash - -cd /data/openpilot -exec ./launch_openpilot.sh -""" - - -class SetupState(IntEnum): - LOW_VOLTAGE = 0 - GETTING_STARTED = 1 - NETWORK_SETUP = 2 - SOFTWARE_SELECTION = 3 - CUSTOM_SOFTWARE = 4 - DOWNLOADING = 5 - DOWNLOAD_FAILED = 6 - CUSTOM_SOFTWARE_WARNING = 7 - - -class Setup(Widget): - def __init__(self): - super().__init__() - self.state = SetupState.GETTING_STARTED - self.network_check_thread = None - self.network_connected = threading.Event() - self.wifi_connected = threading.Event() - self.stop_network_check_thread = threading.Event() - self.failed_url = "" - self.failed_reason = "" - self.download_url = "" - self.download_progress = 0 - self.download_thread = None - self.wifi_ui = WifiManagerUI(WifiManager()) - self.keyboard = Keyboard() - self.selected_radio = None - self.warning = gui_app.texture("icons/warning.png", 150, 150) - self.checkmark = gui_app.texture("icons/circled_check.png", 100, 100) - - self._low_voltage_title_label = Label("WARNING: Low Voltage", TITLE_FONT_SIZE, FontWeight.MEDIUM, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - text_color=rl.Color(255, 89, 79, 255), text_padding=20) - self._low_voltage_body_label = Label("Power your device in a car with a harness or proceed at your own risk.", BODY_FONT_SIZE, - text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - self._low_voltage_continue_button = Button("Continue", self._low_voltage_continue_button_callback) - self._low_voltage_poweroff_button = Button("Power Off", HARDWARE.shutdown) - - self._getting_started_button = Button("", self._getting_started_button_callback, button_style=ButtonStyle.PRIMARY, border_radius=0) - self._getting_started_title_label = Label("Getting Started", TITLE_FONT_SIZE, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - self._getting_started_body_label = Label("Before we get on the road, let's finish installation and cover some details.", - BODY_FONT_SIZE, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - - self._software_selection_openpilot_button = ButtonRadio("openpilot", self.checkmark, font_size=BODY_FONT_SIZE, text_padding=80) - self._software_selection_custom_software_button = ButtonRadio("Custom Software", self.checkmark, font_size=BODY_FONT_SIZE, text_padding=80) - self._software_selection_continue_button = Button("Continue", self._software_selection_continue_button_callback, - button_style=ButtonStyle.PRIMARY) - self._software_selection_continue_button.set_enabled(False) - self._software_selection_back_button = Button("Back", self._software_selection_back_button_callback) - self._software_selection_title_label = Label("Choose Software to Use", TITLE_FONT_SIZE, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - text_padding=20) - - self._download_failed_reboot_button = Button("Reboot device", HARDWARE.reboot) - self._download_failed_startover_button = Button("Start over", self._download_failed_startover_button_callback, button_style=ButtonStyle.PRIMARY) - self._download_failed_title_label = Label("Download Failed", TITLE_FONT_SIZE, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - self._download_failed_url_label = Label("", 52, FontWeight.NORMAL, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - self._download_failed_body_label = Label("", BODY_FONT_SIZE, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - - self._network_setup_back_button = Button("Back", self._network_setup_back_button_callback) - self._network_setup_continue_button = Button("Waiting for internet", self._network_setup_continue_button_callback, - button_style=ButtonStyle.PRIMARY) - self._network_setup_continue_button.set_enabled(False) - self._network_setup_title_label = Label("Connect to Wi-Fi", TITLE_FONT_SIZE, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - - self._custom_software_warning_continue_button = Button("Scroll to continue", self._custom_software_warning_continue_button_callback, - button_style=ButtonStyle.PRIMARY) - self._custom_software_warning_continue_button.set_enabled(False) - self._custom_software_warning_back_button = Button("Back", self._custom_software_warning_back_button_callback) - self._custom_software_warning_title_label = Label("WARNING: Custom Software", 81, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - text_color=rl.Color(255, 89, 79, 255), - text_padding=60) - self._custom_software_warning_body_label = Label("Use caution when installing third-party software.\n\n" - + "⚠️ It has not been tested by comma.\n\n" - + "⚠️ It may not comply with relevant safety standards.\n\n" - + "⚠️ It may cause damage to your device and/or vehicle.\n\n" - + "If you'd like to proceed, use https://flash.comma.ai " - + "to restore your device to a factory state later.", - 68, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=60) - self._custom_software_warning_body_scroll_panel = GuiScrollPanel() - - self._downloading_body_label = Label("Downloading...", TITLE_FONT_SIZE, FontWeight.MEDIUM, text_padding=20) - - try: - with open("/sys/class/hwmon/hwmon1/in1_input") as f: - voltage = float(f.read().strip()) / 1000.0 - if voltage < 7: - self.state = SetupState.LOW_VOLTAGE - except (FileNotFoundError, ValueError): - self.state = SetupState.LOW_VOLTAGE - - def _render(self, rect: rl.Rectangle): - if self.state == SetupState.LOW_VOLTAGE: - self.render_low_voltage(rect) - elif self.state == SetupState.GETTING_STARTED: - self.render_getting_started(rect) - elif self.state == SetupState.NETWORK_SETUP: - self.render_network_setup(rect) - elif self.state == SetupState.SOFTWARE_SELECTION: - self.render_software_selection(rect) - elif self.state == SetupState.CUSTOM_SOFTWARE_WARNING: - self.render_custom_software_warning(rect) - elif self.state == SetupState.CUSTOM_SOFTWARE: - self.render_custom_software() - elif self.state == SetupState.DOWNLOADING: - self.render_downloading(rect) - elif self.state == SetupState.DOWNLOAD_FAILED: - self.render_download_failed(rect) - - def _low_voltage_continue_button_callback(self): - self.state = SetupState.GETTING_STARTED - - def _custom_software_warning_back_button_callback(self): - self.state = SetupState.SOFTWARE_SELECTION - - def _custom_software_warning_continue_button_callback(self): - self.state = SetupState.NETWORK_SETUP - self.stop_network_check_thread.clear() - self.start_network_check() - - def _getting_started_button_callback(self): - self.state = SetupState.SOFTWARE_SELECTION - - def _software_selection_back_button_callback(self): - self.state = SetupState.GETTING_STARTED - - def _software_selection_continue_button_callback(self): - if self._software_selection_openpilot_button.selected: - self.use_openpilot() - else: - self.state = SetupState.CUSTOM_SOFTWARE_WARNING - - def _download_failed_startover_button_callback(self): - self.state = SetupState.GETTING_STARTED - - def _network_setup_back_button_callback(self): - self.state = SetupState.SOFTWARE_SELECTION - - def _network_setup_continue_button_callback(self): - self.stop_network_check_thread.set() - if self._software_selection_openpilot_button.selected: - self.download(OPENPILOT_URL) - else: - self.state = SetupState.CUSTOM_SOFTWARE - - def render_low_voltage(self, rect: rl.Rectangle): - rl.draw_texture(self.warning, int(rect.x + 150), int(rect.y + 110), rl.WHITE) - - self._low_voltage_title_label.render(rl.Rectangle(rect.x + 150, rect.y + 110 + 150 + 100, rect.width - 500 - 150, TITLE_FONT_SIZE * FONT_SCALE)) - self._low_voltage_body_label.render(rl.Rectangle(rect.x + 150, rect.y + 110 + 150 + 150, rect.width - 500, BODY_FONT_SIZE * FONT_SCALE * 3)) - - button_width = (rect.width - MARGIN * 3) / 2 - button_y = rect.height - MARGIN - BUTTON_HEIGHT - self._low_voltage_poweroff_button.render(rl.Rectangle(rect.x + MARGIN, button_y, button_width, BUTTON_HEIGHT)) - self._low_voltage_continue_button.render(rl.Rectangle(rect.x + MARGIN * 2 + button_width, button_y, button_width, BUTTON_HEIGHT)) - - def render_getting_started(self, rect: rl.Rectangle): - self._getting_started_title_label.render(rl.Rectangle(rect.x + 165, rect.y + 280, rect.width - 265, TITLE_FONT_SIZE * FONT_SCALE)) - self._getting_started_body_label.render(rl.Rectangle(rect.x + 165, rect.y + 280 + TITLE_FONT_SIZE * FONT_SCALE, rect.width - 500, - BODY_FONT_SIZE * FONT_SCALE * 3)) - - btn_rect = rl.Rectangle(rect.width - NEXT_BUTTON_WIDTH, 0, NEXT_BUTTON_WIDTH, rect.height) - self._getting_started_button.render(btn_rect) - triangle = gui_app.texture("images/button_continue_triangle.png", 54, int(btn_rect.height)) - rl.draw_texture_v(triangle, rl.Vector2(btn_rect.x + btn_rect.width / 2 - triangle.width / 2, btn_rect.height / 2 - triangle.height / 2), rl.WHITE) - - def check_network_connectivity(self): - while not self.stop_network_check_thread.is_set(): - if self.state == SetupState.NETWORK_SETUP: - try: - urllib.request.urlopen(OPENPILOT_URL, timeout=2) - self.network_connected.set() - if HARDWARE.get_network_type() == NetworkType.wifi: - self.wifi_connected.set() - else: - self.wifi_connected.clear() - except Exception: - self.network_connected.clear() - time.sleep(1) - - def start_network_check(self): - if self.network_check_thread is None or not self.network_check_thread.is_alive(): - self.network_check_thread = threading.Thread(target=self.check_network_connectivity, daemon=True) - self.network_check_thread.start() - - def close(self): - if self.network_check_thread is not None: - self.stop_network_check_thread.set() - self.network_check_thread.join() - - def render_network_setup(self, rect: rl.Rectangle): - self._network_setup_title_label.render(rl.Rectangle(rect.x + MARGIN, rect.y + MARGIN, rect.width - MARGIN * 2, TITLE_FONT_SIZE * FONT_SCALE)) - - wifi_rect = rl.Rectangle(rect.x + MARGIN, rect.y + TITLE_FONT_SIZE * FONT_SCALE + MARGIN + 25, rect.width - MARGIN * 2, - rect.height - TITLE_FONT_SIZE * FONT_SCALE - 25 - BUTTON_HEIGHT - MARGIN * 3) - rl.draw_rectangle_rounded(wifi_rect, 0.05, 10, rl.Color(51, 51, 51, 255)) - wifi_content_rect = rl.Rectangle(wifi_rect.x + MARGIN, wifi_rect.y, wifi_rect.width - MARGIN * 2, wifi_rect.height) - self.wifi_ui.render(wifi_content_rect) - - button_width = (rect.width - BUTTON_SPACING - MARGIN * 2) / 2 - button_y = rect.height - BUTTON_HEIGHT - MARGIN - - self._network_setup_back_button.render(rl.Rectangle(rect.x + MARGIN, button_y, button_width, BUTTON_HEIGHT)) - - # Check network connectivity status - continue_enabled = self.network_connected.is_set() - self._network_setup_continue_button.set_enabled(continue_enabled) - continue_text = ("Continue" if self.wifi_connected.is_set() else "Continue without Wi-Fi") if continue_enabled else "Waiting for internet" - self._network_setup_continue_button.set_text(continue_text) - self._network_setup_continue_button.render(rl.Rectangle(rect.x + MARGIN + button_width + BUTTON_SPACING, button_y, button_width, BUTTON_HEIGHT)) - - def render_software_selection(self, rect: rl.Rectangle): - self._software_selection_title_label.render(rl.Rectangle(rect.x + MARGIN, rect.y + MARGIN, rect.width - MARGIN * 2, TITLE_FONT_SIZE * FONT_SCALE)) - - radio_height = 230 - radio_spacing = 30 - - self._software_selection_continue_button.set_enabled(False) - - openpilot_rect = rl.Rectangle(rect.x + MARGIN, rect.y + TITLE_FONT_SIZE * FONT_SCALE + MARGIN * 2, rect.width - MARGIN * 2, radio_height) - self._software_selection_openpilot_button.render(openpilot_rect) - - if self._software_selection_openpilot_button.selected: - self._software_selection_continue_button.set_enabled(True) - self._software_selection_custom_software_button.selected = False - - custom_rect = rl.Rectangle(rect.x + MARGIN, rect.y + TITLE_FONT_SIZE * FONT_SCALE + MARGIN * 2 + radio_height + radio_spacing, rect.width - MARGIN * 2, - radio_height) - self._software_selection_custom_software_button.render(custom_rect) - - if self._software_selection_custom_software_button.selected: - self._software_selection_continue_button.set_enabled(True) - self._software_selection_openpilot_button.selected = False - - button_width = (rect.width - BUTTON_SPACING - MARGIN * 2) / 2 - button_y = rect.height - BUTTON_HEIGHT - MARGIN - - self._software_selection_back_button.render(rl.Rectangle(rect.x + MARGIN, button_y, button_width, BUTTON_HEIGHT)) - self._software_selection_continue_button.render(rl.Rectangle(rect.x + MARGIN + button_width + BUTTON_SPACING, button_y, button_width, BUTTON_HEIGHT)) - - def render_downloading(self, rect: rl.Rectangle): - self._downloading_body_label.render(rl.Rectangle(rect.x, rect.y + rect.height / 2 - TITLE_FONT_SIZE * FONT_SCALE / 2, rect.width, - TITLE_FONT_SIZE * FONT_SCALE)) - - def render_download_failed(self, rect: rl.Rectangle): - self._download_failed_title_label.render(rl.Rectangle(rect.x + 117, rect.y + 185, rect.width - 117, TITLE_FONT_SIZE * FONT_SCALE)) - self._download_failed_url_label.set_text(self.failed_url) - self._download_failed_url_label.render(rl.Rectangle(rect.x + 117, rect.y + 185 + TITLE_FONT_SIZE * FONT_SCALE + 67, rect.width - 117 - 100, 64)) - - self._download_failed_body_label.set_text(self.failed_reason) - self._download_failed_body_label.render(rl.Rectangle(rect.x + 117, rect.y, rect.width - 117 - 100, rect.height)) - - button_width = (rect.width - BUTTON_SPACING - MARGIN * 2) / 2 - button_y = rect.height - BUTTON_HEIGHT - MARGIN - self._download_failed_reboot_button.render(rl.Rectangle(rect.x + MARGIN, button_y, button_width, BUTTON_HEIGHT)) - self._download_failed_startover_button.render(rl.Rectangle(rect.x + MARGIN + button_width + BUTTON_SPACING, button_y, button_width, BUTTON_HEIGHT)) - - def render_custom_software_warning(self, rect: rl.Rectangle): - warn_rect = rl.Rectangle(rect.x, rect.y, rect.width, 1500) - offset = self._custom_software_warning_body_scroll_panel.update(rect, warn_rect) - - button_width = (rect.width - MARGIN * 3) / 2 - button_y = rect.height - MARGIN - BUTTON_HEIGHT - - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(button_y - BODY_FONT_SIZE * FONT_SCALE)) - y_offset = rect.y + offset - self._custom_software_warning_title_label.render(rl.Rectangle(rect.x + 50, y_offset + 150, rect.width - 265, TITLE_FONT_SIZE * FONT_SCALE)) - self._custom_software_warning_body_label.render(rl.Rectangle(rect.x + 50, y_offset + 400, rect.width - 50, BODY_FONT_SIZE * FONT_SCALE * 3)) - rl.end_scissor_mode() - - self._custom_software_warning_back_button.render(rl.Rectangle(rect.x + MARGIN, button_y, button_width, BUTTON_HEIGHT)) - self._custom_software_warning_continue_button.render(rl.Rectangle(rect.x + MARGIN * 2 + button_width, button_y, button_width, BUTTON_HEIGHT)) - if offset < (rect.height - warn_rect.height): - self._custom_software_warning_continue_button.set_enabled(True) - self._custom_software_warning_continue_button.set_text("Continue") - - def render_custom_software(self): - def handle_keyboard_result(result): - # Enter pressed - if result == 1: - url = self.keyboard.text - self.keyboard.clear() - if url: - self.download(url) - - # Cancel pressed - elif result == 0: - self.state = SetupState.SOFTWARE_SELECTION - - self.keyboard.reset(min_text_size=1) - self.keyboard.set_title("Enter URL", "for Custom Software") - gui_app.set_modal_overlay(self.keyboard, callback=handle_keyboard_result) - - def use_openpilot(self): - if os.path.isdir(INSTALL_PATH) and os.path.isfile(VALID_CACHE_PATH): - os.remove(VALID_CACHE_PATH) - with open(TMP_CONTINUE_PATH, "w") as f: - f.write(CONTINUE) - run_cmd(["chmod", "+x", TMP_CONTINUE_PATH]) - shutil.move(TMP_CONTINUE_PATH, CONTINUE_PATH) - shutil.copyfile(INSTALLER_SOURCE_PATH, INSTALLER_DESTINATION_PATH) - - # give time for installer UI to take over - time.sleep(0.1) - gui_app.request_close() - else: - self.state = SetupState.NETWORK_SETUP - self.stop_network_check_thread.clear() - self.start_network_check() - - def download(self, url: str): - # autocomplete incomplete URLs - if re.match("^([^/.]+)/([^/]+)$", url): - url = f"https://installer.comma.ai/{url}" - - parsed = urlparse(url, scheme='https') - self.download_url = (urlparse(f"https://{url}") if not parsed.netloc else parsed).geturl() - - self.state = SetupState.DOWNLOADING - - self.download_thread = threading.Thread(target=self._download_thread, daemon=True) - self.download_thread.start() - - def _download_thread(self): - try: - import tempfile - - fd, tmpfile = tempfile.mkstemp(prefix="installer_") - - headers = {"User-Agent": USER_AGENT, - "X-openpilot-serial": HARDWARE.get_serial(), - "X-openpilot-device-type": HARDWARE.get_device_type()} - req = urllib.request.Request(self.download_url, headers=headers) - - with open(tmpfile, 'wb') as f, urllib.request.urlopen(req, timeout=30) as response: - total_size = int(response.headers.get('content-length', 0)) - downloaded = 0 - block_size = 8192 - - while True: - buffer = response.read(block_size) - if not buffer: - break - - downloaded += len(buffer) - f.write(buffer) - - if total_size: - self.download_progress = int(downloaded * 100 / total_size) - - is_elf = False - with open(tmpfile, 'rb') as f: - header = f.read(4) - is_elf = header == b'\x7fELF' - - if not is_elf: - self.download_failed(self.download_url, "No custom software found at this URL.") - return - - # AGNOS might try to execute the installer before this process exits. - # Therefore, important to close the fd before renaming the installer. - os.close(fd) - os.rename(tmpfile, INSTALLER_DESTINATION_PATH) - - with open(INSTALLER_URL_PATH, "w") as f: - f.write(self.download_url) - - # give time for installer UI to take over - time.sleep(0.1) - gui_app.request_close() - - except urllib.error.HTTPError as e: - if e.code == 409: - error_msg = e.read().decode("utf-8") - self.download_failed(self.download_url, error_msg) - except Exception: - error_msg = "Ensure the entered URL is valid, and the device's internet connection is good." - self.download_failed(self.download_url, error_msg) - - def download_failed(self, url: str, reason: str): - self.failed_url = url - self.failed_reason = reason - self.state = SetupState.DOWNLOAD_FAILED - - -def main(): - try: - gui_app.init_window("Setup", 20) - setup = Setup() - for should_render in gui_app.render(): - if should_render: - setup.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - setup.close() - except Exception as e: - print(f"Setup error: {e}") - finally: - gui_app.close() - - -if __name__ == "__main__": - main() diff --git a/system/ui/tici_updater.py b/system/ui/tici_updater.py deleted file mode 100755 index ebf4b3bec39921..00000000000000 --- a/system/ui/tici_updater.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -import sys -import subprocess -import threading -import pyray as rl -from enum import IntEnum - -from openpilot.system.hardware import HARDWARE -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.wifi_manager import WifiManager -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.label import gui_text_box, gui_label -from openpilot.system.ui.widgets.network import WifiManagerUI - -# Constants -MARGIN = 50 -BUTTON_HEIGHT = 160 -BUTTON_WIDTH = 400 -PROGRESS_BAR_HEIGHT = 72 -TITLE_FONT_SIZE = 80 -BODY_FONT_SIZE = 65 -BACKGROUND_COLOR = rl.BLACK -PROGRESS_BG_COLOR = rl.Color(41, 41, 41, 255) -PROGRESS_COLOR = rl.Color(54, 77, 239, 255) - - -class Screen(IntEnum): - PROMPT = 0 - WIFI = 1 - PROGRESS = 2 - - -class Updater(Widget): - def __init__(self, updater_path, manifest_path): - super().__init__() - self.updater = updater_path - self.manifest = manifest_path - self.current_screen = Screen.PROMPT - - self.progress_value = 0 - self.progress_text = "Loading..." - self.show_reboot_button = False - self.process = None - self.update_thread = None - self.wifi_manager_ui = WifiManagerUI(WifiManager()) - - # Buttons - self._wifi_button = Button("Connect to Wi-Fi", click_callback=lambda: self.set_current_screen(Screen.WIFI)) - self._install_button = Button("Install", click_callback=self.install_update, button_style=ButtonStyle.PRIMARY) - self._back_button = Button("Back", click_callback=lambda: self.set_current_screen(Screen.PROMPT)) - self._reboot_button = Button("Reboot", click_callback=lambda: HARDWARE.reboot()) - - def set_current_screen(self, screen: Screen): - self.current_screen = screen - - def install_update(self): - self.set_current_screen(Screen.PROGRESS) - self.progress_value = 0 - self.progress_text = "Downloading..." - self.show_reboot_button = False - - # Start the update process in a separate thread - self.update_thread = threading.Thread(target=self._run_update_process) - self.update_thread.daemon = True - self.update_thread.start() - - def _run_update_process(self): - # TODO: just import it and run in a thread without a subprocess - cmd = [self.updater, "--swap", self.manifest] - self.process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, - text=True, bufsize=1, universal_newlines=True) - - if self.process.stdout is not None: - for line in self.process.stdout: - parts = line.strip().split(":") - if len(parts) == 2: - self.progress_text = parts[0] - try: - self.progress_value = int(float(parts[1])) - except ValueError: - pass - - exit_code = self.process.wait() - if exit_code == 0: - HARDWARE.reboot() - else: - self.progress_text = "Update failed" - self.show_reboot_button = True - - def render_prompt_screen(self, rect: rl.Rectangle): - # Title - title_rect = rl.Rectangle(MARGIN + 50, 250, rect.width - MARGIN * 2 - 100, TITLE_FONT_SIZE * FONT_SCALE) - gui_label(title_rect, "Update Required", TITLE_FONT_SIZE, font_weight=FontWeight.BOLD) - - # Description - desc_text = ("An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. " + - "The download size is approximately 1GB.") - - desc_rect = rl.Rectangle(MARGIN + 50, 250 + TITLE_FONT_SIZE * FONT_SCALE + 75, rect.width - MARGIN * 2 - 100, BODY_FONT_SIZE * FONT_SCALE * 4) - gui_text_box(desc_rect, desc_text, BODY_FONT_SIZE) - - # Buttons at the bottom - button_y = rect.height - MARGIN - BUTTON_HEIGHT - button_width = (rect.width - MARGIN * 3) // 2 - - # WiFi button - wifi_button_rect = rl.Rectangle(MARGIN, button_y, button_width, BUTTON_HEIGHT) - self._wifi_button.render(wifi_button_rect) - - # Install button - install_button_rect = rl.Rectangle(MARGIN * 2 + button_width, button_y, button_width, BUTTON_HEIGHT) - self._install_button.render(install_button_rect) - - def render_wifi_screen(self, rect: rl.Rectangle): - # Draw the Wi-Fi manager UI - wifi_rect = rl.Rectangle(rect.x + MARGIN, rect.y + MARGIN, rect.width - MARGIN * 2, - rect.height - BUTTON_HEIGHT - MARGIN * 3) - rl.draw_rectangle_rounded(wifi_rect, 0.035, 10, rl.Color(51, 51, 51, 255)) - wifi_content_rect = rl.Rectangle(wifi_rect.x + 50, wifi_rect.y, wifi_rect.width - 100, wifi_rect.height) - self.wifi_manager_ui.render(wifi_content_rect) - - back_button_rect = rl.Rectangle(MARGIN, rect.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) - self._back_button.render(back_button_rect) - - def render_progress_screen(self, rect: rl.Rectangle): - title_rect = rl.Rectangle(MARGIN + 100, 330, rect.width - MARGIN * 2 - 200, 100) - gui_label(title_rect, self.progress_text, 90, font_weight=FontWeight.SEMI_BOLD) - - # Progress bar - bar_rect = rl.Rectangle(MARGIN + 100, 330 + 100 + 100, rect.width - MARGIN * 2 - 200, PROGRESS_BAR_HEIGHT) - rl.draw_rectangle_rounded(bar_rect, 0.5, 10, PROGRESS_BG_COLOR) - - # Calculate the width of the progress chunk - progress_width = (bar_rect.width * self.progress_value) / 100 - if progress_width > 0: - progress_rect = rl.Rectangle(bar_rect.x, bar_rect.y, progress_width, bar_rect.height) - rl.draw_rectangle_rounded(progress_rect, 0.5, 10, PROGRESS_COLOR) - - # Show reboot button if needed - if self.show_reboot_button: - reboot_rect = rl.Rectangle(MARGIN + 100, rect.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) - self._reboot_button.render(reboot_rect) - - def _render(self, rect: rl.Rectangle): - if self.current_screen == Screen.PROMPT: - self.render_prompt_screen(rect) - elif self.current_screen == Screen.WIFI: - self.render_wifi_screen(rect) - elif self.current_screen == Screen.PROGRESS: - self.render_progress_screen(rect) - - -def main(): - if len(sys.argv) < 3: - print("Usage: updater.py ") - sys.exit(1) - - updater_path = sys.argv[1] - manifest_path = sys.argv[2] - - try: - gui_app.init_window("System Update") - updater = Updater(updater_path, manifest_path) - for should_render in gui_app.render(): - if should_render: - updater.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - # Make sure we clean up even if there's an error - gui_app.close() - - -if __name__ == "__main__": - main() diff --git a/system/ui/updater.py b/system/ui/updater.py deleted file mode 100755 index 42d12d90901813..00000000000000 --- a/system/ui/updater.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python3 -from openpilot.system.ui.lib.application import gui_app -import openpilot.system.ui.tici_updater as tici_updater -import openpilot.system.ui.mici_updater as mici_updater - - -def main(): - if gui_app.big_ui(): - tici_updater.main() - else: - mici_updater.main() - - -if __name__ == "__main__": - main() diff --git a/system/ui/widgets/__init__.py b/system/ui/widgets/__init__.py deleted file mode 100644 index 5d474e8aedf334..00000000000000 --- a/system/ui/widgets/__init__.py +++ /dev/null @@ -1,388 +0,0 @@ -from __future__ import annotations - -import abc -import pyray as rl -from enum import IntEnum -from collections.abc import Callable -from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter -from openpilot.system.ui.lib.application import gui_app, MousePos, MAX_TOUCH_SLOTS, MouseEvent - -try: - from openpilot.selfdrive.ui.ui_state import device -except ImportError: - class Device: - awake = True - device = Device() - - -class DialogResult(IntEnum): - CANCEL = 0 - CONFIRM = 1 - NO_ACTION = -1 - - -class Widget(abc.ABC): - def __init__(self): - self._rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) - self._parent_rect: rl.Rectangle | None = None - self.__is_pressed = [False] * MAX_TOUCH_SLOTS - # if current mouse/touch down started within the widget's rectangle - self.__tracking_is_pressed = [False] * MAX_TOUCH_SLOTS - self._enabled: bool | Callable[[], bool] = True - self._is_visible: bool | Callable[[], bool] = True - self._touch_valid_callback: Callable[[], bool] | None = None - self._click_callback: Callable[[], None] | None = None - self._multi_touch = False - self.__was_awake = True - - @property - def rect(self) -> rl.Rectangle: - return self._rect - - def set_rect(self, rect: rl.Rectangle) -> None: - changed = (self._rect.x != rect.x or self._rect.y != rect.y or - self._rect.width != rect.width or self._rect.height != rect.height) - self._rect = rect - if changed: - self._update_layout_rects() - - def set_parent_rect(self, parent_rect: rl.Rectangle) -> None: - """Can be used like size hint in QT""" - self._parent_rect = parent_rect - - @property - def is_pressed(self) -> bool: - return any(self.__is_pressed) - - @property - def enabled(self) -> bool: - return self._enabled() if callable(self._enabled) else self._enabled - - def set_enabled(self, enabled: bool | Callable[[], bool]) -> None: - self._enabled = enabled - - @property - def is_visible(self) -> bool: - return self._is_visible() if callable(self._is_visible) else self._is_visible - - def set_visible(self, visible: bool | Callable[[], bool]) -> None: - self._is_visible = visible - - def set_click_callback(self, click_callback: Callable[[], None] | None) -> None: - """Set a callback to be called when the widget is clicked.""" - self._click_callback = click_callback - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - """Set a callback to determine if the widget can be clicked.""" - self._touch_valid_callback = touch_callback - - def _touch_valid(self) -> bool: - """Check if the widget can be touched.""" - return self._touch_valid_callback() if self._touch_valid_callback else True - - def set_position(self, x: float, y: float) -> None: - changed = (self._rect.x != x or self._rect.y != y) - self._rect = rl.Rectangle(x, y, self._rect.width, self._rect.height) - if changed: - self._update_layout_rects() - - @property - def _hit_rect(self) -> rl.Rectangle: - # restrict touches to within parent rect if set, useful inside Scroller - if self._parent_rect is None: - return self._rect - return rl.get_collision_rec(self._rect, self._parent_rect) - - def render(self, rect: rl.Rectangle | None = None) -> bool | int | None: - if rect is not None: - self.set_rect(rect) - - self._update_state() - - if not self.is_visible: - return None - - self._layout() - ret = self._render(self._rect) - - # Keep track of whether mouse down started within the widget's rectangle - if self.enabled and self.__was_awake: - self._process_mouse_events() - - self.__was_awake = device.awake - - return ret - - def _process_mouse_events(self) -> None: - hit_rect = self._hit_rect - touch_valid = self._touch_valid() - - for mouse_event in gui_app.mouse_events: - if not self._multi_touch and mouse_event.slot != 0: - continue - - mouse_in_rect = rl.check_collision_point_rec(mouse_event.pos, hit_rect) - # Ignores touches/presses that start outside our rect - # Allows touch to leave the rect and come back in focus if mouse did not release - if mouse_event.left_pressed and touch_valid: - if mouse_in_rect: - self._handle_mouse_press(mouse_event.pos) - self.__is_pressed[mouse_event.slot] = True - self.__tracking_is_pressed[mouse_event.slot] = True - self._handle_mouse_event(mouse_event) - - # Callback such as scroll panel signifies user is scrolling - elif not touch_valid: - self.__is_pressed[mouse_event.slot] = False - self.__tracking_is_pressed[mouse_event.slot] = False - - elif mouse_event.left_released: - self._handle_mouse_event(mouse_event) - if self.__is_pressed[mouse_event.slot] and mouse_in_rect: - self._handle_mouse_release(mouse_event.pos) - self.__is_pressed[mouse_event.slot] = False - self.__tracking_is_pressed[mouse_event.slot] = False - - # Mouse/touch is still within our rect - elif mouse_in_rect: - if self.__tracking_is_pressed[mouse_event.slot]: - self.__is_pressed[mouse_event.slot] = True - self._handle_mouse_event(mouse_event) - - # Mouse/touch left our rect but may come back into focus later - elif not mouse_in_rect: - self.__is_pressed[mouse_event.slot] = False - self._handle_mouse_event(mouse_event) - - def _layout(self) -> None: - """Optionally lay out child widgets separately. This is called before rendering.""" - - def _update_state(self): - """Optionally update the widget's non-layout state. This is called before rendering.""" - - @abc.abstractmethod - def _render(self, rect: rl.Rectangle) -> bool | int | None: - """Render the widget within the given rectangle.""" - - def _update_layout_rects(self) -> None: - """Optionally update any layout rects on Widget rect change.""" - - def _handle_mouse_press(self, mouse_pos: MousePos) -> None: - """Optionally handle mouse press events.""" - - def _handle_mouse_release(self, mouse_pos: MousePos) -> None: - """Optionally handle mouse release events.""" - if self._click_callback: - self._click_callback() - - def _handle_mouse_event(self, mouse_event: MouseEvent) -> None: - """Optionally handle mouse events. This is called before rendering.""" - # Default implementation does nothing, can be overridden by subclasses - - 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""" - - -SWIPE_AWAY_THRESHOLD = 80 # px to dismiss after releasing -START_DISMISSING_THRESHOLD = 40 # px to start dismissing while dragging -BLOCK_SWIPE_AWAY_THRESHOLD = 60 # px horizontal movement to block swipe away - -NAV_BAR_MARGIN = 6 -NAV_BAR_WIDTH = 205 -NAV_BAR_HEIGHT = 8 - -DISMISS_PUSH_OFFSET = 50 + NAV_BAR_MARGIN + NAV_BAR_HEIGHT # px extra to push down when dismissing -DISMISS_TIME_SECONDS = 2.0 - - -class NavBar(Widget): - def __init__(self): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, NAV_BAR_WIDTH, NAV_BAR_HEIGHT)) - self._alpha = 1.0 - self._alpha_filter = FirstOrderFilter(1.0, 0.1, 1 / gui_app.target_fps) - self._fade_time = 0.0 - - def set_alpha(self, alpha: float) -> None: - self._alpha = alpha - self._fade_time = rl.get_time() - - def show_event(self): - super().show_event() - self._alpha = 1.0 - self._alpha_filter.x = 1.0 - self._fade_time = rl.get_time() - - def _render(self, _): - if rl.get_time() - self._fade_time > DISMISS_TIME_SECONDS: - self._alpha = 0.0 - alpha = self._alpha_filter.update(self._alpha) - - # white bar with black border - rl.draw_rectangle_rounded(self._rect, 1.0, 6, rl.Color(255, 255, 255, int(255 * 0.9 * alpha))) - rl.draw_rectangle_rounded_lines_ex(self._rect, 1.0, 6, 2, rl.Color(0, 0, 0, int(255 * 0.3 * alpha))) - - -class NavWidget(Widget, abc.ABC): - """ - A full screen widget that supports back navigation by swiping down from the top. - """ - BACK_TOUCH_AREA_PERCENTAGE = 0.65 - - def __init__(self): - super().__init__() - self._back_callback: Callable[[], None] | None = None - self._back_button_start_pos: MousePos | None = None - self._swiping_away = False # currently swiping away - self._can_swipe_away = True # swipe away is blocked after certain horizontal movement - - self._pos_filter = BounceFilter(0.0, 0.1, 1 / gui_app.target_fps, bounce=1) - self._playing_dismiss_animation = False - self._trigger_animate_in = False - self._back_enabled: bool | Callable[[], bool] = True - self._nav_bar = NavBar() - - self._nav_bar_y_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - - self._set_up = False - - @property - def back_enabled(self) -> bool: - return self._back_enabled() if callable(self._back_enabled) else self._back_enabled - - def set_back_enabled(self, enabled: bool | Callable[[], bool]) -> None: - self._back_enabled = enabled - - def set_back_callback(self, callback: Callable[[], None]) -> None: - self._back_callback = callback - - def _handle_mouse_event(self, mouse_event: MouseEvent) -> None: - super()._handle_mouse_event(mouse_event) - - if not self.back_enabled: - self._back_button_start_pos = None - self._swiping_away = False - self._can_swipe_away = True - return - - if mouse_event.left_pressed: - # user is able to swipe away if starting near top of screen, or anywhere if scroller is at top - self._pos_filter.update_alpha(0.04) - in_dismiss_area = mouse_event.pos.y < self._rect.height * self.BACK_TOUCH_AREA_PERCENTAGE - - scroller_at_top = False - vertical_scroller = False - # TODO: -20? snapping in WiFi dialog can make offset not be positive at the top - if hasattr(self, '_scroller'): - scroller_at_top = self._scroller.scroll_panel.get_offset() >= -20 and not self._scroller._horizontal - vertical_scroller = not self._scroller._horizontal - elif hasattr(self, '_scroll_panel'): - scroller_at_top = self._scroll_panel.get_offset() >= -20 and not self._scroll_panel._horizontal - vertical_scroller = not self._scroll_panel._horizontal - - # Vertical scrollers need to be at the top to swipe away to prevent erroneous swipes - if (not vertical_scroller and in_dismiss_area) or scroller_at_top: - self._can_swipe_away = True - self._back_button_start_pos = mouse_event.pos - - elif mouse_event.left_down: - if self._back_button_start_pos is not None: - # block swiping away if too much horizontal or upward movement - horizontal_movement = abs(mouse_event.pos.x - self._back_button_start_pos.x) > BLOCK_SWIPE_AWAY_THRESHOLD - upward_movement = mouse_event.pos.y - self._back_button_start_pos.y < -BLOCK_SWIPE_AWAY_THRESHOLD - if not self._swiping_away and (horizontal_movement or upward_movement): - self._can_swipe_away = False - self._back_button_start_pos = None - - # block horizontal swiping if now swiping away - if self._can_swipe_away: - if mouse_event.pos.y - self._back_button_start_pos.y > START_DISMISSING_THRESHOLD: - self._swiping_away = True - - elif mouse_event.left_released: - self._pos_filter.update_alpha(0.1) - # if far enough, trigger back navigation callback - if self._back_button_start_pos is not None: - if mouse_event.pos.y - self._back_button_start_pos.y > SWIPE_AWAY_THRESHOLD: - self._playing_dismiss_animation = True - - self._back_button_start_pos = None - self._swiping_away = False - - def _update_state(self): - super()._update_state() - - # Disable self's scroller while swiping away - if not self._set_up: - self._set_up = True - if hasattr(self, '_scroller'): - original_enabled = self._scroller._enabled - self._scroller.set_enabled(lambda: not self._swiping_away and (original_enabled() if callable(original_enabled) else - original_enabled)) - elif hasattr(self, '_scroll_panel'): - original_enabled = self._scroll_panel.enabled - self._scroll_panel.set_enabled(lambda: not self._swiping_away and (original_enabled() if callable(original_enabled) else - original_enabled)) - - if self._trigger_animate_in: - self._pos_filter.x = self._rect.height - self._nav_bar_y_filter.x = -NAV_BAR_MARGIN - NAV_BAR_HEIGHT - self._trigger_animate_in = False - - new_y = 0.0 - - if self._back_button_start_pos is not None: - last_mouse_event = gui_app.last_mouse_event - # push entire widget as user drags it away - new_y = max(last_mouse_event.pos.y - self._back_button_start_pos.y, 0) - if new_y < SWIPE_AWAY_THRESHOLD: - new_y /= 2 # resistance until mouse release would dismiss widget - - if self._swiping_away: - self._nav_bar.set_alpha(1.0) - - if self._playing_dismiss_animation: - new_y = self._rect.height + DISMISS_PUSH_OFFSET - - new_y = round(self._pos_filter.update(new_y)) - if abs(new_y) < 1 and self._pos_filter.velocity.x == 0.0: - new_y = self._pos_filter.x = 0.0 - - if new_y > self._rect.height + DISMISS_PUSH_OFFSET - 10: - if self._back_callback is not None: - self._back_callback() - - self._playing_dismiss_animation = False - self._back_button_start_pos = None - self._swiping_away = False - - self.set_position(self._rect.x, new_y) - - def render(self, rect: rl.Rectangle | None = None) -> bool | int | None: - ret = super().render(rect) - - if self.back_enabled: - bar_x = self._rect.x + (self._rect.width - self._nav_bar.rect.width) / 2 - if self._back_button_start_pos is not None or self._playing_dismiss_animation: - self._nav_bar_y_filter.x = NAV_BAR_MARGIN + self._pos_filter.x - else: - self._nav_bar_y_filter.update(NAV_BAR_MARGIN) - - # draw black above widget when dismissing - if self._rect.y > 0: - rl.draw_rectangle(int(self._rect.x), 0, int(self._rect.width), int(self._rect.y), rl.BLACK) - - self._nav_bar.set_position(bar_x, round(self._nav_bar_y_filter.x)) - self._nav_bar.render() - - return ret - - def show_event(self): - super().show_event() - # FIXME: we don't know the height of the rect at first show_event since it's before the first render :( - # so we need this hacky bool for now - self._trigger_animate_in = True - self._nav_bar.show_event() diff --git a/system/ui/widgets/button.py b/system/ui/widgets/button.py deleted file mode 100644 index 9c0ea75b4283f0..00000000000000 --- a/system/ui/widgets/button.py +++ /dev/null @@ -1,303 +0,0 @@ -from collections.abc import Callable -from enum import IntEnum - -import pyray as rl - -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import Label, UnifiedLabel -from openpilot.common.filter_simple import FirstOrderFilter - - -class ButtonStyle(IntEnum): - NORMAL = 0 # Most common, neutral buttons - PRIMARY = 1 # For main actions - DANGER = 2 # For critical actions, like reboot or delete - TRANSPARENT = 3 # For buttons with transparent background and border - TRANSPARENT_WHITE_TEXT = 9 # For buttons with transparent background and border and white text - TRANSPARENT_WHITE_BORDER = 10 # For buttons with transparent background and white border and text - ACTION = 4 - LIST_ACTION = 5 # For list items with action buttons - NO_EFFECT = 6 - KEYBOARD = 7 - FORGET_WIFI = 8 - - -ICON_PADDING = 15 -DEFAULT_BUTTON_FONT_SIZE = 60 -ACTION_BUTTON_FONT_SIZE = 48 - -BUTTON_TEXT_COLOR = { - ButtonStyle.NORMAL: rl.Color(228, 228, 228, 255), - ButtonStyle.PRIMARY: rl.Color(228, 228, 228, 255), - ButtonStyle.DANGER: rl.Color(228, 228, 228, 255), - ButtonStyle.TRANSPARENT: rl.BLACK, - ButtonStyle.TRANSPARENT_WHITE_TEXT: rl.WHITE, - ButtonStyle.TRANSPARENT_WHITE_BORDER: rl.Color(228, 228, 228, 255), - ButtonStyle.ACTION: rl.BLACK, - ButtonStyle.LIST_ACTION: rl.Color(228, 228, 228, 255), - ButtonStyle.NO_EFFECT: rl.Color(228, 228, 228, 255), - ButtonStyle.KEYBOARD: rl.Color(221, 221, 221, 255), - ButtonStyle.FORGET_WIFI: rl.Color(51, 51, 51, 255), -} - -BUTTON_DISABLED_TEXT_COLORS = { - ButtonStyle.TRANSPARENT_WHITE_TEXT: rl.WHITE, -} - -BUTTON_BACKGROUND_COLORS = { - ButtonStyle.NORMAL: rl.Color(51, 51, 51, 255), - ButtonStyle.PRIMARY: rl.Color(70, 91, 234, 255), - ButtonStyle.DANGER: rl.Color(226, 44, 44, 255), - ButtonStyle.TRANSPARENT: rl.BLACK, - ButtonStyle.TRANSPARENT_WHITE_TEXT: rl.BLANK, - ButtonStyle.TRANSPARENT_WHITE_BORDER: rl.BLACK, - ButtonStyle.ACTION: rl.Color(189, 189, 189, 255), - ButtonStyle.LIST_ACTION: rl.Color(57, 57, 57, 255), - ButtonStyle.NO_EFFECT: rl.Color(51, 51, 51, 255), - ButtonStyle.KEYBOARD: rl.Color(68, 68, 68, 255), - ButtonStyle.FORGET_WIFI: rl.Color(189, 189, 189, 255), -} - -BUTTON_PRESSED_BACKGROUND_COLORS = { - ButtonStyle.NORMAL: rl.Color(74, 74, 74, 255), - ButtonStyle.PRIMARY: rl.Color(48, 73, 244, 255), - ButtonStyle.DANGER: rl.Color(255, 36, 36, 255), - ButtonStyle.TRANSPARENT: rl.BLACK, - ButtonStyle.TRANSPARENT_WHITE_TEXT: rl.BLANK, - ButtonStyle.TRANSPARENT_WHITE_BORDER: rl.BLANK, - ButtonStyle.ACTION: rl.Color(130, 130, 130, 255), - ButtonStyle.LIST_ACTION: rl.Color(74, 74, 74, 74), - ButtonStyle.NO_EFFECT: rl.Color(51, 51, 51, 255), - ButtonStyle.KEYBOARD: rl.Color(51, 51, 51, 255), - ButtonStyle.FORGET_WIFI: rl.Color(130, 130, 130, 255), -} - -BUTTON_DISABLED_BACKGROUND_COLORS = { - ButtonStyle.TRANSPARENT_WHITE_TEXT: rl.BLANK, -} - - -class Button(Widget): - def __init__(self, - text: str | Callable[[], str], - click_callback: Callable[[], None] | None = None, - font_size: int = DEFAULT_BUTTON_FONT_SIZE, - font_weight: FontWeight = FontWeight.MEDIUM, - button_style: ButtonStyle = ButtonStyle.NORMAL, - border_radius: int = 10, - text_alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - text_padding: int = 20, - icon=None, - elide_right: bool = False, - multi_touch: bool = False, - ): - - super().__init__() - self._button_style = button_style - self._border_radius = border_radius - self._background_color = BUTTON_BACKGROUND_COLORS[self._button_style] - - self._label = Label(text, font_size, font_weight, text_alignment, text_padding=text_padding, - text_color=BUTTON_TEXT_COLOR[self._button_style], icon=icon, elide_right=elide_right) - - self._click_callback = click_callback - self._multi_touch = multi_touch - - def set_text(self, text): - self._label.set_text(text) - - def set_button_style(self, button_style: ButtonStyle): - self._button_style = button_style - self._background_color = BUTTON_BACKGROUND_COLORS[self._button_style] - self._label.set_text_color(BUTTON_TEXT_COLOR[self._button_style]) - - def _update_state(self): - if self.enabled: - self._label.set_text_color(BUTTON_TEXT_COLOR[self._button_style]) - if self.is_pressed: - self._background_color = BUTTON_PRESSED_BACKGROUND_COLORS[self._button_style] - else: - self._background_color = BUTTON_BACKGROUND_COLORS[self._button_style] - elif self._button_style != ButtonStyle.NO_EFFECT: - self._background_color = BUTTON_DISABLED_BACKGROUND_COLORS.get(self._button_style, rl.Color(51, 51, 51, 255)) - self._label.set_text_color(BUTTON_DISABLED_TEXT_COLORS.get(self._button_style, rl.Color(228, 228, 228, 51))) - - def _render(self, _): - roundness = self._border_radius / (min(self._rect.width, self._rect.height) / 2) - if self._button_style == ButtonStyle.TRANSPARENT_WHITE_BORDER: - rl.draw_rectangle_rounded(self._rect, roundness, 10, rl.BLACK) - rl.draw_rectangle_rounded_lines_ex(self._rect, roundness, 10, 2, rl.WHITE) - else: - rl.draw_rectangle_rounded(self._rect, roundness, 10, self._background_color) - self._label.render(self._rect) - - -class ButtonRadio(Button): - def __init__(self, - text: str, - icon, - click_callback: Callable[[], None] | None = None, - font_size: int = DEFAULT_BUTTON_FONT_SIZE, - text_alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - border_radius: int = 10, - text_padding: int = 20, - ): - - super().__init__(text, click_callback=click_callback, font_size=font_size, - border_radius=border_radius, text_padding=text_padding, - text_alignment=text_alignment) - self._text_padding = text_padding - self._icon = icon - self.selected = False - - def _handle_mouse_release(self, mouse_pos: MousePos): - super()._handle_mouse_release(mouse_pos) - self.selected = not self.selected - - def _update_state(self): - if self.selected: - self._background_color = BUTTON_BACKGROUND_COLORS[ButtonStyle.PRIMARY] - else: - self._background_color = BUTTON_BACKGROUND_COLORS[ButtonStyle.NORMAL] - - def _render(self, _): - roundness = self._border_radius / (min(self._rect.width, self._rect.height) / 2) - rl.draw_rectangle_rounded(self._rect, roundness, 10, self._background_color) - self._label.render(self._rect) - - if self._icon and self.selected: - icon_y = self._rect.y + (self._rect.height - self._icon.height) / 2 - icon_x = self._rect.x + self._rect.width - self._icon.width - self._text_padding - ICON_PADDING - rl.draw_texture_v(self._icon, rl.Vector2(icon_x, icon_y), rl.WHITE if self.enabled else rl.Color(255, 255, 255, 100)) - - -class IconButton(Widget): - def __init__(self, texture: rl.Texture): - super().__init__() - self._texture = texture - self._opacity_filter = FirstOrderFilter(1.0, 0.1, 1 / gui_app.target_fps) - self.set_rect(rl.Rectangle(0, 0, self._texture.width, self._texture.height)) - - def set_opacity(self, opacity: float, smooth: bool = False): - if smooth: - self._opacity_filter.update(opacity) - else: - self._opacity_filter.x = opacity - - def _render(self, rect: rl.Rectangle): - color = rl.Color(180, 180, 180, int(150 * self._opacity_filter.x)) if self.is_pressed else rl.WHITE - if not self.enabled: - color = rl.Color(255, 255, 255, int(255 * 0.9 * 0.35 * self._opacity_filter.x)) - draw_x = rect.x + (rect.width - self._texture.width) / 2 - draw_y = rect.y + (rect.height - self._texture.height) / 2 - rl.draw_texture(self._texture, int(draw_x), int(draw_y), color) - - -class SmallCircleIconButton(Widget): - def __init__(self, icon_txt: rl.Texture): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, 100, 100)) - self._opacity_filter = FirstOrderFilter(1.0, 0.1, 1 / gui_app.target_fps) - self._icon_bg_txt = gui_app.texture("icons_mici/setup/small_button.png", 100, 100) - self._icon_bg_pressed_txt = gui_app.texture("icons_mici/setup/small_button_pressed.png", 100, 100) - self._icon_bg_disabled_txt = gui_app.texture("icons_mici/setup/small_button_disabled.png", 100, 100) - self._icon_txt = icon_txt - - def set_opacity(self, opacity: float, smooth: bool = False): - if smooth: - self._opacity_filter.update(opacity) - else: - self._opacity_filter.x = opacity - - def _render(self, _): - white = rl.Color(255, 255, 255, int(255 * self._opacity_filter.x)) - if not self.enabled: - bg_txt = self._icon_bg_disabled_txt - icon_white = rl.Color(255, 255, 255, int(white.a * 0.35)) - else: - bg_txt = self._icon_bg_pressed_txt if self.is_pressed else self._icon_bg_txt - icon_white = white - - rl.draw_texture(bg_txt, int(self.rect.x), int(self.rect.y), white) - icon_x = self.rect.x + (self.rect.width - self._icon_txt.width) / 2 - icon_y = self.rect.y + (self.rect.height - self._icon_txt.height) / 2 - rl.draw_texture(self._icon_txt, int(icon_x), int(icon_y), icon_white) - - -class SmallButton(Widget): - def __init__(self, text: str): - super().__init__() - self._opacity_filter = FirstOrderFilter(1.0, 0.1, 1 / gui_app.target_fps) - - self._load_assets() - - self._label = UnifiedLabel(text, 36, font_weight=FontWeight.MEDIUM, - text_color=rl.Color(255, 255, 255, int(255 * 0.9)), - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - self._bg_disabled_txt = None - - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 194, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/reset/small_button.png", 194, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/reset/small_button_pressed.png", 194, 100) - - def set_text(self, text: str): - self._label.set_text(text) - - def set_opacity(self, opacity: float, smooth: bool = False): - if smooth: - self._opacity_filter.update(opacity) - else: - self._opacity_filter.x = opacity - - def _render(self, _): - if not self.enabled and self._bg_disabled_txt is not None: - rl.draw_texture(self._bg_disabled_txt, int(self.rect.x), int(self.rect.y), rl.Color(255, 255, 255, int(255 * self._opacity_filter.x))) - elif self.is_pressed: - rl.draw_texture(self._bg_pressed_txt, int(self.rect.x), int(self.rect.y), rl.Color(255, 255, 255, int(255 * self._opacity_filter.x))) - else: - rl.draw_texture(self._bg_txt, int(self.rect.x), int(self.rect.y), rl.Color(255, 255, 255, int(255 * self._opacity_filter.x))) - - opacity = 0.9 if self.enabled else 0.35 - self._label.set_color(rl.Color(255, 255, 255, int(255 * opacity * self._opacity_filter.x))) - self._label.render(self._rect) - - -class SmallRedPillButton(SmallButton): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 194, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/small_red_pill.png", 194, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/small_red_pill_pressed.png", 194, 100) - - -class SmallerRoundedButton(SmallButton): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 150, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/smaller_button.png", 150, 100) - self._bg_disabled_txt = gui_app.texture("icons_mici/setup/smaller_button_disabled.png", 150, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/smaller_button_pressed.png", 150, 100) - - -class WideRoundedButton(SmallButton): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 316, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/medium_button_bg.png", 316, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/medium_button_pressed_bg.png", 316, 100) - - -class WidishRoundedButton(SmallButton): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 250, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/widish_button.png", 250, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/widish_button_pressed.png", 250, 100) - self._bg_disabled_txt = gui_app.texture("icons_mici/setup/widish_button_disabled.png", 250, 100) - - -class FullRoundedButton(SmallButton): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 520, 100)) - self._bg_txt = gui_app.texture("icons_mici/setup/reset/wide_button.png", 520, 100) - self._bg_pressed_txt = gui_app.texture("icons_mici/setup/reset/wide_button_pressed.png", 520, 100) diff --git a/system/ui/widgets/confirm_dialog.py b/system/ui/widgets/confirm_dialog.py deleted file mode 100644 index 97618660bd1eb1..00000000000000 --- a/system/ui/widgets/confirm_dialog.py +++ /dev/null @@ -1,94 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import DialogResult -from openpilot.system.ui.widgets.button import ButtonStyle, Button -from openpilot.system.ui.widgets.label import Label -from openpilot.system.ui.widgets.html_render import HtmlRenderer, ElementType -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.scroller_tici import Scroller - -OUTER_MARGIN = 200 -RICH_OUTER_MARGIN = 100 -BUTTON_HEIGHT = 160 -MARGIN = 50 -TEXT_PADDING = 10 -BACKGROUND_COLOR = rl.Color(27, 27, 27, 255) - - -class ConfirmDialog(Widget): - def __init__(self, text: str, confirm_text: str, cancel_text: str | None = None, rich: bool = False): - super().__init__() - if cancel_text is None: - cancel_text = tr("Cancel") - self._label = Label(text, 70, FontWeight.BOLD, text_color=rl.Color(201, 201, 201, 255)) - self._html_renderer = HtmlRenderer(text=text, text_size={ElementType.P: 50}, center_text=True) - self._cancel_button = Button(cancel_text, self._cancel_button_callback) - self._confirm_button = Button(confirm_text, self._confirm_button_callback, button_style=ButtonStyle.PRIMARY) - self._rich = rich - self._dialog_result = DialogResult.NO_ACTION - self._cancel_text = cancel_text - self._scroller = Scroller([self._html_renderer], line_separator=False, spacing=0) - - def set_text(self, text): - if not self._rich: - self._label.set_text(text) - else: - self._html_renderer.parse_html_content(text) - - def reset(self): - self._dialog_result = DialogResult.NO_ACTION - - def _cancel_button_callback(self): - self._dialog_result = DialogResult.CANCEL - - def _confirm_button_callback(self): - self._dialog_result = DialogResult.CONFIRM - - def _render(self, rect: rl.Rectangle): - dialog_x = OUTER_MARGIN if not self._rich else RICH_OUTER_MARGIN - dialog_y = OUTER_MARGIN if not self._rich else RICH_OUTER_MARGIN - dialog_width = gui_app.width - 2 * dialog_x - dialog_height = gui_app.height - 2 * dialog_y - dialog_rect = rl.Rectangle(dialog_x, dialog_y, dialog_width, dialog_height) - - bottom = dialog_rect.y + dialog_rect.height - button_width = (dialog_rect.width - 3 * MARGIN) // 2 - cancel_button_x = dialog_rect.x + MARGIN - confirm_button_x = dialog_rect.x + dialog_rect.width - button_width - MARGIN - button_y = bottom - BUTTON_HEIGHT - MARGIN - cancel_button = rl.Rectangle(cancel_button_x, button_y, button_width, BUTTON_HEIGHT) - confirm_button = rl.Rectangle(confirm_button_x, button_y, button_width, BUTTON_HEIGHT) - - rl.draw_rectangle_rec(dialog_rect, BACKGROUND_COLOR) - - text_rect = rl.Rectangle(dialog_rect.x + MARGIN, dialog_rect.y + TEXT_PADDING, - dialog_rect.width - 2 * MARGIN, dialog_rect.height - BUTTON_HEIGHT - MARGIN - TEXT_PADDING * 2) - if not self._rich: - self._label.render(text_rect) - else: - html_rect = rl.Rectangle(text_rect.x, text_rect.y, text_rect.width, - self._html_renderer.get_total_height(int(text_rect.width))) - self._html_renderer.set_rect(html_rect) - self._scroller.render(text_rect) - - if rl.is_key_pressed(rl.KeyboardKey.KEY_ENTER): - self._dialog_result = DialogResult.CONFIRM - elif rl.is_key_pressed(rl.KeyboardKey.KEY_ESCAPE): - self._dialog_result = DialogResult.CANCEL - - if self._cancel_text: - self._confirm_button.render(confirm_button) - self._cancel_button.render(cancel_button) - else: - full_button_width = dialog_rect.width - 2 * MARGIN - full_confirm_button = rl.Rectangle(dialog_rect.x + MARGIN, button_y, full_button_width, BUTTON_HEIGHT) - self._confirm_button.render(full_confirm_button) - - return self._dialog_result - - -def alert_dialog(message: str, button_text: str | None = None): - if button_text is None: - button_text = tr("OK") - return ConfirmDialog(message, button_text, cancel_text="") diff --git a/system/ui/widgets/html_render.py b/system/ui/widgets/html_render.py deleted file mode 100644 index 7d90d5692533d4..00000000000000 --- a/system/ui/widgets/html_render.py +++ /dev/null @@ -1,290 +0,0 @@ -import re -import pyray as rl -from dataclasses import dataclass -from enum import Enum -from typing import Any -from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE -from openpilot.system.ui.lib.multilang import tr -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.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.lib.text_measure import measure_text_cached - -LIST_INDENT_PX = 40 - - -class ElementType(Enum): - H1 = "h1" - H2 = "h2" - H3 = "h3" - H4 = "h4" - H5 = "h5" - H6 = "h6" - P = "p" - B = "b" - UL = "ul" - LI = "li" - BR = "br" - - -TAG_NAMES = '|'.join([t.value for t in ElementType]) -START_TAG_RE = re.compile(f'<({TAG_NAMES})>') -END_TAG_RE = re.compile(f'') -COMMENT_RE = re.compile(r'', flags=re.DOTALL) -DOCTYPE_RE = re.compile(r']*>') -HTML_BODY_TAGS_RE = re.compile(r']*>') -TOKEN_RE = re.compile(r']+>|<[^>]+>|[^<\s]+') - - -def is_tag(token: str) -> tuple[bool, bool, ElementType | None]: - supported_tag = bool(START_TAG_RE.fullmatch(token)) - supported_end_tag = bool(END_TAG_RE.fullmatch(token)) - tag = ElementType(token[1:-1].strip('/')) if supported_tag or supported_end_tag else None - return supported_tag, supported_end_tag, tag - - -@dataclass -class HtmlElement: - type: ElementType - content: str - font_size: int - font_weight: FontWeight - margin_top: int - margin_bottom: int - line_height: float = 0.9 # matches Qt visually, unsure why not default 1.2 - indent_level: int = 0 - - -class HtmlRenderer(Widget): - def __init__(self, file_path: str | None = None, text: str | None = None, - text_size: dict | None = None, text_color: rl.Color = rl.WHITE, center_text: bool = False): - super().__init__() - self._text_color = text_color - self._center_text = center_text - self._normal_font = gui_app.font(FontWeight.NORMAL) - self._bold_font = gui_app.font(FontWeight.BOLD) - self._indent_level = 0 - - if text_size is None: - text_size = {} - - self._cached_height: float | None = None - self._cached_width: int = -1 - - # Base paragraph size (Qt stylesheet default is 48px in offroad alerts) - base_p_size = int(text_size.get(ElementType.P, 48)) - - # Untagged text defaults to

    - self.styles: dict[ElementType, dict[str, Any]] = { - ElementType.H1: {"size": round(base_p_size * 2), "weight": FontWeight.BOLD, "margin_top": 20, "margin_bottom": 16}, - ElementType.H2: {"size": round(base_p_size * 1.50), "weight": FontWeight.BOLD, "margin_top": 24, "margin_bottom": 12}, - ElementType.H3: {"size": round(base_p_size * 1.17), "weight": FontWeight.BOLD, "margin_top": 20, "margin_bottom": 10}, - ElementType.H4: {"size": round(base_p_size * 1.00), "weight": FontWeight.BOLD, "margin_top": 16, "margin_bottom": 8}, - ElementType.H5: {"size": round(base_p_size * 0.83), "weight": FontWeight.BOLD, "margin_top": 12, "margin_bottom": 6}, - ElementType.H6: {"size": round(base_p_size * 0.67), "weight": FontWeight.BOLD, "margin_top": 10, "margin_bottom": 4}, - ElementType.P: {"size": base_p_size, "weight": FontWeight.NORMAL, "margin_top": 8, "margin_bottom": 12}, - ElementType.B: {"size": base_p_size, "weight": FontWeight.BOLD, "margin_top": 8, "margin_bottom": 12}, - ElementType.LI: {"size": base_p_size, "weight": FontWeight.NORMAL, "color": rl.Color(40, 40, 40, 255), "margin_top": 6, "margin_bottom": 6}, - ElementType.BR: {"size": 0, "weight": FontWeight.NORMAL, "margin_top": 0, "margin_bottom": 12}, - } - - self.elements: list[HtmlElement] = [] - if file_path is not None: - self.parse_html_file(file_path) - elif text is not None: - self.parse_html_content(text) - else: - raise ValueError("Either file_path or text must be provided") - - def parse_html_file(self, file_path: str) -> None: - with open(file_path, encoding='utf-8') as file: - content = file.read() - self.parse_html_content(content) - - def parse_html_content(self, html_content: str) -> None: - self.elements.clear() - self._cached_height = None - self._cached_width = -1 - - # Remove HTML comments - html_content = COMMENT_RE.sub('', html_content) - - # Remove DOCTYPE, html, head, body tags but keep their content - html_content = DOCTYPE_RE.sub('', html_content) - html_content = HTML_BODY_TAGS_RE.sub('', html_content) - - # Parse HTML - tokens = TOKEN_RE.findall(html_content) - - def close_tag(): - nonlocal current_content - nonlocal current_tag - - # If no tag is set, default to paragraph so we don't lose text - if current_tag is None: - current_tag = ElementType.P - - text = ' '.join(current_content).strip() - current_content = [] - if text: - if current_tag == ElementType.LI: - text = '• ' + text - self._add_element(current_tag, text) - - current_content: list[str] = [] - current_tag: ElementType | None = None - for token in tokens: - is_start_tag, is_end_tag, tag = is_tag(token) - if tag is not None: - if tag == ElementType.BR: - # Close current tag and add a line break - close_tag() - self._add_element(ElementType.BR, "") - - elif is_start_tag or is_end_tag: - # Always add content regardless of opening or closing tag - close_tag() - - if is_start_tag: - current_tag = tag - else: - current_tag = None - - # increment after we add the content for the current tag - if tag == ElementType.UL: - self._indent_level = self._indent_level + 1 if is_start_tag else max(0, self._indent_level - 1) - - else: - current_content.append(token) - - if current_content: - close_tag() - - def _add_element(self, element_type: ElementType, content: str) -> None: - style = self.styles[element_type] - - element = HtmlElement( - type=element_type, - content=content, - font_size=style["size"], - font_weight=style["weight"], - margin_top=style["margin_top"], - margin_bottom=style["margin_bottom"], - indent_level=self._indent_level, - ) - - self.elements.append(element) - - def _render(self, rect: rl.Rectangle): - # TODO: speed up by removing duplicate calculations across renders - current_y = rect.y - padding = 20 - content_width = rect.width - (padding * 2) - - for element in self.elements: - if element.type == ElementType.BR: - current_y += element.margin_bottom - continue - - current_y += element.margin_top - if current_y > rect.y + rect.height: - break - - if element.content: - font = self._get_font(element.font_weight) - wrapped_lines = wrap_text(font, element.content, element.font_size, int(content_width)) - - for line in wrapped_lines: - # Use FONT_SCALE from wrapped raylib text functions to match what is drawn - if current_y < rect.y - element.font_size * FONT_SCALE: - current_y += element.font_size * FONT_SCALE * element.line_height - continue - - if current_y > rect.y + rect.height: - break - - if self._center_text: - text_width = measure_text_cached(font, line, element.font_size).x - text_x = rect.x + (rect.width - text_width) / 2 - else: # left align - text_x = rect.x + (max(element.indent_level - 1, 0) * LIST_INDENT_PX) - - rl.draw_text_ex(font, line, rl.Vector2(text_x + padding, current_y), element.font_size, 0, self._text_color) - - current_y += element.font_size * FONT_SCALE * element.line_height - - # Apply bottom margin - current_y += element.margin_bottom - - return current_y - rect.y - - def get_total_height(self, content_width: int) -> float: - if self._cached_height is not None and self._cached_width == content_width: - return self._cached_height - - total_height = 0.0 - padding = 20 - usable_width = content_width - (padding * 2) - - for element in self.elements: - if element.type == ElementType.BR: - total_height += element.margin_bottom - continue - - total_height += element.margin_top - - if element.content: - font = self._get_font(element.font_weight) - wrapped_lines = wrap_text(font, element.content, element.font_size, int(usable_width)) - - for _ in wrapped_lines: - total_height += element.font_size * FONT_SCALE * element.line_height - - total_height += element.margin_bottom - - # Store result in cache - self._cached_height = total_height - self._cached_width = content_width - - return total_height - - def _get_font(self, weight: FontWeight): - if weight == FontWeight.BOLD: - return self._bold_font - return self._normal_font - - -class HtmlModal(Widget): - def __init__(self, file_path: str | None = None, text: str | None = None): - super().__init__() - self._content = HtmlRenderer(file_path=file_path, text=text) - self._scroll_panel = GuiScrollPanel() - self._ok_button = Button(tr("OK"), click_callback=lambda: gui_app.set_modal_overlay(None), button_style=ButtonStyle.PRIMARY) - - def _render(self, rect: rl.Rectangle): - margin = 50 - content_rect = rl.Rectangle(rect.x + margin, rect.y + margin, rect.width - (margin * 2), rect.height - (margin * 2)) - - button_height = 160 - button_spacing = 20 - scrollable_height = content_rect.height - button_height - button_spacing - - scrollable_rect = rl.Rectangle(content_rect.x, content_rect.y, content_rect.width, scrollable_height) - - total_height = self._content.get_total_height(int(scrollable_rect.width)) - scroll_content_rect = rl.Rectangle(scrollable_rect.x, scrollable_rect.y, scrollable_rect.width, total_height) - scroll_offset = self._scroll_panel.update(scrollable_rect, scroll_content_rect) - scroll_content_rect.y += scroll_offset - - rl.begin_scissor_mode(int(scrollable_rect.x), int(scrollable_rect.y), int(scrollable_rect.width), int(scrollable_rect.height)) - self._content.render(scroll_content_rect) - rl.end_scissor_mode() - - button_width = (rect.width - 3 * 50) // 3 - button_x = content_rect.x + content_rect.width - button_width - button_y = content_rect.y + content_rect.height - button_height - button_rect = rl.Rectangle(button_x, button_y, button_width, button_height) - self._ok_button.render(button_rect) - - return -1 diff --git a/system/ui/widgets/inputbox.py b/system/ui/widgets/inputbox.py deleted file mode 100644 index f53e3f0ebb188b..00000000000000 --- a/system/ui/widgets/inputbox.py +++ /dev/null @@ -1,228 +0,0 @@ -import pyray as rl -import time -from openpilot.system.ui.lib.application import gui_app, MousePos, FONT_SCALE -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget - -PASSWORD_MASK_CHAR = "•" -PASSWORD_MASK_DELAY = 1.5 # Seconds to show character before masking - - -class InputBox(Widget): - def __init__(self, max_text_size=255, password_mode=False): - super().__init__() - self._max_text_size = max_text_size - self._input_text = "" - self._cursor_position = 0 - self._password_mode = password_mode - self._blink_counter = 0 - self._show_cursor = False - self._last_key_pressed = 0 - self._key_press_time = 0 - self._repeat_delay = 30 - self._repeat_rate = 4 - self._text_offset = 0 - self._visible_width = 0 - self._last_char_time = 0 # Track when last character was added - self._masked_length = 0 # How many characters are currently masked - - @property - def text(self): - return self._input_text - - @text.setter - def text(self, value): - self._input_text = value[: self._max_text_size] - self._cursor_position = len(self._input_text) - self._update_text_offset() - - def set_password_mode(self, password_mode): - self._password_mode = password_mode - - def clear(self): - self._input_text = '' - self._cursor_position = 0 - self._text_offset = 0 - - def set_cursor_position(self, position): - """Set the cursor position and reset the blink counter.""" - if 0 <= position <= len(self._input_text): - self._cursor_position = position - self._blink_counter = 0 - self._show_cursor = True - self._update_text_offset() - - def _update_text_offset(self): - """Ensure the cursor is visible by adjusting text offset.""" - if self._visible_width == 0: - return - - font = gui_app.font() - display_text = self._get_display_text() - padding = 10 - - if self._cursor_position > 0: - cursor_x = measure_text_cached(font, display_text[: self._cursor_position], self._font_size).x - else: - cursor_x = 0 - - visible_width = self._visible_width - (padding * 2) - - # Adjust offset if cursor would be outside visible area - if cursor_x < self._text_offset: - self._text_offset = max(0, cursor_x - padding) - elif cursor_x > self._text_offset + visible_width: - self._text_offset = cursor_x - visible_width + padding - - def add_char_at_cursor(self, char): - """Add a character at the current cursor position.""" - if len(self._input_text) < self._max_text_size: - self._input_text = self._input_text[: self._cursor_position] + char + self._input_text[self._cursor_position:] - self.set_cursor_position(self._cursor_position + 1) - - if self._password_mode: - self._last_char_time = time.monotonic() - - return True - return False - - def delete_char_before_cursor(self): - """Delete the character before the cursor position (backspace).""" - if self._cursor_position > 0: - self._input_text = self._input_text[: self._cursor_position - 1] + self._input_text[self._cursor_position:] - self.set_cursor_position(self._cursor_position - 1) - return True - return False - - def delete_char_at_cursor(self): - """Delete the character at the cursor position (delete).""" - if self._cursor_position < len(self._input_text): - self._input_text = self._input_text[: self._cursor_position] + self._input_text[self._cursor_position + 1:] - self.set_cursor_position(self._cursor_position) - return True - return False - - def _render(self, rect, color=rl.BLACK, border_color=rl.DARKGRAY, text_color=rl.WHITE, font_size=80): - # Store dimensions for text offset calculations - self._visible_width = rect.width - self._font_size = font_size - - # Draw input box - rl.draw_rectangle_rec(rect, color) - - # Process keyboard input - self._handle_keyboard_input() - - # Update cursor blink - self._blink_counter += 1 - if self._blink_counter >= 30: - self._show_cursor = not self._show_cursor - self._blink_counter = 0 - - # Display text - font = gui_app.font() - display_text = self._get_display_text() - padding = 10 - - # Clip text within input box bounds - buffer = 2 - rl.begin_scissor_mode(int(rect.x + padding - buffer), int(rect.y), int(rect.width - padding * 2 + buffer * 2), int(rect.height)) - rl.draw_text_ex( - font, - display_text, - rl.Vector2(int(rect.x + padding - self._text_offset), int(rect.y + rect.height / 2 - font_size * FONT_SCALE / 2)), - font_size, - 0, - text_color, - ) - - # Draw cursor - if self._show_cursor: - cursor_x = rect.x + padding - if len(display_text) > 0 and self._cursor_position > 0: - cursor_x += measure_text_cached(font, display_text[: self._cursor_position], font_size).x - - # Apply text offset to cursor position - cursor_x -= self._text_offset - - cursor_height = font_size * FONT_SCALE + 4 - cursor_y = rect.y + rect.height / 2 - cursor_height / 2 - rl.draw_line(int(cursor_x), int(cursor_y), int(cursor_x), int(cursor_y + cursor_height), rl.WHITE) - - rl.end_scissor_mode() - - def _get_display_text(self): - """Get text to display, applying password masking with delay if needed.""" - if not self._password_mode: - return self._input_text - - # Show character at last edited position if within delay window - masked_text = PASSWORD_MASK_CHAR * len(self._input_text) - recent_edit = time.monotonic() - self._last_char_time < PASSWORD_MASK_DELAY - if recent_edit and self._input_text: - last_pos = max(0, self._cursor_position - 1) - if last_pos < len(self._input_text): - return masked_text[:last_pos] + self._input_text[last_pos] + masked_text[last_pos + 1:] - - return masked_text - - def _handle_mouse_release(self, mouse_pos: MousePos): - # Calculate cursor position from click - if len(self._input_text) > 0: - font = gui_app.font() - display_text = self._get_display_text() - - # Find the closest character position to the click - relative_x = mouse_pos.x - (self._rect.x + 10) + self._text_offset - best_pos = 0 - min_distance = float('inf') - - for i in range(len(self._input_text) + 1): - char_width = measure_text_cached(font, display_text[:i], self._font_size).x - distance = abs(relative_x - char_width) - if distance < min_distance: - min_distance = distance - best_pos = i - - self.set_cursor_position(best_pos) - else: - self.set_cursor_position(0) - - def _handle_keyboard_input(self): - # Handle navigation keys - key = rl.get_key_pressed() - if key != 0: - self._process_key(key) - if key in (rl.KEY_LEFT, rl.KEY_RIGHT, rl.KEY_BACKSPACE, rl.KEY_DELETE): - self._last_key_pressed = key - self._key_press_time = 0 - - # Handle repeats for held keys - elif self._last_key_pressed != 0: - if rl.is_key_down(self._last_key_pressed): - self._key_press_time += 1 - if self._key_press_time > self._repeat_delay and self._key_press_time % self._repeat_rate == 0: - self._process_key(self._last_key_pressed) - else: - self._last_key_pressed = 0 - - # Handle text input - char = rl.get_char_pressed() - if char != 0 and char >= 32: # Filter out control characters - self.add_char_at_cursor(chr(char)) - - def _process_key(self, key): - if key == rl.KEY_LEFT: - if self._cursor_position > 0: - self.set_cursor_position(self._cursor_position - 1) - elif key == rl.KEY_RIGHT: - if self._cursor_position < len(self._input_text): - self.set_cursor_position(self._cursor_position + 1) - elif key == rl.KEY_BACKSPACE: - self.delete_char_before_cursor() - elif key == rl.KEY_DELETE: - self.delete_char_at_cursor() - elif key == rl.KEY_HOME: - self.set_cursor_position(0) - elif key == rl.KEY_END: - self.set_cursor_position(len(self._input_text)) diff --git a/system/ui/widgets/keyboard.py b/system/ui/widgets/keyboard.py deleted file mode 100644 index 4ec92f507a7c44..00000000000000 --- a/system/ui/widgets/keyboard.py +++ /dev/null @@ -1,273 +0,0 @@ -from functools import partial -import time -from typing import Literal - -import pyray as rl - -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import ButtonStyle, Button -from openpilot.system.ui.widgets.inputbox import InputBox -from openpilot.system.ui.widgets.label import Label - -KEY_FONT_SIZE = 96 -DOUBLE_CLICK_THRESHOLD = 0.5 # seconds -DELETE_REPEAT_DELAY = 0.5 -DELETE_REPEAT_INTERVAL = 0.07 - -# Constants for special keys -CONTENT_MARGIN = 50 -BACKSPACE_KEY = "<-" -ENTER_KEY = "->" -SPACE_KEY = " " -SHIFT_INACTIVE_KEY = "SHIFT_OFF" -SHIFT_ACTIVE_KEY = "SHIFT_ON" -CAPS_LOCK_KEY = "CAPS" -NUMERIC_KEY = "123" -SYMBOL_KEY = "#+=" -ABC_KEY = "ABC" - -# Define keyboard layouts as a dictionary for easier access -KEYBOARD_LAYOUTS = { - "lowercase": [ - ["q", "w", "e", "r", "t", "y", "u", "i", "o", "p"], - ["a", "s", "d", "f", "g", "h", "j", "k", "l"], - [SHIFT_INACTIVE_KEY, "z", "x", "c", "v", "b", "n", "m", BACKSPACE_KEY], - [NUMERIC_KEY, "/", "-", SPACE_KEY, ".", ENTER_KEY], - ], - "uppercase": [ - ["Q", "W", "E", "R", "T", "Y", "U", "I", "O", "P"], - ["A", "S", "D", "F", "G", "H", "J", "K", "L"], - [SHIFT_ACTIVE_KEY, "Z", "X", "C", "V", "B", "N", "M", BACKSPACE_KEY], - [NUMERIC_KEY, "/", "-", SPACE_KEY, ".", ENTER_KEY], - ], - "numbers": [ - ["1", "2", "3", "4", "5", "6", "7", "8", "9", "0"], - ["-", "/", ":", ";", "(", ")", "$", "&", "@", "\""], - [SYMBOL_KEY, "_", ",", "?", "!", "`", BACKSPACE_KEY], - [ABC_KEY, SPACE_KEY, ".", ENTER_KEY], - ], - "specials": [ - ["[", "]", "{", "}", "#", "%", "^", "*", "+", "="], - ["_", "\\", "|", "~", "<", ">", "€", "£", "¥", "•"], - [NUMERIC_KEY, "-", ",", "?", "!", "'", BACKSPACE_KEY], - [ABC_KEY, SPACE_KEY, ".", ENTER_KEY], - ], -} - - -class Keyboard(Widget): - def __init__(self, max_text_size: int = 255, min_text_size: int = 0, password_mode: bool = False, show_password_toggle: bool = False): - super().__init__() - self._layout_name: Literal["lowercase", "uppercase", "numbers", "specials"] = "lowercase" - self._caps_lock = False - self._last_shift_press_time = 0 - self._title = Label("", 90, FontWeight.BOLD, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - self._sub_title = Label("", 55, FontWeight.NORMAL, rl.GuiTextAlignment.TEXT_ALIGN_LEFT, text_padding=20) - - self._max_text_size = max_text_size - self._min_text_size = min_text_size - self._input_box = InputBox(max_text_size) - self._password_mode = password_mode - self._show_password_toggle = show_password_toggle - - # Backspace key repeat tracking - self._backspace_pressed: bool = False - self._backspace_press_time: float = 0.0 - self._backspace_last_repeat: float = 0.0 - - self._render_return_status = -1 - self._cancel_button = Button(lambda: tr("Cancel"), self._cancel_button_callback) - - self._eye_button = Button("", self._eye_button_callback, button_style=ButtonStyle.TRANSPARENT) - - self._eye_open_texture = gui_app.texture("icons/eye_open.png", 81, 54) - self._eye_closed_texture = gui_app.texture("icons/eye_closed.png", 81, 54) - self._key_icons = { - BACKSPACE_KEY: gui_app.texture("icons/backspace.png", 80, 80), - SHIFT_INACTIVE_KEY: gui_app.texture("icons/shift.png", 80, 80), - SHIFT_ACTIVE_KEY: gui_app.texture("icons/shift-fill.png", 80, 80), - CAPS_LOCK_KEY: gui_app.texture("icons/capslock-fill.png", 80, 80), - ENTER_KEY: gui_app.texture("icons/arrow-right.png", 80, 80), - } - - self._all_keys = {} - for l in KEYBOARD_LAYOUTS: - for _, keys in enumerate(KEYBOARD_LAYOUTS[l]): - for _, key in enumerate(keys): - if key in self._key_icons: - texture = self._key_icons[key] - self._all_keys[key] = Button("", partial(self._key_callback, key), icon=texture, - button_style=ButtonStyle.PRIMARY if key == ENTER_KEY else ButtonStyle.KEYBOARD, multi_touch=True) - else: - self._all_keys[key] = Button(key, partial(self._key_callback, key), button_style=ButtonStyle.KEYBOARD, font_size=85, multi_touch=True) - self._all_keys[CAPS_LOCK_KEY] = Button("", partial(self._key_callback, CAPS_LOCK_KEY), icon=self._key_icons[CAPS_LOCK_KEY], - button_style=ButtonStyle.KEYBOARD, multi_touch=True) - - def set_text(self, text: str): - self._input_box.text = text - - @property - def text(self): - return self._input_box.text - - def clear(self): - self._layout_name = "lowercase" - self._caps_lock = False - self._input_box.clear() - self._backspace_pressed = False - - def set_title(self, title: str, sub_title: str = ""): - self._title.set_text(title) - self._sub_title.set_text(sub_title) - - def _eye_button_callback(self): - self._password_mode = not self._password_mode - - def _cancel_button_callback(self): - self.clear() - self._render_return_status = 0 - - def _key_callback(self, k): - if k == ENTER_KEY: - self._render_return_status = 1 - else: - self.handle_key_press(k) - - def _render(self, rect: rl.Rectangle): - rect = rl.Rectangle(rect.x + CONTENT_MARGIN, rect.y + CONTENT_MARGIN, rect.width - 2 * CONTENT_MARGIN, rect.height - 2 * CONTENT_MARGIN) - self._title.render(rl.Rectangle(rect.x, rect.y, rect.width, 95)) - self._sub_title.render(rl.Rectangle(rect.x, rect.y + 95, rect.width, 60)) - self._cancel_button.render(rl.Rectangle(rect.x + rect.width - 386, rect.y, 386, 125)) - - # Draw input box and password toggle - input_margin = 25 - input_box_rect = rl.Rectangle(rect.x + input_margin, rect.y + 160, rect.width - input_margin, 100) - self._render_input_area(input_box_rect) - - # Process backspace key repeat if it's held down - if not self._all_keys[BACKSPACE_KEY].is_pressed: - self._backspace_pressed = False - - if self._backspace_pressed: - current_time = time.monotonic() - time_since_press = current_time - self._backspace_press_time - - # After initial delay, start repeating with shorter intervals - if time_since_press > DELETE_REPEAT_DELAY: - time_since_last_repeat = current_time - self._backspace_last_repeat - if time_since_last_repeat > DELETE_REPEAT_INTERVAL: - self._input_box.delete_char_before_cursor() - self._backspace_last_repeat = current_time - - layout = KEYBOARD_LAYOUTS[self._layout_name] - - h_space, v_space = 15, 15 - row_y_start = rect.y + 300 # Starting Y position for the first row - key_height = (rect.height - 300 - 3 * v_space) / 4 - key_max_width = (rect.width - (len(layout[2]) - 1) * h_space) / len(layout[2]) - - # Iterate over the rows of keys in the current layout - for row, keys in enumerate(layout): - key_width = min((rect.width - (180 if row == 1 else 0) - h_space * (len(keys) - 1)) / len(keys), key_max_width) - start_x = rect.x + (90 if row == 1 else 0) - - for i, key in enumerate(keys): - if i > 0: - start_x += h_space - - new_width = (key_width * 3 + h_space * 2) if key == SPACE_KEY else (key_width * 2 + h_space if key == ENTER_KEY else key_width) - key_rect = rl.Rectangle(start_x, row_y_start + row * (key_height + v_space), new_width, key_height) - start_x += new_width - - is_enabled = key != ENTER_KEY or len(self._input_box.text) >= self._min_text_size - - if key == BACKSPACE_KEY and self._all_keys[BACKSPACE_KEY].is_pressed and not self._backspace_pressed: - self._backspace_pressed = True - self._backspace_press_time = time.monotonic() - self._backspace_last_repeat = time.monotonic() - - if key in self._key_icons: - if key == SHIFT_ACTIVE_KEY and self._caps_lock: - key = CAPS_LOCK_KEY - self._all_keys[key].set_enabled(is_enabled) - self._all_keys[key].render(key_rect) - else: - self._all_keys[key].set_enabled(is_enabled) - self._all_keys[key].render(key_rect) - - return self._render_return_status - - def _render_input_area(self, input_rect: rl.Rectangle): - if self._show_password_toggle: - self._input_box.set_password_mode(self._password_mode) - self._input_box.render(rl.Rectangle(input_rect.x, input_rect.y, input_rect.width - 100, input_rect.height)) - - # render eye icon - eye_texture = self._eye_closed_texture if self._password_mode else self._eye_open_texture - - eye_rect = rl.Rectangle(input_rect.x + input_rect.width - 90, input_rect.y, 80, input_rect.height) - self._eye_button.render(eye_rect) - - eye_x = eye_rect.x + (eye_rect.width - eye_texture.width) / 2 - eye_y = eye_rect.y + (eye_rect.height - eye_texture.height) / 2 - - rl.draw_texture_v(eye_texture, rl.Vector2(eye_x, eye_y), rl.WHITE) - else: - self._input_box.render(input_rect) - - rl.draw_line_ex( - rl.Vector2(input_rect.x, input_rect.y + input_rect.height - 2), - rl.Vector2(input_rect.x + input_rect.width, input_rect.y + input_rect.height - 2), - 3.0, # 3 pixel thickness - rl.Color(189, 189, 189, 255), - ) - - def handle_key_press(self, key): - if key in (CAPS_LOCK_KEY, ABC_KEY): - self._caps_lock = False - self._layout_name = "lowercase" - elif key == SHIFT_INACTIVE_KEY: - self._last_shift_press_time = time.monotonic() - self._layout_name = "uppercase" - elif key == SHIFT_ACTIVE_KEY: - if time.monotonic() - self._last_shift_press_time < DOUBLE_CLICK_THRESHOLD: - self._caps_lock = True - else: - self._layout_name = "lowercase" - elif key == NUMERIC_KEY: - self._layout_name = "numbers" - elif key == SYMBOL_KEY: - self._layout_name = "specials" - elif key == BACKSPACE_KEY: - self._input_box.delete_char_before_cursor() - else: - self._input_box.add_char_at_cursor(key) - if not self._caps_lock and self._layout_name == "uppercase": - self._layout_name = "lowercase" - - def reset(self, min_text_size: int | None = None): - if min_text_size is not None: - self._min_text_size = min_text_size - self._render_return_status = -1 - self._last_shift_press_time = 0 - self._backspace_pressed = False - self._backspace_press_time = 0.0 - self._backspace_last_repeat = 0.0 - self.clear() - - -if __name__ == "__main__": - gui_app.init_window("Keyboard") - keyboard = Keyboard(min_text_size=8, show_password_toggle=True) - for _ in gui_app.render(): - keyboard.set_title("Keyboard Input", "Type your text below") - result = keyboard.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - if result == 1: - print(f"You typed: {keyboard.text}") - gui_app.request_close() - elif result == 0: - print("Canceled") - gui_app.request_close() - gui_app.close() diff --git a/system/ui/widgets/label.py b/system/ui/widgets/label.py deleted file mode 100644 index cb0cf66b144e53..00000000000000 --- a/system/ui/widgets/label.py +++ /dev/null @@ -1,796 +0,0 @@ -from enum import IntEnum -from collections.abc import Callable -from itertools import zip_longest -from typing import Union -import pyray as rl - -from openpilot.system.ui.lib.application import gui_app, FontWeight, DEFAULT_TEXT_SIZE, DEFAULT_TEXT_COLOR, FONT_SCALE -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.lib.utils import GuiStyleContext -from openpilot.system.ui.lib.emoji import find_emoji, emoji_tex -from openpilot.system.ui.lib.wrap_text import wrap_text - -ICON_PADDING = 15 - - -# TODO: make this common -def _resolve_value(value, default=""): - if callable(value): - return value() - return value if value is not None else default - - -class ScrollState(IntEnum): - STARTING = 0 - SCROLLING = 1 - - -# TODO: merge anything new here to master -class MiciLabel(Widget): - def __init__(self, - text: str, - font_size: int = DEFAULT_TEXT_SIZE, - width: int | None = None, - color: rl.Color = DEFAULT_TEXT_COLOR, - font_weight: FontWeight = FontWeight.NORMAL, - alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, - spacing: int = 0, - line_height: int | None = None, - elide_right: bool = True, - wrap_text: bool = False, - scroll: bool = False): - super().__init__() - self.text = text - self.wrapped_text: list[str] = [] - self.font_size = font_size - self.width = width - self.color = color - self.font_weight = font_weight - self.alignment = alignment - self.alignment_vertical = alignment_vertical - self.spacing = spacing - self.line_height = line_height if line_height is not None else font_size - self.elide_right = elide_right - self.wrap_text = wrap_text - self._height = 0 - - # Scroll state - self.scroll = scroll - self._needs_scroll = False - self._scroll_offset = 0 - self._scroll_pause_t: float | None = None - self._scroll_state: ScrollState = ScrollState.STARTING - - assert not (self.scroll and self.wrap_text), "Cannot enable both scroll and wrap_text" - assert not (self.scroll and self.elide_right), "Cannot enable both scroll and elide_right" - - self.set_text(text) - - @property - def text_height(self): - return self._height - - def set_font_size(self, font_size: int): - self.font_size = font_size - self.set_text(self.text) - - def set_width(self, width: int): - self.width = width - self._rect.width = width - self.set_text(self.text) - - def set_text(self, txt: str): - self.text = txt - text_size = measure_text_cached(gui_app.font(self.font_weight), self.text, self.font_size, self.spacing) - if self.width is not None: - self._rect.width = self.width - else: - self._rect.width = text_size.x - - if self.wrap_text: - self.wrapped_text = wrap_text(gui_app.font(self.font_weight), self.text, self.font_size, int(self._rect.width)) - self._height = len(self.wrapped_text) * self.line_height - elif self.scroll: - self._needs_scroll = self.scroll and text_size.x > self._rect.width - self._rect.height = text_size.y - - def set_color(self, color: rl.Color): - self.color = color - - def set_font_weight(self, font_weight: FontWeight): - self.font_weight = font_weight - self.set_text(self.text) - - def _render(self, rect: rl.Rectangle): - # Only scissor when we know there is a single scrolling line - if self._needs_scroll: - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - - font = gui_app.font(self.font_weight) - - text_y_offset = 0 - # Draw the text in the specified rectangle - lines = self.wrapped_text or [self.text] - if self.alignment_vertical == rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: - lines = lines[::-1] - - for display_text in lines: - text_size = measure_text_cached(font, display_text, self.font_size, self.spacing) - - # Elide text to fit within the rectangle - if self.elide_right and text_size.x > rect.width: - ellipsis = "..." - left, right = 0, len(display_text) - while left < right: - mid = (left + right) // 2 - candidate = display_text[:mid] + ellipsis - candidate_size = measure_text_cached(font, candidate, self.font_size, self.spacing) - if candidate_size.x <= rect.width: - left = mid + 1 - else: - right = mid - display_text = display_text[: left - 1] + ellipsis if left > 0 else ellipsis - text_size = measure_text_cached(font, display_text, self.font_size, self.spacing) - - # Handle scroll state - elif self.scroll and self._needs_scroll: - if self._scroll_state == ScrollState.STARTING: - if self._scroll_pause_t is None: - self._scroll_pause_t = rl.get_time() + 2.0 - if rl.get_time() >= self._scroll_pause_t: - self._scroll_state = ScrollState.SCROLLING - self._scroll_pause_t = None - - elif self._scroll_state == ScrollState.SCROLLING: - self._scroll_offset -= 0.8 / 60. * gui_app.target_fps - # don't fully hide - if self._scroll_offset <= -text_size.x - self._rect.width / 3: - self._scroll_offset = 0 - self._scroll_state = ScrollState.STARTING - self._scroll_pause_t = None - - # Calculate horizontal position based on alignment - text_x = rect.x + { - rl.GuiTextAlignment.TEXT_ALIGN_LEFT: 0, - rl.GuiTextAlignment.TEXT_ALIGN_CENTER: (rect.width - text_size.x) / 2, - rl.GuiTextAlignment.TEXT_ALIGN_RIGHT: rect.width - text_size.x, - }.get(self.alignment, 0) + self._scroll_offset - - # Calculate vertical position based on alignment - text_y = rect.y + { - rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP: 0, - rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE: (rect.height - text_size.y) / 2, - rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: rect.height - text_size.y, - }.get(self.alignment_vertical, 0) - text_y += text_y_offset - - rl.draw_text_ex(font, display_text, rl.Vector2(round(text_x), text_y), self.font_size, self.spacing, self.color) - # Draw 2nd instance for scrolling - if self._needs_scroll and self._scroll_state != ScrollState.STARTING: - text2_scroll_offset = text_size.x + self._rect.width / 3 - rl.draw_text_ex(font, display_text, rl.Vector2(round(text_x + text2_scroll_offset), text_y), self.font_size, self.spacing, self.color) - if self.alignment_vertical == rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: - text_y_offset -= self.line_height - else: - text_y_offset += self.line_height - - if self._needs_scroll: - # draw black fade on left and right - fade_width = 20 - rl.draw_rectangle_gradient_h(int(rect.x + rect.width - fade_width), int(rect.y), fade_width, int(rect.height), rl.BLANK, rl.BLACK) - if self._scroll_state != ScrollState.STARTING: - rl.draw_rectangle_gradient_h(int(rect.x), int(rect.y), fade_width, int(rect.height), rl.BLACK, rl.BLANK) - - rl.end_scissor_mode() - - -# TODO: This should be a Widget class -def gui_label( - rect: rl.Rectangle, - text: str, - font_size: int = DEFAULT_TEXT_SIZE, - color: rl.Color = DEFAULT_TEXT_COLOR, - font_weight: FontWeight = FontWeight.NORMAL, - alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, - elide_right: bool = True -): - font = gui_app.font(font_weight) - text_size = measure_text_cached(font, text, font_size) - display_text = text - - # Elide text to fit within the rectangle - if elide_right and text_size.x > rect.width: - _ellipsis = "..." - left, right = 0, len(text) - while left < right: - mid = (left + right) // 2 - candidate = text[:mid] + _ellipsis - candidate_size = measure_text_cached(font, candidate, font_size) - if candidate_size.x <= rect.width: - left = mid + 1 - else: - right = mid - display_text = text[: left - 1] + _ellipsis if left > 0 else _ellipsis - text_size = measure_text_cached(font, display_text, font_size) - - # Calculate horizontal position based on alignment - text_x = rect.x + { - rl.GuiTextAlignment.TEXT_ALIGN_LEFT: 0, - rl.GuiTextAlignment.TEXT_ALIGN_CENTER: (rect.width - text_size.x) / 2, - rl.GuiTextAlignment.TEXT_ALIGN_RIGHT: rect.width - text_size.x, - }.get(alignment, 0) - - # Calculate vertical position based on alignment - text_y = rect.y + { - rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP: 0, - rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE: (rect.height - text_size.y) / 2, - rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: rect.height - text_size.y, - }.get(alignment_vertical, 0) - - # Draw the text in the specified rectangle - # TODO: add wrapping and proper centering for multiline text - rl.draw_text_ex(font, display_text, rl.Vector2(text_x, text_y), font_size, 0, color) - - -def gui_text_box( - rect: rl.Rectangle, - text: str, - font_size: int = DEFAULT_TEXT_SIZE, - color: rl.Color = DEFAULT_TEXT_COLOR, - alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, - font_weight: FontWeight = FontWeight.NORMAL, - line_scale: float = 1.0, -): - styles = [ - (rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(color)), - (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_SIZE, round(font_size * FONT_SCALE)), - (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_LINE_SPACING, round(font_size * FONT_SCALE * line_scale)), - (rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_ALIGNMENT, alignment), - (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_ALIGNMENT_VERTICAL, alignment_vertical), - (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_WRAP_MODE, rl.GuiTextWrapMode.TEXT_WRAP_WORD) - ] - if font_weight != FontWeight.NORMAL: - rl.gui_set_font(gui_app.font(font_weight)) - - with GuiStyleContext(styles): - rl.gui_label(rect, text) - - if font_weight != FontWeight.NORMAL: - rl.gui_set_font(gui_app.font(FontWeight.NORMAL)) - - -# Non-interactive text area. Can render emojis and an optional specified icon. -class Label(Widget): - def __init__(self, - text: str | Callable[[], str], - font_size: int = DEFAULT_TEXT_SIZE, - font_weight: FontWeight = FontWeight.NORMAL, - text_alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - text_alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, - text_padding: int = 0, - text_color: rl.Color = DEFAULT_TEXT_COLOR, - icon: Union[rl.Texture, None] = None, - elide_right: bool = False, - line_scale=1.0, - ): - - super().__init__() - self._font_weight = font_weight - self._font = gui_app.font(self._font_weight) - self._font_size = font_size - self._text_alignment = text_alignment - self._text_alignment_vertical = text_alignment_vertical - self._text_padding = text_padding - self._text_color = text_color - self._icon = icon - self._elide_right = elide_right - self._line_scale = line_scale - - self._text = text - self.set_text(text) - - def set_text(self, text): - self._text = text - self._update_text(self._text) - - def set_text_color(self, color): - self._text_color = color - - def set_font_size(self, size): - self._font_size = size - self._update_text(self._text) - - def _update_text(self, text): - self._emojis = [] - self._text_size = [] - text = _resolve_value(text) - - if self._elide_right: - display_text = text - - # Elide text to fit within the rectangle - text_size = measure_text_cached(self._font, text, self._font_size) - content_width = self._rect.width - self._text_padding * 2 - if self._icon: - content_width -= self._icon.width + ICON_PADDING - if text_size.x > content_width: - _ellipsis = "..." - left, right = 0, len(text) - while left < right: - mid = (left + right) // 2 - candidate = text[:mid] + _ellipsis - candidate_size = measure_text_cached(self._font, candidate, self._font_size) - if candidate_size.x <= content_width: - left = mid + 1 - else: - right = mid - display_text = text[: left - 1] + _ellipsis if left > 0 else _ellipsis - - self._text_wrapped = [display_text] - else: - self._text_wrapped = wrap_text(self._font, text, self._font_size, round(self._rect.width - (self._text_padding * 2))) - - for t in self._text_wrapped: - self._emojis.append(find_emoji(t)) - self._text_size.append(measure_text_cached(self._font, t, self._font_size)) - - def _render(self, _): - # Text can be a callable - # TODO: cache until text changed - self._update_text(self._text) - - text_size = self._text_size[0] if self._text_size else rl.Vector2(0.0, 0.0) - if self._text_alignment_vertical == rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE: - total_text_height = sum(ts.y for ts in self._text_size) or self._font_size * FONT_SCALE - text_pos = rl.Vector2(self._rect.x, (self._rect.y + (self._rect.height - total_text_height) // 2)) - else: - text_pos = rl.Vector2(self._rect.x, self._rect.y) - - if self._icon: - icon_y = self._rect.y + (self._rect.height - self._icon.height) / 2 - if len(self._text_wrapped) > 0: - if self._text_alignment == rl.GuiTextAlignment.TEXT_ALIGN_LEFT: - icon_x = self._rect.x + self._text_padding - text_pos.x = self._icon.width + ICON_PADDING - elif self._text_alignment == rl.GuiTextAlignment.TEXT_ALIGN_CENTER: - total_width = self._icon.width + ICON_PADDING + text_size.x - icon_x = self._rect.x + (self._rect.width - total_width) / 2 - text_pos.x = self._icon.width + ICON_PADDING - else: - icon_x = (self._rect.x + self._rect.width - text_size.x - self._text_padding) - ICON_PADDING - self._icon.width - else: - icon_x = self._rect.x + (self._rect.width - self._icon.width) / 2 - rl.draw_texture_v(self._icon, rl.Vector2(icon_x, icon_y), rl.WHITE) - - for text, text_size, emojis in zip_longest(self._text_wrapped, self._text_size, self._emojis, fillvalue=[]): - line_pos = rl.Vector2(text_pos.x, text_pos.y) - if self._text_alignment == rl.GuiTextAlignment.TEXT_ALIGN_LEFT: - line_pos.x += self._text_padding - elif self._text_alignment == rl.GuiTextAlignment.TEXT_ALIGN_CENTER: - line_pos.x += (self._rect.width - text_size.x) // 2 - elif self._text_alignment == rl.GuiTextAlignment.TEXT_ALIGN_RIGHT: - line_pos.x += self._rect.width - text_size.x - self._text_padding - - prev_index = 0 - for start, end, emoji in emojis: - text_before = text[prev_index:start] - width_before = measure_text_cached(self._font, text_before, self._font_size) - rl.draw_text_ex(self._font, text_before, line_pos, self._font_size, 0, self._text_color) - line_pos.x += width_before.x - - tex = emoji_tex(emoji) - rl.draw_texture_ex(tex, line_pos, 0.0, self._font_size / tex.height * FONT_SCALE, self._text_color) - line_pos.x += self._font_size * FONT_SCALE - prev_index = end - rl.draw_text_ex(self._font, text[prev_index:], line_pos, self._font_size, 0, self._text_color) - text_pos.y += (text_size.y or self._font_size * FONT_SCALE) * self._line_scale - - -class UnifiedLabel(Widget): - """ - Unified label widget that combines functionality from gui_label, gui_text_box, Label, and MiciLabel. - - Supports: - - Emoji rendering - - Text wrapping - - Automatic eliding (single-line or multiline) - - Proper multiline vertical alignment - - Height calculation for layout purposes - """ - def __init__(self, - text: str | Callable[[], str], - font_size: int = DEFAULT_TEXT_SIZE, - font_weight: FontWeight = FontWeight.NORMAL, - text_color: rl.Color = DEFAULT_TEXT_COLOR, - alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, - text_padding: int = 0, - max_width: int | None = None, - elide: bool = True, - wrap_text: bool = True, - scroll: bool = False, - line_height: float = 1.0, - letter_spacing: float = 0.0): - super().__init__() - self._text = text - self._font_size = font_size - self._font_weight = font_weight - self._font = gui_app.font(self._font_weight) - self._text_color = text_color - self._alignment = alignment - self._alignment_vertical = alignment_vertical - self._text_padding = text_padding - self._max_width = max_width - self._elide = elide - self._wrap_text = wrap_text - self._scroll = scroll - self._line_height = line_height * 0.9 - self._letter_spacing = letter_spacing # 0.1 = 10% - self._spacing_pixels = font_size * letter_spacing - - # Scroll state - self._scroll = scroll - self._needs_scroll = False - self._scroll_offset = 0 - self._scroll_pause_t: float | None = None - self._scroll_state: ScrollState = ScrollState.STARTING - - # Scroll mode does not support eliding or multiline wrapping - if self._scroll: - self._elide = False - self._wrap_text = False - - # Cached data - self._cached_text: str | None = None - self._cached_wrapped_lines: list[str] = [] - self._cached_line_sizes: list[rl.Vector2] = [] - self._cached_line_emojis: list[list[tuple[int, int, str]]] = [] - self._cached_total_height: float | None = None - self._cached_width: int = -1 - - # If max_width is set, initialize rect size for Scroller support - if max_width is not None: - self._rect.width = max_width - self._rect.height = self.get_content_height(max_width) - - def set_text(self, text: str | Callable[[], str]): - """Update the text content.""" - self._text = text - # No need to update cache here, will be done on next render if needed - - @property - def text(self) -> str: - """Get the current text content.""" - return str(_resolve_value(self._text)) - - def set_text_color(self, color: rl.Color): - """Update the text color.""" - self._text_color = color - - def set_color(self, color: rl.Color): - """Update the text color (alias for set_text_color).""" - self.set_text_color(color) - - def set_font_size(self, size: int): - """Update the font size.""" - if self._font_size != size: - self._font_size = size - self._spacing_pixels = size * self._letter_spacing # Recalculate spacing - self._cached_text = None # Invalidate cache - - def set_letter_spacing(self, letter_spacing: float): - """Update letter spacing (as percentage, e.g., 0.1 = 10%).""" - if self._letter_spacing != letter_spacing: - self._letter_spacing = letter_spacing - self._spacing_pixels = self._font_size * letter_spacing - self._cached_text = None # Invalidate cache - - def set_font_weight(self, font_weight: FontWeight): - """Update the font weight.""" - if self._font_weight != font_weight: - self._font_weight = font_weight - self._font = gui_app.font(self._font_weight) - self._cached_text = None # Invalidate cache - - def set_alignment(self, alignment: int): - """Update the horizontal text alignment.""" - self._alignment = alignment - - def set_alignment_vertical(self, alignment_vertical: int): - """Update the vertical text alignment.""" - self._alignment_vertical = alignment_vertical - - def reset_scroll(self): - """Reset scroll state to initial position.""" - self._scroll_offset = 0 - self._scroll_pause_t = None - self._scroll_state = ScrollState.STARTING - - def set_max_width(self, max_width: int | None): - """Set the maximum width constraint for wrapping/eliding.""" - if self._max_width != max_width: - self._max_width = max_width - self._cached_text = None # Invalidate cache - # Update rect size for Scroller support - if max_width is not None: - self._rect.width = max_width - self._rect.height = self.get_content_height(max_width) - - def _update_text_cache(self, available_width: int): - """Update cached text processing data.""" - text = self.text - - # Check if cache is still valid - if (self._cached_text == text and - self._cached_width == available_width and - self._cached_wrapped_lines): - return - - self._cached_text = text - self._cached_width = available_width - - # Determine wrapping width - content_width = available_width - (self._text_padding * 2) - if content_width <= 0: - content_width = 1 - - # Wrap text if enabled - if self._wrap_text: - self._cached_wrapped_lines = wrap_text(self._font, text, self._font_size, content_width, self._spacing_pixels) - else: - # Split by newlines but don't wrap - self._cached_wrapped_lines = text.split('\n') if text else [""] - - # Elide lines if needed (for width constraint) - self._cached_wrapped_lines = [self._elide_line(line, content_width) for line in self._cached_wrapped_lines] - - if self._scroll: - self._cached_wrapped_lines = self._cached_wrapped_lines[:1] # Only first line for scrolling - - # Process each line: measure and find emojis - self._cached_line_sizes = [] - self._cached_line_emojis = [] - - for line in self._cached_wrapped_lines: - emojis = find_emoji(line) - self._cached_line_emojis.append(emojis) - # Empty lines should still have height (use font size as line height) - if not line: - size = rl.Vector2(0, self._font_size * FONT_SCALE) - else: - size = measure_text_cached(self._font, line, self._font_size, self._spacing_pixels) - - # This is the only line - if self._scroll: - self._needs_scroll = size.x > content_width - - self._cached_line_sizes.append(size) - - # Calculate total height - # Each line contributes its measured height * line_height (matching Label's behavior) - # This includes spacing to the next line - if self._cached_line_sizes: - # Match the rendering logic: first line doesn't get line_height scaling - total_height = 0.0 - for idx, size in enumerate(self._cached_line_sizes): - if idx == 0: - total_height += size.y - else: - total_height += size.y * self._line_height - self._cached_total_height = total_height - else: - self._cached_total_height = 0.0 - - def _elide_line(self, line: str, max_width: int, force: bool = False) -> str: - """Elide a single line if it exceeds max_width. If force is True, always elide even if it fits.""" - if not self._elide and not force: - return line - - text_size = measure_text_cached(self._font, line, self._font_size, self._spacing_pixels) - if text_size.x <= max_width and not force: - return line - - ellipsis = "..." - # If force=True and line fits, just append ellipsis without truncating - if force and text_size.x <= max_width: - ellipsis_size = measure_text_cached(self._font, ellipsis, self._font_size, self._spacing_pixels) - if text_size.x + ellipsis_size.x <= max_width: - return line + ellipsis - # If line + ellipsis doesn't fit, need to truncate - # Fall through to binary search below - - left, right = 0, len(line) - while left < right: - mid = (left + right) // 2 - candidate = line[:mid] + ellipsis - candidate_size = measure_text_cached(self._font, candidate, self._font_size, self._spacing_pixels) - if candidate_size.x <= max_width: - left = mid + 1 - else: - right = mid - return line[:left - 1] + ellipsis if left > 0 else ellipsis - - def get_content_height(self, max_width: int) -> float: - """ - Returns the height needed for text at given max_width. - Similar to HtmlRenderer.get_total_height(). - """ - # Use max_width if provided, otherwise use self._max_width or a default - width = max_width if max_width > 0 else (self._max_width if self._max_width else 1000) - self._update_text_cache(width) - - if self._cached_total_height is not None: - return self._cached_total_height - return 0.0 - - def _render(self, _): - """Render the label.""" - if self._rect.width <= 0 or self._rect.height <= 0: - return - - # Determine available width - available_width = self._rect.width - if self._max_width is not None: - available_width = min(available_width, self._max_width) - - # Update text cache - self._update_text_cache(int(available_width)) - - if not self._cached_wrapped_lines: - return - - # Calculate which lines fit in the available height - visible_lines: list[str] = [] - visible_sizes: list[rl.Vector2] = [] - visible_emojis: list[list[tuple[int, int, str]]] = [] - - current_height = 0.0 - broke_early = False - for line, size, emojis in zip( - self._cached_wrapped_lines, - self._cached_line_sizes, - self._cached_line_emojis, - strict=True): - - # Calculate height needed for this line - # Each line contributes its height * line_height (matching Label's behavior) - line_height_needed = size.y * self._line_height - - # Check if this line fits - if current_height + line_height_needed > self._rect.height: - # This line doesn't fit - if len(visible_lines) == 0: - # First line doesn't fit by height - still show it (will be clipped by scissor if needed) - # Continue to add this line below - pass - else: - # We have visible lines and this one doesn't fit - mark that we broke early - broke_early = True - break - - visible_lines.append(line) - visible_sizes.append(size) - visible_emojis.append(emojis) - - current_height += line_height_needed - - # If we broke early (there are more lines that don't fit) and elide is enabled, elide the last visible line - if broke_early and len(visible_lines) > 0 and self._elide: - content_width = int(available_width - (self._text_padding * 2)) - if content_width <= 0: - content_width = 1 - - last_line_idx = len(visible_lines) - 1 - last_line = visible_lines[last_line_idx] - # Force elide the last line to show "..." even if it fits in width (to indicate more content) - elided = self._elide_line(last_line, content_width, force=True) - visible_lines[last_line_idx] = elided - visible_sizes[last_line_idx] = measure_text_cached(self._font, elided, self._font_size, self._spacing_pixels) - - if not visible_lines: - return - - # Calculate total visible text block height - # First line is not changed by line_height scaling - total_visible_height = 0.0 - for idx, size in enumerate(visible_sizes): - if idx == 0: - total_visible_height += size.y - else: - total_visible_height += size.y * self._line_height - - # Calculate vertical alignment offset - if self._alignment_vertical == rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP: - start_y = self._rect.y - elif self._alignment_vertical == rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM: - start_y = self._rect.y + self._rect.height - total_visible_height - else: # TEXT_ALIGN_MIDDLE - start_y = self._rect.y + (self._rect.height - total_visible_height) / 2 - - # Only scissor when we know there is a single scrolling line - # Pad a little since descenders like g or j may overflow below rect from font_scale - if self._needs_scroll: - rl.begin_scissor_mode(int(self._rect.x), int(self._rect.y - self._font_size / 2), int(self._rect.width), int(self._rect.height + self._font_size)) - - # Render each line - current_y = start_y - for idx, (line, size, emojis) in enumerate(zip(visible_lines, visible_sizes, visible_emojis, strict=True)): - if self._needs_scroll: - if self._scroll_state == ScrollState.STARTING: - if self._scroll_pause_t is None: - self._scroll_pause_t = rl.get_time() + 2.0 - if rl.get_time() >= self._scroll_pause_t: - self._scroll_state = ScrollState.SCROLLING - self._scroll_pause_t = None - - elif self._scroll_state == ScrollState.SCROLLING: - self._scroll_offset -= 0.8 / 60. * gui_app.target_fps - # don't fully hide - if self._scroll_offset <= -size.x - self._rect.width / 3: - self._scroll_offset = 0 - self._scroll_state = ScrollState.STARTING - self._scroll_pause_t = None - else: - self.reset_scroll() - - self._render_line(line, size, emojis, current_y) - - # Draw 2nd instance for scrolling - if self._needs_scroll and self._scroll_state != ScrollState.STARTING: - text2_scroll_offset = size.x + self._rect.width / 3 - self._render_line(line, size, emojis, current_y, text2_scroll_offset) - - # Move to next line (if not last line) - if idx < len(visible_lines) - 1: - # Use current line's height * line_height for spacing to next line - current_y += size.y * self._line_height - - if self._needs_scroll: - # draw black fade on left and right - fade_width = 20 - rl.draw_rectangle_gradient_h(int(self._rect.x + self._rect.width - fade_width), int(self._rect.y), fade_width, int(self._rect.height), rl.BLANK, rl.BLACK) - if self._scroll_state != ScrollState.STARTING: - rl.draw_rectangle_gradient_h(int(self._rect.x), int(self._rect.y), fade_width, int(self._rect.height), rl.BLACK, rl.BLANK) - - rl.end_scissor_mode() - - def _render_line(self, line, size, emojis, current_y, x_offset=0.0): - # Calculate horizontal position - if self._alignment == rl.GuiTextAlignment.TEXT_ALIGN_LEFT: - line_x = self._rect.x + self._text_padding - elif self._alignment == rl.GuiTextAlignment.TEXT_ALIGN_CENTER: - line_x = self._rect.x + (self._rect.width - size.x) / 2 - elif self._alignment == rl.GuiTextAlignment.TEXT_ALIGN_RIGHT: - line_x = self._rect.x + self._rect.width - size.x - self._text_padding - else: - line_x = self._rect.x + self._text_padding - line_x += self._scroll_offset + x_offset - - # Render line with emojis - line_pos = rl.Vector2(line_x, current_y) - prev_index = 0 - - for start, end, emoji in emojis: - # Draw text before emoji - text_before = line[prev_index:start] - if text_before: - rl.draw_text_ex(self._font, text_before, line_pos, self._font_size, self._spacing_pixels, self._text_color) - width_before = measure_text_cached(self._font, text_before, self._font_size, self._spacing_pixels) - line_pos.x += width_before.x - - # Draw emoji - tex = emoji_tex(emoji) - emoji_scale = self._font_size / tex.height * FONT_SCALE - rl.draw_texture_ex(tex, line_pos, 0.0, emoji_scale, self._text_color) - # Emoji width is font_size * FONT_SCALE (as per measure_text_cached) - line_pos.x += self._font_size * FONT_SCALE - prev_index = end - - # Draw remaining text after last emoji - text_after = line[prev_index:] - if text_after: - rl.draw_text_ex(self._font, text_after, line_pos, self._font_size, self._spacing_pixels, self._text_color) diff --git a/system/ui/widgets/list_view.py b/system/ui/widgets/list_view.py deleted file mode 100644 index 32bf01cfc86225..00000000000000 --- a/system/ui/widgets/list_view.py +++ /dev/null @@ -1,467 +0,0 @@ -import os -import pyray as rl -from collections.abc import Callable -from abc import ABC -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.toggle import Toggle, WIDTH as TOGGLE_WIDTH, HEIGHT as TOGGLE_HEIGHT -from openpilot.system.ui.widgets.label import gui_label -from openpilot.system.ui.widgets.html_render import HtmlRenderer, ElementType - -ITEM_BASE_WIDTH = 600 -ITEM_BASE_HEIGHT = 170 -ITEM_PADDING = 20 -ITEM_TEXT_FONT_SIZE = 50 -ITEM_TEXT_COLOR = rl.WHITE -ITEM_TEXT_VALUE_COLOR = rl.Color(170, 170, 170, 255) -ITEM_DESC_TEXT_COLOR = rl.Color(128, 128, 128, 255) -ITEM_DESC_FONT_SIZE = 40 -ITEM_DESC_V_OFFSET = 140 -RIGHT_ITEM_PADDING = 20 -ICON_SIZE = 80 -BUTTON_WIDTH = 250 -BUTTON_HEIGHT = 100 -BUTTON_BORDER_RADIUS = 50 -BUTTON_FONT_SIZE = 35 -BUTTON_FONT_WEIGHT = FontWeight.MEDIUM - -TEXT_PADDING = 20 - - -def _resolve_value(value, default=""): - if callable(value): - return value() - return value if value is not None else default - - -# Abstract base class for right-side items -class ItemAction(Widget, ABC): - def __init__(self, width: int = BUTTON_HEIGHT, enabled: bool | Callable[[], bool] = True): - super().__init__() - self.set_rect(rl.Rectangle(0, 0, width, 0)) - self._enabled_source = enabled - - def get_width_hint(self) -> float: - # Return's action ideal width, 0 means use full width - return self._rect.width - - def set_enabled(self, enabled: bool | Callable[[], bool]): - self._enabled_source = enabled - - @property - def enabled(self): - return _resolve_value(self._enabled_source, False) - - -class ToggleAction(ItemAction): - def __init__(self, initial_state: bool = False, width: int = TOGGLE_WIDTH, enabled: bool | Callable[[], bool] = True, - callback: Callable[[bool], None] | None = None): - super().__init__(width, enabled) - self.toggle = Toggle(initial_state=initial_state, callback=callback) - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - super().set_touch_valid_callback(touch_callback) - self.toggle.set_touch_valid_callback(touch_callback) - - def _render(self, rect: rl.Rectangle) -> bool: - self.toggle.set_enabled(self.enabled) - clicked = self.toggle.render(rl.Rectangle(rect.x, rect.y + (rect.height - TOGGLE_HEIGHT) / 2, self._rect.width, TOGGLE_HEIGHT)) - return bool(clicked) - - def set_state(self, state: bool): - self.toggle.set_state(state) - - def get_state(self) -> bool: - return self.toggle.get_state() - - -class ButtonAction(ItemAction): - def __init__(self, text: str | Callable[[], str], width: int = BUTTON_WIDTH, enabled: bool | Callable[[], bool] = True): - super().__init__(width, enabled) - self._text_source = text - self._value_source: str | Callable[[], str] | None = None - self._pressed = False - self._font = gui_app.font(FontWeight.NORMAL) - - def pressed(): - self._pressed = True - - self._button = Button( - self.text, - font_size=BUTTON_FONT_SIZE, - font_weight=BUTTON_FONT_WEIGHT, - button_style=ButtonStyle.LIST_ACTION, - border_radius=BUTTON_BORDER_RADIUS, - click_callback=pressed, - text_padding=0, - ) - self.set_enabled(enabled) - - def get_width_hint(self) -> float: - value_text = self.value - if value_text: - text_width = measure_text_cached(self._font, value_text, ITEM_TEXT_FONT_SIZE).x - return text_width + BUTTON_WIDTH + TEXT_PADDING - else: - return BUTTON_WIDTH - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - super().set_touch_valid_callback(touch_callback) - self._button.set_touch_valid_callback(touch_callback) - - def set_text(self, text: str | Callable[[], str]): - self._text_source = text - - def set_value(self, value: str | Callable[[], str]): - self._value_source = value - - @property - def text(self): - return _resolve_value(self._text_source, tr("Error")) - - @property - def value(self): - return _resolve_value(self._value_source, "") - - def _render(self, rect: rl.Rectangle) -> bool: - self._button.set_text(self.text) - self._button.set_enabled(_resolve_value(self.enabled)) - button_rect = rl.Rectangle(rect.x + rect.width - BUTTON_WIDTH, rect.y + (rect.height - BUTTON_HEIGHT) / 2, BUTTON_WIDTH, BUTTON_HEIGHT) - self._button.render(button_rect) - - value_text = self.value - if value_text: - value_rect = rl.Rectangle(rect.x, rect.y, rect.width - BUTTON_WIDTH - TEXT_PADDING, rect.height) - gui_label(value_rect, value_text, font_size=ITEM_TEXT_FONT_SIZE, color=ITEM_TEXT_VALUE_COLOR, - font_weight=FontWeight.NORMAL, alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - - # TODO: just use the generic Widget click callbacks everywhere, no returning from render - pressed = self._pressed - self._pressed = False - return pressed - - -class TextAction(ItemAction): - def __init__(self, text: str | Callable[[], str], color: rl.Color = ITEM_TEXT_COLOR, enabled: bool | Callable[[], bool] = True): - self._text_source = text - self.color = color - - self._font = gui_app.font(FontWeight.NORMAL) - initial_text = _resolve_value(text, "") - text_width = measure_text_cached(self._font, initial_text, ITEM_TEXT_FONT_SIZE).x - super().__init__(int(text_width + TEXT_PADDING), enabled) - - @property - def text(self): - return _resolve_value(self._text_source, tr("Error")) - - def get_width_hint(self) -> float: - text_width = measure_text_cached(self._font, self.text, ITEM_TEXT_FONT_SIZE).x - return text_width + TEXT_PADDING - - def _render(self, rect: rl.Rectangle) -> bool: - gui_label(self._rect, self.text, font_size=ITEM_TEXT_FONT_SIZE, color=self.color, - font_weight=FontWeight.NORMAL, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) - return False - - def set_text(self, text: str | Callable[[], str]): - self._text_source = text - - -class DualButtonAction(ItemAction): - def __init__(self, left_text: str | Callable[[], str], right_text: str | Callable[[], str], left_callback: Callable | None = None, - right_callback: Callable | None = None, enabled: bool | Callable[[], bool] = True): - super().__init__(width=0, enabled=enabled) # Width 0 means use full width - self.left_button = Button(left_text, click_callback=left_callback, button_style=ButtonStyle.NORMAL, text_padding=0) - self.right_button = Button(right_text, click_callback=right_callback, button_style=ButtonStyle.DANGER, text_padding=0) - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - super().set_touch_valid_callback(touch_callback) - self.left_button.set_touch_valid_callback(touch_callback) - self.right_button.set_touch_valid_callback(touch_callback) - - def _render(self, rect: rl.Rectangle): - button_spacing = 30 - button_height = 120 - button_width = (rect.width - button_spacing) / 2 - button_y = rect.y + (rect.height - button_height) / 2 - - left_rect = rl.Rectangle(rect.x, button_y, button_width, button_height) - right_rect = rl.Rectangle(rect.x + button_width + button_spacing, button_y, button_width, button_height) - - # expand one to full width if other is not visible - if not self.left_button.is_visible: - right_rect.x = rect.x - right_rect.width = rect.width - elif not self.right_button.is_visible: - left_rect.width = rect.width - - # Render buttons - self.left_button.render(left_rect) - self.right_button.render(right_rect) - - -class MultipleButtonAction(ItemAction): - def __init__(self, buttons: list[str | Callable[[], str]], button_width: int, selected_index: int = 0, callback: Callable | None = None): - super().__init__(width=len(buttons) * button_width + (len(buttons) - 1) * RIGHT_ITEM_PADDING, enabled=True) - self.buttons = buttons - self.button_width = button_width - self.selected_button = selected_index - self.callback = callback - self._font = gui_app.font(FontWeight.MEDIUM) - - def set_selected_button(self, index: int): - if 0 <= index < len(self.buttons): - self.selected_button = index - - def get_selected_button(self) -> int: - return self.selected_button - - def _render(self, rect: rl.Rectangle): - spacing = RIGHT_ITEM_PADDING - button_y = rect.y + (rect.height - BUTTON_HEIGHT) / 2 - - for i, _text in enumerate(self.buttons): - button_x = rect.x + i * (self.button_width + spacing) - button_rect = rl.Rectangle(button_x, button_y, self.button_width, BUTTON_HEIGHT) - - # Check button state - mouse_pos = rl.get_mouse_position() - is_pressed = rl.check_collision_point_rec(mouse_pos, button_rect) and self.enabled and self.is_pressed - is_selected = i == self.selected_button - - # Button colors - if is_selected: - bg_color = rl.Color(51, 171, 76, 255) # Green - elif is_pressed: - bg_color = rl.Color(74, 74, 74, 255) # Dark gray - else: - bg_color = rl.Color(57, 57, 57, 255) # Gray - - if not self.enabled: - bg_color = rl.Color(bg_color.r, bg_color.g, bg_color.b, 150) # Dim - - # Draw button - rl.draw_rectangle_rounded(button_rect, 1.0, 20, bg_color) - - # Draw text - text = _resolve_value(_text, "") - text_size = measure_text_cached(self._font, text, 40) - text_x = button_x + (self.button_width - text_size.x) / 2 - text_y = button_y + (BUTTON_HEIGHT - text_size.y) / 2 - text_color = rl.Color(228, 228, 228, 255) if self.enabled else rl.Color(150, 150, 150, 255) - rl.draw_text_ex(self._font, text, rl.Vector2(text_x, text_y), 40, 0, text_color) - - def _handle_mouse_release(self, mouse_pos: MousePos): - spacing = RIGHT_ITEM_PADDING - button_y = self._rect.y + (self._rect.height - BUTTON_HEIGHT) / 2 - for i, _ in enumerate(self.buttons): - button_x = self._rect.x + i * (self.button_width + spacing) - button_rect = rl.Rectangle(button_x, button_y, self.button_width, BUTTON_HEIGHT) - if rl.check_collision_point_rec(mouse_pos, button_rect): - self.selected_button = i - if self.callback: - self.callback(i) - - -class ListItem(Widget): - def __init__(self, title: str | Callable[[], str] = "", icon: str | None = None, description: str | Callable[[], str] | None = None, - description_visible: bool = False, callback: Callable | None = None, - action_item: ItemAction | None = None): - super().__init__() - self._title = title - self.set_icon(icon) - self._description = description - self.description_visible = description_visible - self.callback = callback - self.description_opened_callback: Callable | None = None - self.action_item = action_item - - self.set_rect(rl.Rectangle(0, 0, ITEM_BASE_WIDTH, ITEM_BASE_HEIGHT)) - self._font = gui_app.font(FontWeight.NORMAL) - - self._html_renderer = HtmlRenderer(text="", text_size={ElementType.P: ITEM_DESC_FONT_SIZE}, - text_color=ITEM_DESC_TEXT_COLOR) - self._parse_description(self.description) - - # Cached properties for performance - self._prev_description: str | None = self.description - - def show_event(self): - self._set_description_visible(False) - - def set_description_opened_callback(self, callback: Callable) -> None: - self.description_opened_callback = callback - - def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: - super().set_touch_valid_callback(touch_callback) - if self.action_item: - self.action_item.set_touch_valid_callback(touch_callback) - - def set_parent_rect(self, parent_rect: rl.Rectangle): - super().set_parent_rect(parent_rect) - self._rect.width = parent_rect.width - - def _handle_mouse_release(self, mouse_pos: MousePos): - if not self.is_visible: - return - - # Check not in action rect - if self.action_item: - action_rect = self.get_right_item_rect(self._rect) - if rl.check_collision_point_rec(mouse_pos, action_rect): - # Click was on right item, don't toggle description - return - - self._set_description_visible(not self.description_visible) - - def _set_description_visible(self, visible: bool): - if self.description and self.description_visible != visible: - self.description_visible = visible - # do callback first in case receiver changes description - if self.description_visible and self.description_opened_callback is not None: - self.description_opened_callback() - # Call _update_state to catch any description changes - self._update_state() - - content_width = int(self._rect.width - ITEM_PADDING * 2) - self._rect.height = self.get_item_height(self._font, content_width) - - def _update_state(self): - # Detect changes if description is callback - new_description = self.description - if new_description != self._prev_description: - self._parse_description(new_description) - - def _render(self, _): - if not self.is_visible: - return - - # Don't draw items that are not in parent's viewport - if ((self._rect.y + self.rect.height) <= self._parent_rect.y or - self._rect.y >= (self._parent_rect.y + self._parent_rect.height)): - return - - content_x = self._rect.x + ITEM_PADDING - text_x = content_x - - # Only draw title and icon for items that have them - if self.title: - # Draw icon if present - if self.icon: - rl.draw_texture(self._icon_texture, int(content_x), int(self._rect.y + (ITEM_BASE_HEIGHT - self._icon_texture.height) // 2), rl.WHITE) - text_x += ICON_SIZE + ITEM_PADDING - - # Draw main text - text_size = measure_text_cached(self._font, self.title, ITEM_TEXT_FONT_SIZE) - item_y = self._rect.y + (ITEM_BASE_HEIGHT - text_size.y) // 2 - rl.draw_text_ex(self._font, self.title, rl.Vector2(text_x, item_y), ITEM_TEXT_FONT_SIZE, 0, ITEM_TEXT_COLOR) - - # Draw description if visible - if self.description_visible: - content_width = int(self._rect.width - ITEM_PADDING * 2) - description_height = self._html_renderer.get_total_height(content_width) - description_rect = rl.Rectangle( - self._rect.x + ITEM_PADDING, - self._rect.y + ITEM_DESC_V_OFFSET, - content_width, - description_height - ) - self._html_renderer.render(description_rect) - - # Draw right item if present - if self.action_item: - right_rect = self.get_right_item_rect(self._rect) - right_rect.y = self._rect.y - if self.action_item.render(right_rect) and self.action_item.enabled: - # Right item was clicked/activated - if self.callback: - self.callback() - - def set_icon(self, icon: str | None): - self.icon = icon - self._icon_texture = gui_app.texture(os.path.join("icons", self.icon), ICON_SIZE, ICON_SIZE) if self.icon else None - - def set_description(self, description: str | Callable[[], str] | None): - self._description = description - - def _parse_description(self, new_desc): - self._html_renderer.parse_html_content(new_desc) - self._prev_description = new_desc - - @property - def title(self): - return _resolve_value(self._title, "") - - @property - def description(self): - return _resolve_value(self._description, "") - - def get_item_height(self, font: rl.Font, max_width: int) -> float: - if not self.is_visible: - return 0 - - height = float(ITEM_BASE_HEIGHT) - if self.description_visible: - description_height = self._html_renderer.get_total_height(max_width) - height += description_height - (ITEM_BASE_HEIGHT - ITEM_DESC_V_OFFSET) + ITEM_PADDING - return height - - def get_right_item_rect(self, item_rect: rl.Rectangle) -> rl.Rectangle: - if not self.action_item: - return rl.Rectangle(0, 0, 0, 0) - - right_width = self.action_item.get_width_hint() - if right_width == 0: # Full width action (like DualButtonAction) - return rl.Rectangle(item_rect.x + ITEM_PADDING, item_rect.y, - item_rect.width - (ITEM_PADDING * 2), ITEM_BASE_HEIGHT) - - # Clip width to available space, never overlapping this Item's title - content_width = item_rect.width - (ITEM_PADDING * 2) - title_width = measure_text_cached(self._font, self.title, ITEM_TEXT_FONT_SIZE).x - right_width = min(content_width - title_width, right_width) - - right_x = item_rect.x + item_rect.width - right_width - right_y = item_rect.y - return rl.Rectangle(right_x, right_y, right_width, ITEM_BASE_HEIGHT) - - -# Factory functions -def simple_item(title: str | Callable[[], str], callback: Callable | None = None) -> ListItem: - return ListItem(title=title, callback=callback) - - -def toggle_item(title: str | Callable[[], str], description: str | Callable[[], str] | None = None, initial_state: bool = False, - callback: Callable | None = None, icon: str = "", enabled: bool | Callable[[], bool] = True) -> ListItem: - action = ToggleAction(initial_state=initial_state, enabled=enabled, callback=callback) - return ListItem(title=title, description=description, action_item=action, icon=icon) - - -def button_item(title: str | Callable[[], str], button_text: str | Callable[[], str], description: str | Callable[[], str] | None = None, - callback: Callable | None = None, enabled: bool | Callable[[], bool] = True) -> ListItem: - action = ButtonAction(text=button_text, enabled=enabled) - return ListItem(title=title, description=description, action_item=action, callback=callback) - - -def text_item(title: str | Callable[[], str], value: str | Callable[[], str], description: str | Callable[[], str] | None = None, - callback: Callable | None = None, enabled: bool | Callable[[], bool] = True) -> ListItem: - action = TextAction(text=value, color=ITEM_TEXT_VALUE_COLOR, enabled=enabled) - return ListItem(title=title, description=description, action_item=action, callback=callback) - - -def dual_button_item(left_text: str | Callable[[], str], right_text: str | Callable[[], str], - left_callback: Callable | None = None, right_callback: Callable | None = None, - description: str | Callable[[], str] | None = None, enabled: bool | Callable[[], bool] = True) -> ListItem: - action = DualButtonAction(left_text, right_text, left_callback, right_callback, enabled) - return ListItem(title="", description=description, action_item=action) - - -def multiple_button_item(title: str | Callable[[], str], description: str | Callable[[], str], buttons: list[str | Callable[[], str]], selected_index: int, - button_width: int = BUTTON_WIDTH, callback: Callable | None = None, icon: str = ""): - action = MultipleButtonAction(buttons, button_width, selected_index, callback=callback) - return ListItem(title=title, description=description, icon=icon, action_item=action) diff --git a/system/ui/widgets/mici_keyboard.py b/system/ui/widgets/mici_keyboard.py deleted file mode 100644 index 6d2e08e0539f77..00000000000000 --- a/system/ui/widgets/mici_keyboard.py +++ /dev/null @@ -1,397 +0,0 @@ -from enum import IntEnum -import pyray as rl -import numpy as np -from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, MouseEvent -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.system.ui.widgets import Widget -from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter - -CHAR_FONT_SIZE = 42 -CHAR_NEAR_FONT_SIZE = CHAR_FONT_SIZE * 2 -SELECTED_CHAR_FONT_SIZE = 128 -CHAR_CAPS_FONT_SIZE = 38 # TODO: implement this -NUMBER_LAYER_SWITCH_FONT_SIZE = 24 -KEYBOARD_COLUMN_PADDING = 33 -KEYBOARD_ROW_PADDING = {0: 44, 1: 33, 2: 44} # TODO: 2 should be 116 with extra control keys added in - -KEY_TOUCH_AREA_OFFSET = 10 # px -KEY_DRAG_HYSTERESIS = 5 # px -KEY_MIN_ANIMATION_TIME = 0.075 # s - -DEBUG = False -ANIMATION_SCALE = 0.65 - - -def zip_repeat(a, b): - la, lb = len(a), len(b) - for i in range(max(la, lb)): - yield (a[i] if i < la else a[-1], - b[i] if i < lb else b[-1]) - - -def fast_euclidean_distance(dx, dy): - # https://en.wikibooks.org/wiki/Algorithms/Distance_approximations - max_d, min_d = abs(dx), abs(dy) - if max_d < min_d: - max_d, min_d = min_d, max_d - return 0.941246 * max_d + 0.41 * min_d - - -class Key(Widget): - def __init__(self, char: str, font_weight: FontWeight = FontWeight.SEMI_BOLD): - super().__init__() - self.char = char - self._font = gui_app.font(font_weight) - self._x_filter = BounceFilter(0.0, 0.1 * ANIMATION_SCALE, 1 / gui_app.target_fps) - self._y_filter = BounceFilter(0.0, 0.1 * ANIMATION_SCALE, 1 / gui_app.target_fps) - self._size_filter = BounceFilter(CHAR_FONT_SIZE, 0.1 * ANIMATION_SCALE, 1 / gui_app.target_fps) - self._alpha_filter = BounceFilter(1.0, 0.075 * ANIMATION_SCALE, 1 / gui_app.target_fps) - - self._color = rl.Color(255, 255, 255, 255) - - self._position_initialized = False - self.original_position = rl.Vector2(0, 0) - - def set_position(self, x: float, y: float, smooth: bool = True): - # Smooth keys within parent rect - base_y = self._parent_rect.y if self._parent_rect else 0.0 - local_y = y - base_y - - if not self._position_initialized: - self._x_filter.x = x - self._y_filter.x = local_y - # keep track of original position so dragging around feels consistent. also move touch area down a bit - self.original_position = rl.Vector2(x, y + KEY_TOUCH_AREA_OFFSET) - self._position_initialized = True - - if not smooth: - self._x_filter.x = x - self._y_filter.x = local_y - - self._rect.x = self._x_filter.update(x) - self._rect.y = base_y + self._y_filter.update(local_y) - - def set_alpha(self, alpha: float): - self._alpha_filter.update(alpha) - - def get_position(self) -> tuple[float, float]: - return self._rect.x, self._rect.y - - def _update_state(self): - self._color.a = min(int(255 * self._alpha_filter.x), 255) - - def _render(self, _): - # center char at rect position - text_size = measure_text_cached(self._font, self.char, self._get_font_size()) - x = self._rect.x + self._rect.width / 2 - text_size.x / 2 - y = self._rect.y + self._rect.height / 2 - text_size.y / 2 - rl.draw_text_ex(self._font, self.char, (x, y), self._get_font_size(), 0, self._color) - - if DEBUG: - rl.draw_circle(int(self._rect.x), int(self._rect.y), 5, rl.RED) # Debug: draw circle around key - rl.draw_rectangle_lines_ex(self._rect, 2, rl.RED) - - def set_font_size(self, size: float): - self._size_filter.update(size) - - def _get_font_size(self) -> int: - return int(round(self._size_filter.x)) - - -class SmallKey(Key): - def __init__(self, chars: str): - super().__init__(chars, FontWeight.BOLD) - self._size_filter.x = NUMBER_LAYER_SWITCH_FONT_SIZE - - def set_font_size(self, size: float): - self._size_filter.update(size * (NUMBER_LAYER_SWITCH_FONT_SIZE / CHAR_FONT_SIZE)) - - -class IconKey(Key): - def __init__(self, icon: str, vertical_align: str = "center", char: str = "", icon_size: tuple[int, int] = (38, 38)): - super().__init__(char) - self._icon_size = icon_size - self._icon = gui_app.texture(icon, *icon_size) - self._vertical_align = vertical_align - - def set_icon(self, icon: str, icon_size: tuple[int, int] | None = None): - size = icon_size if icon_size is not None else self._icon_size - self._icon = gui_app.texture(icon, *size) - - def _render(self, _): - scale = np.interp(self._size_filter.x, [CHAR_FONT_SIZE, CHAR_NEAR_FONT_SIZE], [1, 1.5]) - - if self._vertical_align == "center": - dest_rec = rl.Rectangle(self._rect.x + (self._rect.width - self._icon.width * scale) / 2, - self._rect.y + (self._rect.height - self._icon.height * scale) / 2, - self._icon.width * scale, self._icon.height * scale) - src_rec = rl.Rectangle(0, 0, self._icon.width, self._icon.height) - rl.draw_texture_pro(self._icon, src_rec, dest_rec, rl.Vector2(0, 0), 0, self._color) - - elif self._vertical_align == "bottom": - dest_rec = rl.Rectangle(self._rect.x + (self._rect.width - self._icon.width * scale) / 2, self._rect.y, - self._icon.width * scale, self._icon.height * scale) - src_rec = rl.Rectangle(0, 0, self._icon.width, self._icon.height) - rl.draw_texture_pro(self._icon, src_rec, dest_rec, rl.Vector2(0, 0), 0, self._color) - - if DEBUG: - rl.draw_circle(int(self._rect.x), int(self._rect.y), 5, rl.RED) # Debug: draw circle around key - rl.draw_rectangle_lines_ex(self._rect, 2, rl.RED) - - -class CapsState(IntEnum): - LOWER = 0 - UPPER = 1 - LOCK = 2 - - -class MiciKeyboard(Widget): - def __init__(self): - super().__init__() - - lower_chars = [ - "qwertyuiop", - "asdfghjkl", - "zxcvbnm", - ] - upper_chars = ["".join([char.upper() for char in row]) for row in lower_chars] - special_chars = [ - "1234567890", - "-/:;()$&@\"", - "~.,?!'#%", - ] - super_special_chars = [ - "1234567890", - "`[]{}^*+=_", - "\\|<>¥€£•", - ] - - self._lower_keys = [[Key(char) for char in row] for row in lower_chars] - self._upper_keys = [[Key(char) for char in row] for row in upper_chars] - self._special_keys = [[Key(char) for char in row] for row in special_chars] - self._super_special_keys = [[Key(char) for char in row] for row in super_special_chars] - - # control keys - self._space_key = IconKey("icons_mici/settings/keyboard/space.png", char=" ", vertical_align="bottom", icon_size=(43, 14)) - self._caps_key = IconKey("icons_mici/settings/keyboard/caps_lower.png", icon_size=(38, 33)) - # these two are in different places on some layouts - self._123_key, self._123_key2 = SmallKey("123"), SmallKey("123") - self._abc_key = SmallKey("abc") - self._super_special_key = SmallKey("#+=") - - # insert control keys - for keys in (self._lower_keys, self._upper_keys): - keys[2].insert(0, self._caps_key) - keys[2].append(self._123_key) - - for keys in (self._lower_keys, self._upper_keys, self._special_keys, self._super_special_keys): - keys[1].append(self._space_key) - - for keys in (self._special_keys, self._super_special_keys): - keys[2].append(self._abc_key) - - self._special_keys[2].insert(0, self._super_special_key) - self._super_special_keys[2].insert(0, self._123_key2) - - # set initial keys - self._current_keys: list[list[Key]] = [] - self._set_keys(self._lower_keys) - self._caps_state = CapsState.LOWER - self._initialized = False - - self._load_images() - - self._closest_key: tuple[Key | None, float] = None, float('inf') - self._selected_key_t: float | None = None # time key was initially selected - self._unselect_key_t: float | None = None # time to unselect key after release - self._dragging_on_keyboard = False - - self._text: str = "" - - self._bg_scale_filter = BounceFilter(1.0, 0.1 * ANIMATION_SCALE, 1 / gui_app.target_fps) - self._selected_key_filter = FirstOrderFilter(0.0, 0.075 * ANIMATION_SCALE, 1 / gui_app.target_fps) - - def get_candidate_character(self) -> str: - # return str of character about to be added to text - key = self._closest_key[0] - return key.char if key is not None and key.__class__ is Key and self._dragging_on_keyboard else "" - - def get_keyboard_height(self) -> int: - return int(self._txt_bg.height) - - def _load_images(self): - self._txt_bg = gui_app.texture("icons_mici/settings/keyboard/keyboard_background.png", 520, 170, keep_aspect_ratio=False) - - def _set_keys(self, keys: list[list[Key]]): - # inherit previous keys' positions to fix switching animation - for current_row, row in zip(self._current_keys, keys, strict=False): - # not all layouts have the same number of keys - for current_key, key in zip_repeat(current_row, row): - current_pos = current_key.get_position() - key.set_position(current_pos[0], current_pos[1], smooth=False) - - self._current_keys = keys - - def set_text(self, text: str): - self._text = text - - def text(self) -> str: - return self._text - - def _handle_mouse_event(self, mouse_event: MouseEvent) -> None: - keyboard_pos_y = self._rect.y + self._rect.height - self._txt_bg.height - if mouse_event.left_pressed: - if mouse_event.pos.y > keyboard_pos_y: - self._dragging_on_keyboard = True - elif mouse_event.left_released: - self._dragging_on_keyboard = False - - if mouse_event.left_down and self._dragging_on_keyboard: - self._closest_key = self._get_closest_key() - if self._selected_key_t is None: - self._selected_key_t = rl.get_time() - - # unselect key temporarily if mouse goes above keyboard - if mouse_event.pos.y <= keyboard_pos_y: - self._closest_key = (None, float('inf')) - - if DEBUG: - print('HANDLE MOUSE EVENT', mouse_event, self._closest_key[0].char if self._closest_key[0] else 'None') - - def _get_closest_key(self) -> tuple[Key | None, float]: - closest_key: tuple[Key | None, float] = (None, float('inf')) - for row in self._current_keys: - for key in row: - mouse_pos = gui_app.last_mouse_event.pos - # approximate distance for comparison is accurate enough - dist = abs(key.original_position.x - mouse_pos.x) + abs(key.original_position.y - mouse_pos.y) - if dist < closest_key[1]: - if self._closest_key[0] is None or key is self._closest_key[0] or dist < self._closest_key[1] - KEY_DRAG_HYSTERESIS: - closest_key = (key, dist) - return closest_key - - def _set_uppercase(self, cycle: bool): - self._set_keys(self._upper_keys if cycle else self._lower_keys) - if not cycle: - self._caps_state = CapsState.LOWER - self._caps_key.set_icon("icons_mici/settings/keyboard/caps_lower.png", icon_size=(38, 33)) - else: - if self._caps_state == CapsState.LOWER: - self._caps_state = CapsState.UPPER - self._caps_key.set_icon("icons_mici/settings/keyboard/caps_upper.png", icon_size=(38, 33)) - elif self._caps_state == CapsState.UPPER: - self._caps_state = CapsState.LOCK - self._caps_key.set_icon("icons_mici/settings/keyboard/caps_lock.png", icon_size=(39, 38)) - else: - self._set_uppercase(False) - - def _handle_mouse_release(self, mouse_pos: MousePos): - if self._closest_key[0] is not None: - if self._closest_key[0] == self._caps_key: - self._set_uppercase(True) - elif self._closest_key[0] in (self._123_key, self._123_key2): - self._set_keys(self._special_keys) - elif self._closest_key[0] == self._abc_key: - self._set_uppercase(False) - elif self._closest_key[0] == self._super_special_key: - self._set_keys(self._super_special_keys) - else: - self._text += self._closest_key[0].char - - # Reset caps state - if self._caps_state == CapsState.UPPER: - self._set_uppercase(False) - - # ensure minimum selected animation time - key_selected_dt = rl.get_time() - (self._selected_key_t or 0) - cur_t = rl.get_time() - self._unselect_key_t = cur_t + KEY_MIN_ANIMATION_TIME if (key_selected_dt < KEY_MIN_ANIMATION_TIME) else cur_t - - def backspace(self): - if self._text: - self._text = self._text[:-1] - - def space(self): - self._text += ' ' - - def _update_state(self): - # update selected key filter - self._selected_key_filter.update(self._closest_key[0] is not None) - - # unselect key after animation plays - if self._unselect_key_t is not None and rl.get_time() > self._unselect_key_t: - self._closest_key = (None, float('inf')) - self._unselect_key_t = None - self._selected_key_t = None - - def _lay_out_keys(self, bg_x, bg_y, keys: list[list[Key]]): - key_rect = rl.Rectangle(bg_x, bg_y, self._txt_bg.width, self._txt_bg.height) - for row_idx, row in enumerate(keys): - padding = KEYBOARD_ROW_PADDING[row_idx] - step_y = (key_rect.height - 2 * KEYBOARD_COLUMN_PADDING) / (len(keys) - 1) - for key_idx, key in enumerate(row): - key_x = key_rect.x + padding + key_idx * ((key_rect.width - 2 * padding) / (len(row) - 1)) - key_y = key_rect.y + KEYBOARD_COLUMN_PADDING + row_idx * step_y - - if self._closest_key[0] is None: - key.set_alpha(1.0) - key.set_font_size(CHAR_FONT_SIZE) - elif key == self._closest_key[0]: - # push key up with a max and inward so user can see key easier - key_y = max(key_y - 120, 40) - key_x += np.interp(key_x, [self._rect.x, self._rect.x + self._rect.width], [100, -100]) - key.set_alpha(1.0) - key.set_font_size(SELECTED_CHAR_FONT_SIZE) - - # draw black circle behind selected key - circle_alpha = int(self._selected_key_filter.x * 225) - rl.draw_circle_gradient(int(key_x + key.rect.width / 2), int(key_y + key.rect.height / 2), - SELECTED_CHAR_FONT_SIZE, rl.Color(0, 0, 0, circle_alpha), rl.BLANK) - else: - # move other keys away from selected key a bit - dx = key.original_position.x - self._closest_key[0].original_position.x - dy = key.original_position.y - self._closest_key[0].original_position.y - distance_from_selected_key = fast_euclidean_distance(dx, dy) - - inv = 1 / (distance_from_selected_key or 1.0) - ux = dx * inv - uy = dy * inv - - # NOTE: hardcode to 20 to get entire keyboard to move - push_pixels = np.interp(distance_from_selected_key, [0, 250], [20, 0]) - key_x += ux * push_pixels - key_y += uy * push_pixels - - # TODO: slow enough to use an approximation or nah? also caching might work - font_size = np.interp(distance_from_selected_key, [0, 150], [CHAR_NEAR_FONT_SIZE, CHAR_FONT_SIZE]) - - key_alpha = np.interp(distance_from_selected_key, [0, 100], [1.0, 0.35]) - key.set_alpha(key_alpha) - key.set_font_size(font_size) - - # TODO: I like the push amount, so we should clip the pos inside the keyboard rect - key.set_parent_rect(self._rect) - key.set_position(key_x, key_y) - - def _render(self, _): - # draw bg - bg_x = self._rect.x + (self._rect.width - self._txt_bg.width) / 2 - bg_y = self._rect.y + self._rect.height - self._txt_bg.height - - scale = self._bg_scale_filter.update(1.0307692307692307 if self._closest_key[0] is not None else 1.0) - src_rec = rl.Rectangle(0, 0, self._txt_bg.width, self._txt_bg.height) - dest_rec = rl.Rectangle(self._rect.x + self._rect.width / 2 - self._txt_bg.width * scale / 2, bg_y, - self._txt_bg.width * scale, self._txt_bg.height) - - rl.draw_texture_pro(self._txt_bg, src_rec, dest_rec, rl.Vector2(0, 0), 0.0, rl.WHITE) - - # draw keys - if not self._initialized: - for keys in (self._lower_keys, self._upper_keys, self._special_keys, self._super_special_keys): - self._lay_out_keys(bg_x, bg_y, keys) - self._initialized = True - - self._lay_out_keys(bg_x, bg_y, self._current_keys) - for row in self._current_keys: - for key in row: - key.render() diff --git a/system/ui/widgets/network.py b/system/ui/widgets/network.py deleted file mode 100644 index 8f5168958f0075..00000000000000 --- a/system/ui/widgets/network.py +++ /dev/null @@ -1,486 +0,0 @@ -from enum import IntEnum -from functools import partial -from typing import cast - -import pyray as rl -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.wifi_manager import WifiManager, SecurityType, Network, MeteredType -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 -from openpilot.system.ui.widgets.keyboard import Keyboard -from openpilot.system.ui.widgets.label import gui_label -from openpilot.system.ui.widgets.scroller_tici import Scroller -from openpilot.system.ui.widgets.list_view import ButtonAction, ListItem, MultipleButtonAction, ToggleAction, button_item, text_item - -# These are only used for AdvancedNetworkSettings, standalone apps just need WifiManagerUI -try: - from openpilot.common.params import Params - from openpilot.selfdrive.ui.ui_state import ui_state - from openpilot.selfdrive.ui.lib.prime_state import PrimeType -except Exception: - Params = None - ui_state = None - PrimeType = None - -NM_DEVICE_STATE_NEED_AUTH = 60 -MIN_PASSWORD_LENGTH = 8 -MAX_PASSWORD_LENGTH = 64 -ITEM_HEIGHT = 160 -ICON_SIZE = 50 - -STRENGTH_ICONS = [ - "icons/wifi_strength_low.png", - "icons/wifi_strength_medium.png", - "icons/wifi_strength_high.png", - "icons/wifi_strength_full.png", -] - - -class PanelType(IntEnum): - WIFI = 0 - ADVANCED = 1 - - -class UIState(IntEnum): - IDLE = 0 - CONNECTING = 1 - NEEDS_AUTH = 2 - SHOW_FORGET_CONFIRM = 3 - FORGETTING = 4 - - -class NavButton(Widget): - def __init__(self, text: str): - super().__init__() - self.text = text - self.set_rect(rl.Rectangle(0, 0, 400, 100)) - - def _render(self, _): - color = rl.Color(74, 74, 74, 255) if self.is_pressed else rl.Color(57, 57, 57, 255) - rl.draw_rectangle_rounded(self._rect, 0.6, 10, color) - gui_label(self.rect, self.text, font_size=60, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - - -class NetworkUI(Widget): - def __init__(self, wifi_manager: WifiManager): - super().__init__() - self._wifi_manager = wifi_manager - self._current_panel: PanelType = PanelType.WIFI - self._wifi_panel = WifiManagerUI(wifi_manager) - self._advanced_panel = AdvancedNetworkSettings(wifi_manager) - self._nav_button = NavButton(tr("Advanced")) - self._nav_button.set_click_callback(self._cycle_panel) - - def show_event(self): - self._set_current_panel(PanelType.WIFI) - self._wifi_panel.show_event() - - def hide_event(self): - self._wifi_panel.hide_event() - - def _cycle_panel(self): - if self._current_panel == PanelType.WIFI: - self._set_current_panel(PanelType.ADVANCED) - else: - self._set_current_panel(PanelType.WIFI) - - def _render(self, _): - # subtract button - content_rect = rl.Rectangle(self._rect.x, self._rect.y + self._nav_button.rect.height + 40, - self._rect.width, self._rect.height - self._nav_button.rect.height - 40) - if self._current_panel == PanelType.WIFI: - self._nav_button.text = tr("Advanced") - self._nav_button.set_position(self._rect.x + self._rect.width - self._nav_button.rect.width, self._rect.y + 20) - self._wifi_panel.render(content_rect) - else: - self._nav_button.text = tr("Back") - self._nav_button.set_position(self._rect.x, self._rect.y + 20) - self._advanced_panel.render(content_rect) - - self._nav_button.render() - - def _set_current_panel(self, panel: PanelType): - self._current_panel = panel - - -class AdvancedNetworkSettings(Widget): - def __init__(self, wifi_manager: WifiManager): - super().__init__() - self._wifi_manager = wifi_manager - self._wifi_manager.add_callbacks(networks_updated=self._on_network_updated) - self._params = Params() - - self._keyboard = Keyboard(max_text_size=MAX_PASSWORD_LENGTH, min_text_size=MIN_PASSWORD_LENGTH, show_password_toggle=True) - - # Tethering - self._tethering_action = ToggleAction(initial_state=False) - tethering_btn = ListItem(lambda: tr("Enable Tethering"), action_item=self._tethering_action, callback=self._toggle_tethering) - - # Edit tethering password - self._tethering_password_action = ButtonAction(lambda: tr("EDIT")) - tethering_password_btn = ListItem(lambda: tr("Tethering Password"), action_item=self._tethering_password_action, callback=self._edit_tethering_password) - - # Roaming toggle - roaming_enabled = self._params.get_bool("GsmRoaming") - self._roaming_action = ToggleAction(initial_state=roaming_enabled) - self._roaming_btn = ListItem(lambda: tr("Enable Roaming"), action_item=self._roaming_action, callback=self._toggle_roaming) - - # Cellular metered toggle - cellular_metered = self._params.get_bool("GsmMetered") - self._cellular_metered_action = ToggleAction(initial_state=cellular_metered) - self._cellular_metered_btn = ListItem(lambda: tr("Cellular Metered"), - description=lambda: tr("Prevent large data uploads when on a metered cellular connection"), - action_item=self._cellular_metered_action, callback=self._toggle_cellular_metered) - - # APN setting - self._apn_btn = button_item(lambda: tr("APN Setting"), lambda: tr("EDIT"), callback=self._edit_apn) - - # Wi-Fi metered toggle - self._wifi_metered_action = MultipleButtonAction([lambda: tr("default"), lambda: tr("metered"), lambda: tr("unmetered")], 255, 0, - callback=self._toggle_wifi_metered) - wifi_metered_btn = ListItem(lambda: tr("Wi-Fi Network Metered"), description=lambda: tr("Prevent large data uploads when on a metered Wi-Fi connection"), - action_item=self._wifi_metered_action) - - items: list[Widget] = [ - tethering_btn, - tethering_password_btn, - text_item(lambda: tr("IP Address"), lambda: self._wifi_manager.ipv4_address), - self._roaming_btn, - self._apn_btn, - self._cellular_metered_btn, - wifi_metered_btn, - button_item(lambda: tr("Hidden Network"), lambda: tr("CONNECT"), callback=self._connect_to_hidden_network), - ] - - self._scroller = Scroller(items, line_separator=True, spacing=0) - - # Set initial config - metered = self._params.get_bool("GsmMetered") - self._wifi_manager.update_gsm_settings(roaming_enabled, self._params.get("GsmApn") or "", metered) - - def _on_network_updated(self, networks: list[Network]): - self._tethering_action.set_enabled(True) - self._tethering_action.set_state(self._wifi_manager.is_tethering_active()) - self._tethering_password_action.set_enabled(True) - - if self._wifi_manager.is_tethering_active() or self._wifi_manager.ipv4_address == "": - self._wifi_metered_action.set_enabled(False) - self._wifi_metered_action.selected_button = 0 - elif self._wifi_manager.ipv4_address != "": - metered = self._wifi_manager.current_network_metered - self._wifi_metered_action.set_enabled(True) - self._wifi_metered_action.selected_button = int(metered) if metered in (MeteredType.UNKNOWN, MeteredType.YES, MeteredType.NO) else 0 - - def _toggle_tethering(self): - checked = self._tethering_action.get_state() - self._tethering_action.set_enabled(False) - if checked: - self._wifi_metered_action.set_enabled(False) - self._wifi_manager.set_tethering_active(checked) - - def _toggle_roaming(self): - roaming_state = self._roaming_action.get_state() - self._params.put_bool("GsmRoaming", roaming_state) - self._wifi_manager.update_gsm_settings(roaming_state, self._params.get("GsmApn") or "", self._params.get_bool("GsmMetered")) - - def _edit_apn(self): - def update_apn(result): - if result != 1: - return - - apn = self._keyboard.text.strip() - if apn == "": - self._params.remove("GsmApn") - else: - self._params.put("GsmApn", apn) - - self._wifi_manager.update_gsm_settings(self._params.get_bool("GsmRoaming"), apn, self._params.get_bool("GsmMetered")) - - current_apn = self._params.get("GsmApn") or "" - self._keyboard.reset(min_text_size=0) - self._keyboard.set_title(tr("Enter APN"), tr("leave blank for automatic configuration")) - self._keyboard.set_text(current_apn) - gui_app.set_modal_overlay(self._keyboard, update_apn) - - def _toggle_cellular_metered(self): - metered = self._cellular_metered_action.get_state() - self._params.put_bool("GsmMetered", metered) - self._wifi_manager.update_gsm_settings(self._params.get_bool("GsmRoaming"), self._params.get("GsmApn") or "", metered) - - def _toggle_wifi_metered(self, metered): - metered_type = {0: MeteredType.UNKNOWN, 1: MeteredType.YES, 2: MeteredType.NO}.get(metered, MeteredType.UNKNOWN) - self._wifi_metered_action.set_enabled(False) - self._wifi_manager.set_current_network_metered(metered_type) - - def _connect_to_hidden_network(self): - def connect_hidden(result): - if result != 1: - return - - ssid = self._keyboard.text - if not ssid: - return - - def enter_password(result): - password = self._keyboard.text - if password == "": - # connect without password - self._wifi_manager.connect_to_network(ssid, "", hidden=True) - return - - self._wifi_manager.connect_to_network(ssid, password, hidden=True) - - self._keyboard.reset(min_text_size=0) - self._keyboard.set_title(tr("Enter password"), tr("for \"{}\"").format(ssid)) - gui_app.set_modal_overlay(self._keyboard, enter_password) - - self._keyboard.reset(min_text_size=1) - self._keyboard.set_title(tr("Enter SSID"), "") - gui_app.set_modal_overlay(self._keyboard, connect_hidden) - - def _edit_tethering_password(self): - def update_password(result): - if result != 1: - return - - password = self._keyboard.text - self._wifi_manager.set_tethering_password(password) - self._tethering_password_action.set_enabled(False) - - self._keyboard.reset(min_text_size=MIN_PASSWORD_LENGTH) - self._keyboard.set_title(tr("Enter new tethering password"), "") - self._keyboard.set_text(self._wifi_manager.tethering_password) - gui_app.set_modal_overlay(self._keyboard, update_password) - - def _update_state(self): - self._wifi_manager.process_callbacks() - - # If not using prime SIM, show GSM settings and enable IPv4 forwarding - show_cell_settings = ui_state.prime_state.get_type() in (PrimeType.NONE, PrimeType.LITE) - self._wifi_manager.set_ipv4_forward(show_cell_settings) - self._roaming_btn.set_visible(show_cell_settings) - self._apn_btn.set_visible(show_cell_settings) - self._cellular_metered_btn.set_visible(show_cell_settings) - - def _render(self, _): - self._scroller.render(self._rect) - - -class WifiManagerUI(Widget): - def __init__(self, wifi_manager: WifiManager): - super().__init__() - self._wifi_manager = wifi_manager - self.state: UIState = UIState.IDLE - 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[Network] = [] - self._networks_buttons: dict[str, Button] = {} - self._forget_networks_buttons: dict[str, Button] = {} - - self._wifi_manager.add_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 _update_state(self): - self._wifi_manager.process_callbacks() - - def _render(self, rect: rl.Rectangle): - if not self._networks: - gui_label(rect, tr("Scanning Wi-Fi networks..."), 72, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - return - - if self.state == UIState.NEEDS_AUTH and self._state_network: - self.keyboard.set_title(tr("Wrong password") if self._password_retry else tr("Enter password"), tr("for \"{}\"").format(self._state_network.ssid)) - self.keyboard.reset(min_text_size=MIN_PASSWORD_LENGTH) - 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: - confirm_dialog = ConfirmDialog("", tr("Forget"), tr("Cancel")) - confirm_dialog.set_text(tr("Forget Wi-Fi Network \"{}\"?").format(self._state_network.ssid)) - confirm_dialog.reset() - gui_app.set_modal_overlay(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() - - if len(password) >= MIN_PASSWORD_LENGTH: - self.connect_to_network(network, password) - elif result == 0: - self.state = UIState.IDLE - - def on_forgot_confirm_finished(self, network, result: int): - if result == 1: - self.forget_network(network) - elif result == 0: - self.state = UIState.IDLE - - def _draw_network_list(self, rect: rl.Rectangle): - content_rect = rl.Rectangle(rect.x, rect.y, rect.width, len(self._networks) * ITEM_HEIGHT) - offset = self.scroll_panel.update(rect, content_rect) - - rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) - for i, network in enumerate(self._networks): - y_offset = rect.y + i * ITEM_HEIGHT + offset - item_rect = rl.Rectangle(rect.x, y_offset, rect.width, ITEM_HEIGHT) - if not rl.check_collision_recs(item_rect, rect): - continue - - self._draw_network_item(item_rect, network) - if i < len(self._networks) - 1: - line_y = int(item_rect.y + item_rect.height - 1) - rl.draw_line(int(item_rect.x), int(line_y), int(item_rect.x + item_rect.width), line_y, rl.LIGHTGRAY) - - rl.end_scissor_mode() - - def _draw_network_item(self, rect, network: Network): - 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) - security_icon_rect = rl.Rectangle(signal_icon_rect.x - spacing - ICON_SIZE, rect.y + (ITEM_HEIGHT - ICON_SIZE) / 2, ICON_SIZE, ICON_SIZE) - - status_text = "" - if self.state == UIState.CONNECTING and self._state_network: - if self._state_network.ssid == network.ssid: - self._networks_buttons[network.ssid].set_enabled(False) - status_text = tr("CONNECTING...") - elif self.state == UIState.FORGETTING and self._state_network: - if self._state_network.ssid == network.ssid: - self._networks_buttons[network.ssid].set_enabled(False) - status_text = tr("FORGETTING...") - elif network.security_type == SecurityType.UNSUPPORTED: - self._networks_buttons[network.ssid].set_enabled(False) - else: - self._networks_buttons[network.ssid].set_enabled(True) - - self._networks_buttons[network.ssid].render(ssid_rect) - - if status_text: - status_text_rect = rl.Rectangle(security_icon_rect.x - 410, rect.y, 410, ITEM_HEIGHT) - gui_label(status_text_rect, status_text, font_size=48, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - else: - # If the network is saved, show the "Forget" button - if network.is_saved: - forget_btn_rect = rl.Rectangle( - security_icon_rect.x - self.btn_width - spacing, - rect.y + (ITEM_HEIGHT - 80) / 2, - self.btn_width, - 80, - ) - self._forget_networks_buttons[network.ssid].render(forget_btn_rect) - - self._draw_status_icon(security_icon_rect, network) - self._draw_signal_strength_icon(signal_icon_rect, network) - - def _networks_buttons_callback(self, network): - if not network.is_saved and network.security_type != SecurityType.OPEN: - self.state = UIState.NEEDS_AUTH - self._state_network = network - self._password_retry = False - elif not network.is_connected: - self.connect_to_network(network) - - def _forget_networks_buttons_callback(self, network): - self.state = UIState.SHOW_FORGET_CONFIRM - self._state_network = network - - 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 and self.state != UIState.CONNECTING: - icon_file = "icons/checkmark.png" - elif network.security_type == SecurityType.UNSUPPORTED: - icon_file = "icons/circled_slash.png" - elif network.security_type != SecurityType.OPEN: - icon_file = "icons/lock_closed.png" - - if not icon_file: - return - - texture = gui_app.texture(icon_file, ICON_SIZE, ICON_SIZE) - 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: 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: Network, password=''): - self.state = UIState.CONNECTING - self._state_network = network - if network.is_saved and not password: - self._wifi_manager.activate_connection(network.ssid) - else: - self._wifi_manager.connect_to_network(network.ssid, password) - - def forget_network(self, network: Network): - self.state = UIState.FORGETTING - self._state_network = network - self._wifi_manager.forget_connection(network.ssid) - - 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=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, button_style=ButtonStyle.TRANSPARENT_WHITE_TEXT) - self._networks_buttons[n.ssid].set_touch_valid_callback(lambda: self.scroll_panel.is_touch_valid()) - self._forget_networks_buttons[n.ssid] = Button(tr("Forget"), partial(self._forget_networks_buttons_callback, n), button_style=ButtonStyle.FORGET_WIFI, - font_size=45) - self._forget_networks_buttons[n.ssid].set_touch_valid_callback(lambda: self.scroll_panel.is_touch_valid()) - - def _on_need_auth(self, ssid): - 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): - if self.state == UIState.CONNECTING: - self.state = UIState.IDLE - - def _on_forgotten(self): - if self.state == UIState.FORGETTING: - 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_ui = WifiManagerUI(WifiManager()) - - for _ in gui_app.render(): - wifi_ui.render(rl.Rectangle(50, 50, gui_app.width - 100, gui_app.height - 100)) - - gui_app.close() - - -if __name__ == "__main__": - main() diff --git a/system/ui/widgets/option_dialog.py b/system/ui/widgets/option_dialog.py deleted file mode 100644 index 62578d1cfba9d6..00000000000000 --- a/system/ui/widgets/option_dialog.py +++ /dev/null @@ -1,78 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.application import FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.widgets import Widget, DialogResult -from openpilot.system.ui.widgets.button import Button, ButtonStyle -from openpilot.system.ui.widgets.label import gui_label -from openpilot.system.ui.widgets.scroller_tici import Scroller - -# Constants -MARGIN = 50 -TITLE_FONT_SIZE = 70 -ITEM_HEIGHT = 135 -BUTTON_SPACING = 50 -BUTTON_HEIGHT = 160 -ITEM_SPACING = 50 -LIST_ITEM_SPACING = 25 - - -class MultiOptionDialog(Widget): - def __init__(self, title, options, current="", option_font_weight=FontWeight.MEDIUM): - super().__init__() - self.title = title - self.options = options - self.current = current - self.selection = current - self._result: DialogResult = DialogResult.NO_ACTION - - # Create scroller with option buttons - self.option_buttons = [Button(option, click_callback=lambda opt=option: self._on_option_clicked(opt), - font_weight=option_font_weight, - text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT, button_style=ButtonStyle.NORMAL, - text_padding=50, elide_right=True) for option in options] - self.scroller = Scroller(self.option_buttons, spacing=LIST_ITEM_SPACING) - - self.cancel_button = Button(lambda: tr("Cancel"), click_callback=lambda: self._set_result(DialogResult.CANCEL)) - self.select_button = Button(lambda: tr("Select"), click_callback=lambda: self._set_result(DialogResult.CONFIRM), button_style=ButtonStyle.PRIMARY) - - def _set_result(self, result: DialogResult): - self._result = result - - def _on_option_clicked(self, option): - self.selection = option - - def _render(self, rect): - dialog_rect = rl.Rectangle(rect.x + MARGIN, rect.y + MARGIN, rect.width - 2 * MARGIN, rect.height - 2 * MARGIN) - rl.draw_rectangle_rounded(dialog_rect, 0.02, 20, rl.Color(30, 30, 30, 255)) - - content_rect = rl.Rectangle(dialog_rect.x + MARGIN, dialog_rect.y + MARGIN, - dialog_rect.width - 2 * MARGIN, dialog_rect.height - 2 * MARGIN) - - gui_label(rl.Rectangle(content_rect.x, content_rect.y, content_rect.width, TITLE_FONT_SIZE), self.title, 70, font_weight=FontWeight.BOLD) - - # Options area - options_y = content_rect.y + TITLE_FONT_SIZE + ITEM_SPACING - options_h = content_rect.height - TITLE_FONT_SIZE - BUTTON_HEIGHT - 2 * ITEM_SPACING - options_rect = rl.Rectangle(content_rect.x, options_y, content_rect.width, options_h) - - # Update button styles and set width based on selection - for i, option in enumerate(self.options): - selected = option == self.selection - button = self.option_buttons[i] - button.set_button_style(ButtonStyle.PRIMARY if selected else ButtonStyle.NORMAL) - button.set_rect(rl.Rectangle(0, 0, options_rect.width, ITEM_HEIGHT)) - - self.scroller.render(options_rect) - - # Buttons - button_y = content_rect.y + content_rect.height - BUTTON_HEIGHT - button_w = (content_rect.width - BUTTON_SPACING) / 2 - - cancel_rect = rl.Rectangle(content_rect.x, button_y, button_w, BUTTON_HEIGHT) - self.cancel_button.render(cancel_rect) - - select_rect = rl.Rectangle(content_rect.x + button_w + BUTTON_SPACING, button_y, button_w, BUTTON_HEIGHT) - self.select_button.set_enabled(self.selection != self.current) - self.select_button.render(select_rect) - - return self._result diff --git a/system/ui/widgets/scroller.py b/system/ui/widgets/scroller.py deleted file mode 100644 index f33ba941bf9190..00000000000000 --- a/system/ui/widgets/scroller.py +++ /dev/null @@ -1,264 +0,0 @@ -import pyray as rl -import numpy as np -from collections.abc import Callable - -from openpilot.common.filter_simple import FirstOrderFilter, BounceFilter -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState -from openpilot.system.ui.widgets import Widget - -ITEM_SPACING = 20 -LINE_COLOR = rl.GRAY -LINE_PADDING = 40 -ANIMATION_SCALE = 0.6 - -MIN_ZOOM_ANIMATION_TIME = 0.075 # seconds -DO_ZOOM = False -DO_JELLO = False -SCROLL_BAR = False - - -class LineSeparator(Widget): - def __init__(self, height: int = 1): - super().__init__() - self._rect = rl.Rectangle(0, 0, 0, height) - - def set_parent_rect(self, parent_rect: rl.Rectangle) -> None: - super().set_parent_rect(parent_rect) - self._rect.width = parent_rect.width - - def _render(self, _): - rl.draw_line(int(self._rect.x) + LINE_PADDING, int(self._rect.y), - int(self._rect.x + self._rect.width) - LINE_PADDING, int(self._rect.y), - LINE_COLOR) - - -class Scroller(Widget): - def __init__(self, items: list[Widget], horizontal: bool = True, snap_items: bool = True, spacing: int = ITEM_SPACING, - line_separator: bool = False, pad_start: int = ITEM_SPACING, pad_end: int = ITEM_SPACING): - super().__init__() - self._items: list[Widget] = [] - self._horizontal = horizontal - self._snap_items = snap_items - self._spacing = spacing - self._line_separator = LineSeparator() if line_separator else None - self._pad_start = pad_start - self._pad_end = pad_end - - self._reset_scroll_at_show = True - - self._scrolling_to: float | None = None - self._scroll_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - self._zoom_filter = FirstOrderFilter(1.0, 0.2, 1 / gui_app.target_fps) - self._zoom_out_t: float = 0.0 - - # layout state - self._visible_items: list[Widget] = [] - self._content_size: float = 0.0 - self._scroll_offset: float = 0.0 - - self._item_pos_filter = BounceFilter(0.0, 0.05, 1 / gui_app.target_fps) - - # when not pressed, snap to closest item to be center - self._scroll_snap_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) - - self.scroll_panel = GuiScrollPanel2(self._horizontal, handle_out_of_bounds=not self._snap_items) - self._scroll_enabled: bool | Callable[[], bool] = True - - self._txt_scroll_indicator = gui_app.texture("icons_mici/settings/vertical_scroll_indicator.png", 40, 80) - - for item in items: - self.add_widget(item) - - def set_reset_scroll_at_show(self, scroll: bool): - self._reset_scroll_at_show = scroll - - def scroll_to(self, pos: float, smooth: bool = False): - # already there - if abs(pos) < 1: - return - - # FIXME: the padding correction doesn't seem correct - scroll_offset = self.scroll_panel.get_offset() - pos - if smooth: - self._scrolling_to = scroll_offset - else: - self.scroll_panel.set_offset(scroll_offset) - - @property - def is_auto_scrolling(self) -> bool: - return self._scrolling_to is not None - - def add_widget(self, item: Widget) -> None: - self._items.append(item) - item.set_touch_valid_callback(lambda: self.scroll_panel.is_touch_valid() and self.enabled) - - def set_scrolling_enabled(self, enabled: bool | Callable[[], bool]) -> None: - """Set whether scrolling is enabled (does not affect widget enabled state).""" - self._scroll_enabled = enabled - - def _update_state(self): - if DO_ZOOM: - if self._scrolling_to is not None or self.scroll_panel.state != ScrollState.STEADY: - self._zoom_out_t = rl.get_time() + MIN_ZOOM_ANIMATION_TIME - self._zoom_filter.update(0.85) - else: - if self._zoom_out_t is not None: - if rl.get_time() > self._zoom_out_t: - self._zoom_filter.update(1.0) - else: - self._zoom_filter.update(0.85) - - # Cancel auto-scroll if user starts manually scrolling - if self._scrolling_to is not None and (self.scroll_panel.state == ScrollState.PRESSED or self.scroll_panel.state == ScrollState.MANUAL_SCROLL): - self._scrolling_to = None - - if self._scrolling_to is not None: - self._scroll_filter.update(self._scrolling_to) - self.scroll_panel.set_offset(self._scroll_filter.x) - - if abs(self._scroll_filter.x - self._scrolling_to) < 1: - self.scroll_panel.set_offset(self._scrolling_to) - self._scrolling_to = None - else: - # keep current scroll position up to date - self._scroll_filter.x = self.scroll_panel.get_offset() - - def _get_scroll(self, visible_items: list[Widget], content_size: float) -> float: - scroll_enabled = self._scroll_enabled() if callable(self._scroll_enabled) else self._scroll_enabled - self.scroll_panel.set_enabled(scroll_enabled and self.enabled) - self.scroll_panel.update(self._rect, content_size) - if not self._snap_items: - return round(self.scroll_panel.get_offset()) - - # Snap closest item to center - center_pos = self._rect.x + self._rect.width / 2 if self._horizontal else self._rect.y + self._rect.height / 2 - closest_delta_pos = float('inf') - scroll_snap_idx: int | None = None - for idx, item in enumerate(visible_items): - if self._horizontal: - delta_pos = (item.rect.x + item.rect.width / 2) - center_pos - else: - delta_pos = (item.rect.y + item.rect.height / 2) - center_pos - if abs(delta_pos) < abs(closest_delta_pos): - closest_delta_pos = delta_pos - scroll_snap_idx = idx - - if scroll_snap_idx is not None: - snap_item = visible_items[scroll_snap_idx] - if self.is_pressed: - # no snapping until released - self._scroll_snap_filter.x = 0 - else: - # TODO: this doesn't handle two small buttons at the edges well - if self._horizontal: - snap_delta_pos = (center_pos - (snap_item.rect.x + snap_item.rect.width / 2)) / 10 - snap_delta_pos = min(snap_delta_pos, -self.scroll_panel.get_offset() / 10) - snap_delta_pos = max(snap_delta_pos, (self._rect.width - self.scroll_panel.get_offset() - content_size) / 10) - else: - snap_delta_pos = (center_pos - (snap_item.rect.y + snap_item.rect.height / 2)) / 10 - snap_delta_pos = min(snap_delta_pos, -self.scroll_panel.get_offset() / 10) - snap_delta_pos = max(snap_delta_pos, (self._rect.height - self.scroll_panel.get_offset() - content_size) / 10) - self._scroll_snap_filter.update(snap_delta_pos) - - self.scroll_panel.set_offset(self.scroll_panel.get_offset() + self._scroll_snap_filter.x) - - return self.scroll_panel.get_offset() - - def _layout(self): - self._visible_items = [item for item in self._items if item.is_visible] - - # Add line separator between items - if self._line_separator is not None: - l = len(self._visible_items) - for i in range(1, len(self._visible_items)): - self._visible_items.insert(l - i, self._line_separator) - - self._content_size = sum(item.rect.width if self._horizontal else item.rect.height for item in self._visible_items) - self._content_size += self._spacing * (len(self._visible_items) - 1) - self._content_size += self._pad_start + self._pad_end - - self._scroll_offset = self._get_scroll(self._visible_items, self._content_size) - - rl.begin_scissor_mode(int(self._rect.x), int(self._rect.y), - int(self._rect.width), int(self._rect.height)) - - self._item_pos_filter.update(self._scroll_offset) - - cur_pos = 0 - for idx, item in enumerate(self._visible_items): - spacing = self._spacing if (idx > 0) else self._pad_start - # Nicely lay out items horizontally/vertically - if self._horizontal: - x = self._rect.x + cur_pos + spacing - y = self._rect.y + (self._rect.height - item.rect.height) / 2 - cur_pos += item.rect.width + spacing - else: - x = self._rect.x + (self._rect.width - item.rect.width) / 2 - y = self._rect.y + cur_pos + spacing - cur_pos += item.rect.height + spacing - - # Consider scroll - if self._horizontal: - x += self._scroll_offset - else: - y += self._scroll_offset - - # Add some jello effect when scrolling - if DO_JELLO: - if self._horizontal: - cx = self._rect.x + self._rect.width / 2 - jello_offset = self._scroll_offset - np.interp(x + item.rect.width / 2, - [self._rect.x, cx, self._rect.x + self._rect.width], - [self._item_pos_filter.x, self._scroll_offset, self._item_pos_filter.x]) - x -= np.clip(jello_offset, -20, 20) - else: - cy = self._rect.y + self._rect.height / 2 - jello_offset = self._scroll_offset - np.interp(y + item.rect.height / 2, - [self._rect.y, cy, self._rect.y + self._rect.height], - [self._item_pos_filter.x, self._scroll_offset, self._item_pos_filter.x]) - y -= np.clip(jello_offset, -20, 20) - - # Update item state - item.set_position(round(x), round(y)) # round to prevent jumping when settling - item.set_parent_rect(self._rect) - - def _render(self, _): - for item in self._visible_items: - # Skip rendering if not in viewport - if not rl.check_collision_recs(item.rect, self._rect): - continue - - # Scale each element around its own origin when scrolling - scale = self._zoom_filter.x - if scale != 1.0: - rl.rl_push_matrix() - rl.rl_scalef(scale, scale, 1.0) - rl.rl_translatef((1 - scale) * (item.rect.x + item.rect.width / 2) / scale, - (1 - scale) * (item.rect.y + item.rect.height / 2) / scale, 0) - item.render() - rl.rl_pop_matrix() - else: - item.render() - - # Draw scroll indicator - if SCROLL_BAR and not self._horizontal and len(self._visible_items) > 0: - _real_content_size = self._content_size - self._rect.height + self._txt_scroll_indicator.height - scroll_bar_y = -self._scroll_offset / _real_content_size * self._rect.height - scroll_bar_y = min(max(scroll_bar_y, self._rect.y), self._rect.y + self._rect.height - self._txt_scroll_indicator.height) - rl.draw_texture_ex(self._txt_scroll_indicator, rl.Vector2(self._rect.x, scroll_bar_y), 0, 1.0, rl.WHITE) - - rl.end_scissor_mode() - - def show_event(self): - super().show_event() - if self._reset_scroll_at_show: - self.scroll_panel.set_offset(0.0) - - for item in self._items: - item.show_event() - - def hide_event(self): - super().hide_event() - for item in self._items: - item.hide_event() diff --git a/system/ui/widgets/scroller_tici.py b/system/ui/widgets/scroller_tici.py deleted file mode 100644 index a843010d56bac6..00000000000000 --- a/system/ui/widgets/scroller_tici.py +++ /dev/null @@ -1,90 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.widgets import Widget - -ITEM_SPACING = 40 -LINE_COLOR = rl.GRAY -LINE_PADDING = 40 - - -class LineSeparator(Widget): - def __init__(self, height: int = 1): - super().__init__() - self._rect = rl.Rectangle(0, 0, 0, height) - - def set_parent_rect(self, parent_rect: rl.Rectangle) -> None: - super().set_parent_rect(parent_rect) - self._rect.width = parent_rect.width - - def _render(self, _): - rl.draw_line(int(self._rect.x) + LINE_PADDING, int(self._rect.y), - int(self._rect.x + self._rect.width) - LINE_PADDING, int(self._rect.y), - LINE_COLOR) - - -class Scroller(Widget): - def __init__(self, items: list[Widget], spacing: int = ITEM_SPACING, line_separator: bool = False, pad_end: bool = True): - super().__init__() - self._items: list[Widget] = [] - self._spacing = spacing - self._line_separator = LineSeparator() if line_separator else None - self._pad_end = pad_end - - self.scroll_panel = GuiScrollPanel() - - for item in items: - self.add_widget(item) - - def add_widget(self, item: Widget) -> None: - self._items.append(item) - item.set_touch_valid_callback(self.scroll_panel.is_touch_valid) - - def _render(self, _): - # TODO: don't draw items that are not in the viewport - visible_items = [item for item in self._items if item.is_visible] - - # Add line separator between items - if self._line_separator is not None: - l = len(visible_items) - for i in range(1, len(visible_items)): - visible_items.insert(l - i, self._line_separator) - - content_height = sum(item.rect.height for item in visible_items) + self._spacing * (len(visible_items)) - if not self._pad_end: - content_height -= self._spacing - scroll = self.scroll_panel.update(self._rect, rl.Rectangle(0, 0, self._rect.width, content_height)) - - rl.begin_scissor_mode(int(self._rect.x), int(self._rect.y), - int(self._rect.width), int(self._rect.height)) - - cur_height = 0 - for idx, item in enumerate(visible_items): - if not item.is_visible: - continue - - # Nicely lay out items vertically - x = self._rect.x - y = self._rect.y + cur_height + self._spacing * (idx != 0) - cur_height += item.rect.height + self._spacing * (idx != 0) - - # Consider scroll - y += scroll - - # Update item state - item.set_position(x, y) - item.set_parent_rect(self._rect) - item.render() - - rl.end_scissor_mode() - - def show_event(self): - super().show_event() - # Reset to top - self.scroll_panel.set_offset(0) - for item in self._items: - item.show_event() - - def hide_event(self): - super().hide_event() - for item in self._items: - item.hide_event() diff --git a/system/ui/widgets/slider.py b/system/ui/widgets/slider.py deleted file mode 100644 index 455cdeef712adb..00000000000000 --- a/system/ui/widgets/slider.py +++ /dev/null @@ -1,186 +0,0 @@ -from collections.abc import Callable - -import pyray as rl - -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.label import UnifiedLabel -from openpilot.common.filter_simple import FirstOrderFilter - - -class SmallSlider(Widget): - HORIZONTAL_PADDING = 8 - CONFIRM_DELAY = 0.2 - - def __init__(self, title: str, confirm_callback: Callable | None = None): - # TODO: unify this with BigConfirmationDialogV2 - super().__init__() - self._confirm_callback = confirm_callback - - self._font = gui_app.font(FontWeight.DISPLAY) - - self._load_assets() - - self._drag_threshold = -self._rect.width // 2 - - # State - self._opacity_filter = FirstOrderFilter(1.0, 0.1, 1 / gui_app.target_fps) - self._confirmed_time = 0.0 - self._confirm_callback_called = False # we keep dialog open by default, only call once - self._start_x_circle = 0.0 - self._scroll_x_circle = 0.0 - self._scroll_x_circle_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) - - self._is_dragging_circle = False - - self._label = UnifiedLabel(title, font_size=36, font_weight=FontWeight.MEDIUM, text_color=rl.Color(255, 255, 255, int(255 * 0.65)), - alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, - alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, line_height=0.9) - - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 316 + self.HORIZONTAL_PADDING * 2, 100)) - - self._bg_txt = gui_app.texture("icons_mici/setup/small_slider/slider_bg.png", 316, 100) - self._circle_bg_txt = gui_app.texture("icons_mici/setup/small_slider/slider_red_circle.png", 100, 100) - self._circle_arrow_txt = gui_app.texture("icons_mici/setup/small_slider/slider_arrow.png", 37, 32) - - @property - def confirmed(self) -> bool: - return self._confirmed_time > 0.0 - - def reset(self): - # reset all slider state - self._is_dragging_circle = False - self._confirmed_time = 0.0 - self._confirm_callback_called = False - - def set_opacity(self, opacity: float, smooth: bool = False): - if smooth: - self._opacity_filter.update(opacity) - else: - self._opacity_filter.x = opacity - - @property - def slider_percentage(self): - activated_pos = -self._bg_txt.width + self._circle_bg_txt.width - return min(max(-self._scroll_x_circle_filter.x / abs(activated_pos), 0.0), 1.0) - - def _on_confirm(self): - if self._confirm_callback: - self._confirm_callback() - - def _handle_mouse_event(self, mouse_event): - super()._handle_mouse_event(mouse_event) - - if mouse_event.left_pressed: - # touch rect goes to the padding - circle_button_rect = rl.Rectangle( - self._rect.x + (self._rect.width - self._circle_bg_txt.width) + self._scroll_x_circle_filter.x - self.HORIZONTAL_PADDING * 2, - self._rect.y, - self._circle_bg_txt.width + self.HORIZONTAL_PADDING * 2, - self._rect.height, - ) - if rl.check_collision_point_rec(mouse_event.pos, circle_button_rect): - self._start_x_circle = mouse_event.pos.x - self._is_dragging_circle = True - - elif mouse_event.left_released: - # swiped to left - if self._scroll_x_circle_filter.x < self._drag_threshold: - self._confirmed_time = rl.get_time() - - self._is_dragging_circle = False - - if self._is_dragging_circle: - self._scroll_x_circle = mouse_event.pos.x - self._start_x_circle - - def _update_state(self): - super()._update_state() - # TODO: this math can probably be cleaned up to remove duplicate stuff - activated_pos = int(-self._bg_txt.width + self._circle_bg_txt.width) - self._scroll_x_circle = max(min(self._scroll_x_circle, 0), activated_pos) - - if self._confirmed_time > 0: - # swiped left to confirm - self._scroll_x_circle_filter.update(activated_pos) - - # activate once animation completes, small threshold for small floats - if self._scroll_x_circle_filter.x < (activated_pos + 1): - if not self._confirm_callback_called and (rl.get_time() - self._confirmed_time) >= self.CONFIRM_DELAY: - self._on_confirm() - self._confirm_callback_called = True - - elif not self._is_dragging_circle: - # reset back to right - self._scroll_x_circle_filter.update(0) - else: - # not activated yet, keep movement 1:1 - self._scroll_x_circle_filter.x = self._scroll_x_circle - - def _render(self, _): - # TODO: iOS text shimmering animation - - white = rl.Color(255, 255, 255, int(255 * self._opacity_filter.x)) - - bg_txt_x = self._rect.x + (self._rect.width - self._bg_txt.width) / 2 - bg_txt_y = self._rect.y + (self._rect.height - self._bg_txt.height) / 2 - rl.draw_texture_ex(self._bg_txt, rl.Vector2(bg_txt_x, bg_txt_y), 0.0, 1.0, white) - - btn_x = bg_txt_x + self._bg_txt.width - self._circle_bg_txt.width + self._scroll_x_circle_filter.x - btn_y = self._rect.y + (self._rect.height - self._circle_bg_txt.height) / 2 - - if self._confirmed_time == 0.0 or self._scroll_x_circle > 0: - self._label.set_text_color(rl.Color(255, 255, 255, int(255 * 0.65 * (1.0 - self.slider_percentage) * self._opacity_filter.x))) - label_rect = rl.Rectangle( - self._rect.x + 20, - self._rect.y, - self._rect.width - self._circle_bg_txt.width - 20 * 2.5, - self._rect.height, - ) - self._label.render(label_rect) - - # circle and arrow - rl.draw_texture_ex(self._circle_bg_txt, rl.Vector2(btn_x, btn_y), 0.0, 1.0, white) - - arrow_x = btn_x + (self._circle_bg_txt.width - self._circle_arrow_txt.width) / 2 - arrow_y = btn_y + (self._circle_bg_txt.height - self._circle_arrow_txt.height) / 2 - rl.draw_texture_ex(self._circle_arrow_txt, rl.Vector2(arrow_x, arrow_y), 0.0, 1.0, white) - - -class LargerSlider(SmallSlider): - def __init__(self, title: str, confirm_callback: Callable | None = None, green: bool = True): - self._green = green - super().__init__(title, confirm_callback=confirm_callback) - - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 520 + self.HORIZONTAL_PADDING * 2, 115)) - - self._bg_txt = gui_app.texture("icons_mici/setup/small_slider/slider_bg_larger.png", 520, 115) - circle_fn = "slider_green_rounded_rectangle" if self._green else "slider_black_rounded_rectangle" - self._circle_bg_txt = gui_app.texture(f"icons_mici/setup/small_slider/{circle_fn}.png", 180, 115) - self._circle_arrow_txt = gui_app.texture("icons_mici/setup/small_slider/slider_arrow.png", 64, 55) - - -class BigSlider(SmallSlider): - def __init__(self, title: str, icon: rl.Texture, confirm_callback: Callable | None = None): - self._icon = icon - super().__init__(title, confirm_callback=confirm_callback) - self._label = UnifiedLabel(title, font_size=48, font_weight=FontWeight.DISPLAY, text_color=rl.Color(255, 255, 255, int(255 * 0.65)), - alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, - line_height=0.875) - - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 520 + self.HORIZONTAL_PADDING * 2, 180)) - - self._bg_txt = gui_app.texture("icons_mici/buttons/slider_bg.png", 520, 180) - self._circle_bg_txt = gui_app.texture("icons_mici/buttons/button_circle.png", 180, 180) - self._circle_arrow_txt = self._icon - - -class RedBigSlider(BigSlider): - def _load_assets(self): - self.set_rect(rl.Rectangle(0, 0, 520 + self.HORIZONTAL_PADDING * 2, 180)) - - self._bg_txt = gui_app.texture("icons_mici/buttons/slider_bg.png", 520, 180) - self._circle_bg_txt = gui_app.texture("icons_mici/buttons/button_circle_red.png", 180, 180) - self._circle_arrow_txt = self._icon diff --git a/system/ui/widgets/toggle.py b/system/ui/widgets/toggle.py deleted file mode 100644 index 0fbf3c844a10b9..00000000000000 --- a/system/ui/widgets/toggle.py +++ /dev/null @@ -1,81 +0,0 @@ -import pyray as rl -from collections.abc import Callable -from openpilot.system.ui.lib.application import MousePos -from openpilot.system.ui.widgets import Widget - -ON_COLOR = rl.Color(51, 171, 76, 255) -OFF_COLOR = rl.Color(0x39, 0x39, 0x39, 255) -KNOB_COLOR = rl.WHITE -DISABLED_ON_COLOR = rl.Color(0x22, 0x77, 0x22, 255) # Dark green when disabled + on -DISABLED_OFF_COLOR = rl.Color(0x39, 0x39, 0x39, 255) -DISABLED_KNOB_COLOR = rl.Color(0x88, 0x88, 0x88, 255) -WIDTH, HEIGHT = 160, 80 -BG_HEIGHT = 60 -ANIMATION_SPEED = 8.0 - - -class Toggle(Widget): - def __init__(self, initial_state: bool = False, callback: Callable[[bool], None] | None = None): - super().__init__() - self._state = initial_state - self._callback = callback - self._enabled = True - self._progress = 1.0 if initial_state else 0.0 - self._target = self._progress - self._clicked = False - - def set_rect(self, rect: rl.Rectangle): - self._rect = rl.Rectangle(rect.x, rect.y, WIDTH, HEIGHT) - - def _handle_mouse_release(self, mouse_pos: MousePos): - if not self._enabled: - return - - self._clicked = True - self._state = not self._state - self._target = 1.0 if self._state else 0.0 - if self._callback: - self._callback(self._state) - - def get_state(self) -> bool: - return self._state - - def set_state(self, state: bool): - self._state = state - self._target = 1.0 if state else 0.0 - - def is_enabled(self): - return self._enabled - - def update(self): - if abs(self._progress - self._target) > 0.01: - delta = rl.get_frame_time() * ANIMATION_SPEED - self._progress += delta if self._progress < self._target else -delta - self._progress = max(0.0, min(1.0, self._progress)) - - def _render(self, rect: rl.Rectangle): - self.update() - - if self._enabled: - bg_color = self._blend_color(OFF_COLOR, ON_COLOR, self._progress) - knob_color = KNOB_COLOR - else: - bg_color = self._blend_color(DISABLED_OFF_COLOR, DISABLED_ON_COLOR, self._progress) - knob_color = DISABLED_KNOB_COLOR - - # Draw background - bg_rect = rl.Rectangle(self._rect.x + 5, self._rect.y + 10, WIDTH - 10, BG_HEIGHT) - rl.draw_rectangle_rounded(bg_rect, 1.0, 10, bg_color) - - # Draw knob - knob_x = self._rect.x + HEIGHT / 2 + (WIDTH - HEIGHT) * self._progress - knob_y = self._rect.y + HEIGHT / 2 - rl.draw_circle(int(knob_x), int(knob_y), HEIGHT / 2, knob_color) - - # TODO: use click callback - clicked = self._clicked - self._clicked = False - return clicked - - def _blend_color(self, c1, c2, t): - return rl.Color(int(c1.r + (c2.r - c1.r) * t), int(c1.g + (c2.g - c1.g) * t), int(c1.b + (c2.b - c1.b) * t), 255) diff --git a/system/updated/casync/casync.py b/system/updated/casync/casync.py deleted file mode 100755 index 2bd46a1ffba45c..00000000000000 --- a/system/updated/casync/casync.py +++ /dev/null @@ -1,246 +0,0 @@ -#!/usr/bin/env python3 -import io -import lzma -import os -import pathlib -import struct -import sys -import time -from abc import ABC, abstractmethod -from collections import defaultdict, namedtuple -from collections.abc import Callable -from typing import IO - -import requests -from Crypto.Hash import SHA512 -from openpilot.system.updated.casync import tar -from openpilot.system.updated.casync.common import create_casync_tar_package - -CA_FORMAT_INDEX = 0x96824d9c7b129ff9 -CA_FORMAT_TABLE = 0xe75b9e112f17417d -CA_FORMAT_TABLE_TAIL_MARKER = 0xe75b9e112f17417 -FLAGS = 0xb000000000000000 - -CA_HEADER_LEN = 48 -CA_TABLE_HEADER_LEN = 16 -CA_TABLE_ENTRY_LEN = 40 -CA_TABLE_MIN_LEN = CA_TABLE_HEADER_LEN + CA_TABLE_ENTRY_LEN - -CHUNK_DOWNLOAD_TIMEOUT = 60 -CHUNK_DOWNLOAD_RETRIES = 3 - -CAIBX_DOWNLOAD_TIMEOUT = 120 - -Chunk = namedtuple('Chunk', ['sha', 'offset', 'length']) -ChunkDict = dict[bytes, Chunk] - - -class ChunkReader(ABC): - @abstractmethod - def read(self, chunk: Chunk) -> bytes: - ... - - -class BinaryChunkReader(ChunkReader): - """Reads chunks from a local file""" - def __init__(self, file_like: IO[bytes]) -> None: - super().__init__() - self.f = file_like - - def read(self, chunk: Chunk) -> bytes: - self.f.seek(chunk.offset) - return self.f.read(chunk.length) - - -class FileChunkReader(BinaryChunkReader): - def __init__(self, path: str) -> None: - super().__init__(open(path, 'rb')) - - def __del__(self): - self.f.close() - - -class RemoteChunkReader(ChunkReader): - """Reads lzma compressed chunks from a remote store""" - - def __init__(self, url: str) -> None: - super().__init__() - self.url = url - self.session = requests.Session() - - def read(self, chunk: Chunk) -> bytes: - sha_hex = chunk.sha.hex() - url = os.path.join(self.url, sha_hex[:4], sha_hex + ".cacnk") - - if os.path.isfile(url): - with open(url, 'rb') as f: - contents = f.read() - else: - for i in range(CHUNK_DOWNLOAD_RETRIES): - try: - resp = self.session.get(url, timeout=CHUNK_DOWNLOAD_TIMEOUT) - break - except Exception: - if i == CHUNK_DOWNLOAD_RETRIES - 1: - raise - time.sleep(CHUNK_DOWNLOAD_TIMEOUT) - - resp.raise_for_status() - contents = resp.content - - decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO) - return decompressor.decompress(contents) - - -class DirectoryTarChunkReader(BinaryChunkReader): - """creates a tar archive of a directory and reads chunks from it""" - - def __init__(self, path: str, cache_file: str) -> None: - create_casync_tar_package(pathlib.Path(path), pathlib.Path(cache_file)) - - self.f = open(cache_file, "rb") - super().__init__(self.f) - - def __del__(self): - self.f.close() - os.unlink(self.f.name) - - -def parse_caibx(caibx_path: str) -> list[Chunk]: - """Parses the chunks from a caibx file. Can handle both local and remote files. - Returns a list of chunks with hash, offset and length""" - caibx: io.BufferedIOBase - if os.path.isfile(caibx_path): - caibx = open(caibx_path, 'rb') - else: - resp = requests.get(caibx_path, timeout=CAIBX_DOWNLOAD_TIMEOUT) - resp.raise_for_status() - caibx = io.BytesIO(resp.content) - - caibx.seek(0, os.SEEK_END) - caibx_len = caibx.tell() - caibx.seek(0, os.SEEK_SET) - - # Parse header - length, magic, flags, min_size, _, max_size = struct.unpack("= min_size - - chunks.append(Chunk(sha, offset, length)) - offset = new_offset - - caibx.close() - return chunks - - -def build_chunk_dict(chunks: list[Chunk]) -> ChunkDict: - """Turn a list of chunks into a dict for faster lookups based on hash. - Keep first chunk since it's more likely to be already downloaded.""" - r = {} - for c in chunks: - if c.sha not in r: - r[c.sha] = c - return r - - -def extract(target: list[Chunk], - sources: list[tuple[str, ChunkReader, ChunkDict]], - out_path: str, - progress: Callable[[int], None] | None = None): - stats: dict[str, int] = defaultdict(int) - - mode = 'rb+' if os.path.exists(out_path) else 'wb' - with open(out_path, mode) as out: - for cur_chunk in target: - - # Find source for desired chunk - for name, chunk_reader, store_chunks in sources: - if cur_chunk.sha in store_chunks: - bts = chunk_reader.read(store_chunks[cur_chunk.sha]) - - # Check length - if len(bts) != cur_chunk.length: - continue - - # Check hash - if SHA512.new(bts, truncate="256").digest() != cur_chunk.sha: - continue - - # Write to output - out.seek(cur_chunk.offset) - out.write(bts) - - stats[name] += cur_chunk.length - - if progress is not None: - progress(sum(stats.values())) - - break - else: - raise RuntimeError("Desired chunk not found in provided stores") - - return stats - - -def extract_directory(target: list[Chunk], - sources: list[tuple[str, ChunkReader, ChunkDict]], - out_path: str, - tmp_file: str, - progress: Callable[[int], None] | None = None): - """extract a directory stored as a casync tar archive""" - - stats = extract(target, sources, tmp_file, progress) - - with open(tmp_file, "rb") as f: - tar.extract_tar_archive(f, pathlib.Path(out_path)) - - return stats - - -def print_stats(stats: dict[str, int]): - total_bytes = sum(stats.values()) - print(f"Total size: {total_bytes / 1024 / 1024:.2f} MB") - for name, total in stats.items(): - print(f" {name}: {total / 1024 / 1024:.2f} MB ({total / total_bytes * 100:.1f}%)") - - -def extract_simple(caibx_path, out_path, store_path): - # (name, callback, chunks) - target = parse_caibx(caibx_path) - sources = [ - # (store_path, RemoteChunkReader(store_path), build_chunk_dict(target)), - (store_path, FileChunkReader(store_path), build_chunk_dict(target)), - ] - - return extract(target, sources, out_path) - - -if __name__ == "__main__": - caibx = sys.argv[1] - out = sys.argv[2] - store = sys.argv[3] - - stats = extract_simple(caibx, out, store) - print_stats(stats) diff --git a/system/updated/casync/common.py b/system/updated/casync/common.py deleted file mode 100644 index 6979f5cb06a329..00000000000000 --- a/system/updated/casync/common.py +++ /dev/null @@ -1,61 +0,0 @@ -import dataclasses -import json -import pathlib -import subprocess - -from openpilot.system.version import BUILD_METADATA_FILENAME, BuildMetadata -from openpilot.system.updated.casync import tar - - -CASYNC_ARGS = ["--with=symlinks", "--with=permissions", "--compression=xz", "--chunk-size=16M"] -CASYNC_FILES = [BUILD_METADATA_FILENAME] - - -def run(cmd): - return subprocess.check_output(cmd) - - -def get_exclude_set(path) -> set[str]: - exclude_set = set(CASYNC_FILES) - - for file in path.rglob("*"): - if file.is_file() or file.is_symlink(): - - while file.resolve() != path.resolve(): - exclude_set.add(str(file.relative_to(path))) - - file = file.parent - - return exclude_set - - -def create_build_metadata_file(path: pathlib.Path, build_metadata: BuildMetadata): - with open(path / BUILD_METADATA_FILENAME, "w") as f: - build_metadata_dict = dataclasses.asdict(build_metadata) - build_metadata_dict["openpilot"].pop("is_dirty") # this is determined at runtime - build_metadata_dict.pop("channel") # channel is unrelated to the build itself - f.write(json.dumps(build_metadata_dict)) - - -def is_not_git(path: pathlib.Path) -> bool: - return ".git" not in path.parts - - -def create_casync_tar_package(target_dir: pathlib.Path, output_path: pathlib.Path): - tar.create_tar_archive(output_path, target_dir, is_not_git) - - -def create_casync_from_file(file: pathlib.Path, output_dir: pathlib.Path, caibx_name: str): - caibx_file = output_dir / f"{caibx_name}.caibx" - run(["casync", "make", *CASYNC_ARGS, caibx_file, str(file)]) - - return caibx_file - - -def create_casync_release(target_dir: pathlib.Path, output_dir: pathlib.Path, caibx_name: str): - tar_file = output_dir / f"{caibx_name}.tar" - create_casync_tar_package(target_dir, tar_file) - caibx_file = create_casync_from_file(tar_file, output_dir, caibx_name) - tar_file.unlink() - digest = run(["casync", "digest", *CASYNC_ARGS, target_dir]).decode("utf-8").strip() - return digest, caibx_file diff --git a/system/updated/casync/tar.py b/system/updated/casync/tar.py deleted file mode 100644 index a5a8238bbadc8b..00000000000000 --- a/system/updated/casync/tar.py +++ /dev/null @@ -1,39 +0,0 @@ -import pathlib -import tarfile -from typing import IO -from collections.abc import Callable - - -def include_default(_) -> bool: - return True - - -def create_tar_archive(filename: pathlib.Path, directory: pathlib.Path, include: Callable[[pathlib.Path], bool] = include_default): - """Creates a tar archive of a directory""" - - with tarfile.open(filename, 'w') as tar: - for file in sorted(directory.rglob("*"), key=lambda f: f.stat().st_size if f.is_file() else 0, reverse=True): - if not include(file): - continue - relative_path = str(file.relative_to(directory)) - if file.is_symlink(): - info = tarfile.TarInfo(relative_path) - info.type = tarfile.SYMTYPE - info.linkpath = str(file.readlink()) - tar.addfile(info) - - elif file.is_file(): - info = tarfile.TarInfo(relative_path) - info.size = file.stat().st_size - info.type = tarfile.REGTYPE - info.mode = file.stat().st_mode - with file.open('rb') as f: - tar.addfile(info, f) - - -def extract_tar_archive(fh: IO[bytes], directory: pathlib.Path): - """Extracts a tar archive to a directory""" - - tar = tarfile.open(fileobj=fh, mode='r') - tar.extractall(str(directory), filter=lambda info, path: info) - tar.close() diff --git a/system/updated/casync/tests/test_casync.py b/system/updated/casync/tests/test_casync.py deleted file mode 100644 index bc171e7432b415..00000000000000 --- a/system/updated/casync/tests/test_casync.py +++ /dev/null @@ -1,264 +0,0 @@ -import pytest -import os -import pathlib -import tempfile -import subprocess - -from openpilot.system.updated.casync import casync -from openpilot.system.updated.casync import tar - -# dd if=/dev/zero of=/tmp/img.raw bs=1M count=2 -# sudo losetup -f /tmp/img.raw -# losetup -a | grep img.raw -LOOPBACK = os.environ.get('LOOPBACK', None) - - -@pytest.mark.skip("not used yet") -class TestCasync: - @classmethod - def setup_class(cls): - cls.tmpdir = tempfile.TemporaryDirectory() - - # Build example contents - chunk_a = [i % 256 for i in range(1024)] * 512 - chunk_b = [(256 - i) % 256 for i in range(1024)] * 512 - zeroes = [0] * (1024 * 128) - contents = chunk_a + chunk_b + zeroes + chunk_a - - cls.contents = bytes(contents) - - # Write to file - cls.orig_fn = os.path.join(cls.tmpdir.name, 'orig.bin') - with open(cls.orig_fn, 'wb') as f: - f.write(cls.contents) - - # Create casync files - cls.manifest_fn = os.path.join(cls.tmpdir.name, 'orig.caibx') - cls.store_fn = os.path.join(cls.tmpdir.name, 'store') - subprocess.check_output(["casync", "make", "--compression=xz", "--store", cls.store_fn, cls.manifest_fn, cls.orig_fn]) - - target = casync.parse_caibx(cls.manifest_fn) - hashes = [c.sha.hex() for c in target] - - # Ensure we have chunk reuse - assert len(hashes) > len(set(hashes)) - - def setup_method(self): - # Clear target_lo - if LOOPBACK is not None: - self.target_lo = LOOPBACK - with open(self.target_lo, 'wb') as f: - f.write(b"0" * len(self.contents)) - - self.target_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) - self.seed_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) - - def teardown_method(self): - for fn in [self.target_fn, self.seed_fn]: - try: - os.unlink(fn) - except FileNotFoundError: - pass - - def test_simple_extract(self): - target = casync.parse_caibx(self.manifest_fn) - - sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - stats = casync.extract(target, sources, self.target_fn) - - with open(self.target_fn, 'rb') as target_f: - assert target_f.read() == self.contents - - assert stats['remote'] == len(self.contents) - - def test_seed(self): - target = casync.parse_caibx(self.manifest_fn) - - # Populate seed with half of the target contents - with open(self.seed_fn, 'wb') as seed_f: - seed_f.write(self.contents[:len(self.contents) // 2]) - - sources = [('seed', casync.FileChunkReader(self.seed_fn), casync.build_chunk_dict(target))] - sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - stats = casync.extract(target, sources, self.target_fn) - - with open(self.target_fn, 'rb') as target_f: - assert target_f.read() == self.contents - - assert stats['seed'] > 0 - assert stats['remote'] < len(self.contents) - - def test_already_done(self): - """Test that an already flashed target doesn't download any chunks""" - target = casync.parse_caibx(self.manifest_fn) - - with open(self.target_fn, 'wb') as f: - f.write(self.contents) - - sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] - sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - - stats = casync.extract(target, sources, self.target_fn) - - with open(self.target_fn, 'rb') as f: - assert f.read() == self.contents - - assert stats['target'] == len(self.contents) - - def test_chunk_reuse(self): - """Test that chunks that are reused are only downloaded once""" - target = casync.parse_caibx(self.manifest_fn) - - # Ensure target exists - with open(self.target_fn, 'wb'): - pass - - sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] - sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - - stats = casync.extract(target, sources, self.target_fn) - - with open(self.target_fn, 'rb') as f: - assert f.read() == self.contents - - assert stats['remote'] < len(self.contents) - - @pytest.mark.skipif(not LOOPBACK, reason="requires loopback device") - def test_lo_simple_extract(self): - target = casync.parse_caibx(self.manifest_fn) - sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - - stats = casync.extract(target, sources, self.target_lo) - - with open(self.target_lo, 'rb') as target_f: - assert target_f.read(len(self.contents)) == self.contents - - assert stats['remote'] == len(self.contents) - - @pytest.mark.skipif(not LOOPBACK, reason="requires loopback device") - def test_lo_chunk_reuse(self): - """Test that chunks that are reused are only downloaded once""" - target = casync.parse_caibx(self.manifest_fn) - - sources = [('target', casync.FileChunkReader(self.target_lo), casync.build_chunk_dict(target))] - sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - - stats = casync.extract(target, sources, self.target_lo) - - with open(self.target_lo, 'rb') as f: - assert f.read(len(self.contents)) == self.contents - - assert stats['remote'] < len(self.contents) - - -@pytest.mark.skip("not used yet") -class TestCasyncDirectory: - """Tests extracting a directory stored as a casync tar archive""" - - NUM_FILES = 16 - - @classmethod - def setup_cache(cls, directory, files=None): - if files is None: - files = range(cls.NUM_FILES) - - chunk_a = [i % 256 for i in range(1024)] * 512 - chunk_b = [(256 - i) % 256 for i in range(1024)] * 512 - zeroes = [0] * (1024 * 128) - cls.contents = chunk_a + chunk_b + zeroes + chunk_a - cls.contents = bytes(cls.contents) - - for i in files: - with open(os.path.join(directory, f"file_{i}.txt"), "wb") as f: - f.write(cls.contents) - - os.symlink(f"file_{i}.txt", os.path.join(directory, f"link_{i}.txt")) - - @classmethod - def setup_class(cls): - cls.tmpdir = tempfile.TemporaryDirectory() - - # Create casync files - cls.manifest_fn = os.path.join(cls.tmpdir.name, 'orig.caibx') - cls.store_fn = os.path.join(cls.tmpdir.name, 'store') - - cls.directory_to_extract = tempfile.TemporaryDirectory() - cls.setup_cache(cls.directory_to_extract.name) - - cls.orig_fn = os.path.join(cls.tmpdir.name, 'orig.tar') - tar.create_tar_archive(cls.orig_fn, pathlib.Path(cls.directory_to_extract.name)) - - subprocess.check_output(["casync", "make", "--compression=xz", "--store", cls.store_fn, cls.manifest_fn, cls.orig_fn]) - - @classmethod - def teardown_class(cls): - cls.tmpdir.cleanup() - cls.directory_to_extract.cleanup() - - def setup_method(self): - self.cache_dir = tempfile.TemporaryDirectory() - self.working_dir = tempfile.TemporaryDirectory() - self.out_dir = tempfile.TemporaryDirectory() - - def teardown_method(self): - self.cache_dir.cleanup() - self.working_dir.cleanup() - self.out_dir.cleanup() - - def run_test(self): - target = casync.parse_caibx(self.manifest_fn) - - cache_filename = os.path.join(self.working_dir.name, "cache.tar") - tmp_filename = os.path.join(self.working_dir.name, "tmp.tar") - - sources = [('cache', casync.DirectoryTarChunkReader(self.cache_dir.name, cache_filename), casync.build_chunk_dict(target))] - sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] - - stats = casync.extract_directory(target, sources, pathlib.Path(self.out_dir.name), tmp_filename) - - with open(os.path.join(self.out_dir.name, "file_0.txt"), "rb") as f: - assert f.read() == self.contents - - with open(os.path.join(self.out_dir.name, "link_0.txt"), "rb") as f: - assert f.read() == self.contents - assert os.readlink(os.path.join(self.out_dir.name, "link_0.txt")) == "file_0.txt" - - return stats - - def test_no_cache(self): - self.setup_cache(self.cache_dir.name, []) - stats = self.run_test() - assert stats['remote'] > 0 - assert stats['cache'] == 0 - - def test_full_cache(self): - self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) - stats = self.run_test() - assert stats['remote'] == 0 - assert stats['cache'] > 0 - - def test_one_file_cache(self): - self.setup_cache(self.cache_dir.name, range(1)) - stats = self.run_test() - assert stats['remote'] > 0 - assert stats['cache'] > 0 - assert stats['cache'] < stats['remote'] - - def test_one_file_incorrect_cache(self): - self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) - with open(os.path.join(self.cache_dir.name, "file_0.txt"), "wb") as f: - f.write(b"1234") - - stats = self.run_test() - assert stats['remote'] > 0 - assert stats['cache'] > 0 - assert stats['cache'] > stats['remote'] - - def test_one_file_missing_cache(self): - self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) - os.unlink(os.path.join(self.cache_dir.name, "file_12.txt")) - - stats = self.run_test() - assert stats['remote'] > 0 - assert stats['cache'] > 0 - assert stats['cache'] > stats['remote'] diff --git a/system/updated/common.py b/system/updated/common.py deleted file mode 100644 index 6bb745f6b099d3..00000000000000 --- a/system/updated/common.py +++ /dev/null @@ -1,16 +0,0 @@ -import os -import pathlib - - -def get_consistent_flag(path: str) -> bool: - consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) - return consistent_file.is_file() - -def set_consistent_flag(path: str, consistent: bool) -> None: - os.sync() - consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) - if consistent: - consistent_file.touch() - elif not consistent: - consistent_file.unlink(missing_ok=True) - os.sync() diff --git a/system/updated/tests/test_base.py b/system/updated/tests/test_base.py deleted file mode 100644 index c4894f271134fd..00000000000000 --- a/system/updated/tests/test_base.py +++ /dev/null @@ -1,259 +0,0 @@ -import os -import pathlib -import shutil -import signal -import stat -import subprocess -import tempfile -import time -import pytest - -from openpilot.common.params import Params -from openpilot.system.manager.process import ManagerProcess -from openpilot.selfdrive.test.helpers import processes_context - - -def get_consistent_flag(path: str) -> bool: - consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) - return consistent_file.is_file() - - -def run(args, **kwargs): - return subprocess.check_output(args, **kwargs) - - -def update_release(directory, name, version, agnos_version, release_notes): - with open(directory / "RELEASES.md", "w") as f: - f.write(release_notes) - - (directory / "common").mkdir(exist_ok=True) - - with open(directory / "common" / "version.h", "w") as f: - f.write(f'#define COMMA_VERSION "{version}"') - - launch_env = directory / "launch_env.sh" - with open(launch_env, "w") as f: - f.write(f'export AGNOS_VERSION="{agnos_version}"') - - st = os.stat(launch_env) - os.chmod(launch_env, st.st_mode | stat.S_IEXEC) - - test_symlink = directory / "test_symlink" - if not os.path.exists(str(test_symlink)): - os.symlink("common/version.h", test_symlink) - - -def get_version(path: str) -> str: - with open(os.path.join(path, "common", "version.h")) as f: - return f.read().split('"')[1] - - -@pytest.mark.slow # TODO: can we test overlayfs in GHA? -class TestBaseUpdate: - @classmethod - def setup_class(cls): - if "Base" in cls.__name__: - pytest.skip() - - def setup_method(self): - self.tmpdir = tempfile.mkdtemp() - - run(["sudo", "mount", "-t", "tmpfs", "tmpfs", self.tmpdir]) # overlayfs doesn't work inside of docker unless this is a tmpfs - - self.mock_update_path = pathlib.Path(self.tmpdir) - - self.params = Params() - - self.basedir = self.mock_update_path / "openpilot" - self.basedir.mkdir() - - self.staging_root = self.mock_update_path / "safe_staging" - self.staging_root.mkdir() - - self.remote_dir = self.mock_update_path / "remote" - self.remote_dir.mkdir() - - os.environ["UPDATER_STAGING_ROOT"] = str(self.staging_root) - os.environ["UPDATER_LOCK_FILE"] = str(self.mock_update_path / "safe_staging_overlay.lock") - - self.MOCK_RELEASES = { - "release3": ("0.1.2", "1.2", "0.1.2 release notes"), - "master": ("0.1.3", "1.2", "0.1.3 release notes"), - } - - @pytest.fixture(autouse=True) - def mock_basedir(self, mocker): - mocker.patch("openpilot.common.basedir.BASEDIR", self.basedir) - - def set_target_branch(self, branch): - self.params.put("UpdaterTargetBranch", branch) - - def setup_basedir_release(self, release): - self.params = Params() - self.set_target_branch(release) - - def update_remote_release(self, release): - raise NotImplementedError("") - - def setup_remote_release(self, release): - raise NotImplementedError("") - - def additional_context(self): - raise NotImplementedError("") - - def teardown_method(self): - try: - run(["sudo", "umount", "-l", str(self.staging_root / "merged")]) - run(["sudo", "umount", "-l", self.tmpdir]) - shutil.rmtree(self.tmpdir) - except Exception: - print("cleanup failed...") - - def wait_for_condition(self, condition, timeout=12): - start = time.monotonic() - while True: - waited = time.monotonic() - start - if condition(): - print(f"waited {waited}s for condition ") - return waited - - if waited > timeout: - raise TimeoutError("timed out waiting for condition") - - time.sleep(1) - - def _test_finalized_update(self, branch, version, agnos_version, release_notes): - assert get_version(str(self.staging_root / "finalized")) == version - assert get_consistent_flag(str(self.staging_root / "finalized")) - assert os.access(str(self.staging_root / "finalized" / "launch_env.sh"), os.X_OK) - - with open(self.staging_root / "finalized" / "test_symlink") as f: - assert version in f.read() - -class ParamsBaseUpdateTest(TestBaseUpdate): - def _test_finalized_update(self, branch, version, agnos_version, release_notes): - assert self.params.get("UpdaterNewDescription").startswith(f"{version} / {branch}") - assert self.params.get("UpdaterNewReleaseNotes") == f"{release_notes}\n".encode() - super()._test_finalized_update(branch, version, agnos_version, release_notes) - - def send_check_for_updates_signal(self, updated: ManagerProcess): - updated.signal(signal.SIGUSR1.value) - - def send_download_signal(self, updated: ManagerProcess): - updated.signal(signal.SIGHUP.value) - - def _test_params(self, branch, fetch_available, update_available): - assert self.params.get("UpdaterTargetBranch") == branch - assert self.params.get_bool("UpdaterFetchAvailable") == fetch_available - assert self.params.get_bool("UpdateAvailable") == update_available - - def wait_for_idle(self): - self.wait_for_condition(lambda: self.params.get("UpdaterState") == "idle") - - def wait_for_failed(self): - self.wait_for_condition(lambda: self.params.get("UpdateFailedCount") is not None and \ - self.params.get("UpdateFailedCount") > 0) - - def wait_for_fetch_available(self): - self.wait_for_condition(lambda: self.params.get_bool("UpdaterFetchAvailable")) - - def wait_for_update_available(self): - self.wait_for_condition(lambda: self.params.get_bool("UpdateAvailable")) - - def test_no_update(self): - # Start on release3, ensure we don't fetch any updates - self.setup_remote_release("release3") - self.setup_basedir_release("release3") - - with self.additional_context(), processes_context(["updated"]) as [updated]: - self._test_params("release3", False, False) - self.wait_for_idle() - self._test_params("release3", False, False) - - self.send_check_for_updates_signal(updated) - - self.wait_for_idle() - - self._test_params("release3", False, False) - - def test_new_release(self): - # Start on release3, simulate a release3 commit, ensure we fetch that update properly - self.setup_remote_release("release3") - self.setup_basedir_release("release3") - - with self.additional_context(), processes_context(["updated"]) as [updated]: - self._test_params("release3", False, False) - self.wait_for_idle() - self._test_params("release3", False, False) - - self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes") - self.update_remote_release("release3") - - self.send_check_for_updates_signal(updated) - - self.wait_for_fetch_available() - - self._test_params("release3", True, False) - - self.send_download_signal(updated) - - self.wait_for_update_available() - - self._test_params("release3", False, True) - self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) - - def test_switch_branches(self): - # Start on release3, request to switch to master manually, ensure we switched - self.setup_remote_release("release3") - self.setup_remote_release("master") - self.setup_basedir_release("release3") - - with self.additional_context(), processes_context(["updated"]) as [updated]: - self._test_params("release3", False, False) - self.wait_for_idle() - self._test_params("release3", False, False) - - self.set_target_branch("master") - self.send_check_for_updates_signal(updated) - - self.wait_for_fetch_available() - - self._test_params("master", True, False) - - self.send_download_signal(updated) - - self.wait_for_update_available() - - self._test_params("master", False, True) - self._test_finalized_update("master", *self.MOCK_RELEASES["master"]) - - def test_agnos_update(self, mocker): - # Start on release3, push an update with an agnos change - self.setup_remote_release("release3") - self.setup_basedir_release("release3") - - with self.additional_context(), processes_context(["updated"]) as [updated]: - mocker.patch("openpilot.system.hardware.AGNOS", "True") - mocker.patch("openpilot.system.hardware.tici.hardware.Tici.get_os_version", "1.2") - mocker.patch("openpilot.system.hardware.tici.agnos.get_target_slot_number") - mocker.patch("openpilot.system.hardware.tici.agnos.flash_agnos_update") - - self._test_params("release3", False, False) - self.wait_for_idle() - self._test_params("release3", False, False) - - self.MOCK_RELEASES["release3"] = ("0.1.3", "1.3", "0.1.3 release notes") - self.update_remote_release("release3") - - self.send_check_for_updates_signal(updated) - - self.wait_for_fetch_available() - - self._test_params("release3", True, False) - - self.send_download_signal(updated) - - self.wait_for_update_available() - - self._test_params("release3", False, True) - self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) diff --git a/system/updated/tests/test_git.py b/system/updated/tests/test_git.py deleted file mode 100644 index 5a5a27000b0c18..00000000000000 --- a/system/updated/tests/test_git.py +++ /dev/null @@ -1,22 +0,0 @@ -import contextlib -from openpilot.system.updated.tests.test_base import ParamsBaseUpdateTest, run, update_release - - -class TestUpdateDGitStrategy(ParamsBaseUpdateTest): - def update_remote_release(self, release): - update_release(self.remote_dir, release, *self.MOCK_RELEASES[release]) - run(["git", "add", "."], cwd=self.remote_dir) - run(["git", "commit", "-m", f"openpilot release {release}"], cwd=self.remote_dir) - - def setup_remote_release(self, release): - run(["git", "init"], cwd=self.remote_dir) - run(["git", "checkout", "-b", release], cwd=self.remote_dir) - self.update_remote_release(release) - - def setup_basedir_release(self, release): - super().setup_basedir_release(release) - run(["git", "clone", "-b", release, self.remote_dir, self.basedir]) - - @contextlib.contextmanager - def additional_context(self): - yield diff --git a/system/updated/updated.py b/system/updated/updated.py deleted file mode 100755 index ffd10e038db79d..00000000000000 --- a/system/updated/updated.py +++ /dev/null @@ -1,514 +0,0 @@ -#!/usr/bin/env python3 -import os -import re -import datetime -import subprocess -import psutil -import shutil -import signal -import fcntl -import time -import threading -from collections import defaultdict -from pathlib import Path - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.common.time_helpers import system_time_valid -from openpilot.common.markdown import parse_markdown -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert -from openpilot.system.hardware import AGNOS, HARDWARE -from openpilot.system.version import get_build_metadata - -LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") -STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") - -OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper") -OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata") -OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged") -FINALIZED = os.path.join(STAGING_ROOT, "finalized") - -OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init")) - -# do not allow to engage after this many hours onroad and this many routes -HOURS_NO_CONNECTIVITY_MAX = 27 -ROUTES_NO_CONNECTIVITY_MAX = 84 -# send an offroad prompt after this many hours onroad and this many routes -HOURS_NO_CONNECTIVITY_PROMPT = 23 -ROUTES_NO_CONNECTIVITY_PROMPT = 80 - - -class UserRequest: - NONE = 0 - CHECK = 1 - FETCH = 2 - -class WaitTimeHelper: - def __init__(self): - self.ready_event = threading.Event() - self.user_request = UserRequest.NONE - signal.signal(signal.SIGHUP, self.update_now) - signal.signal(signal.SIGUSR1, self.check_now) - - def update_now(self, signum: int, frame) -> None: - cloudlog.info("caught SIGHUP, attempting to downloading update") - self.user_request = UserRequest.FETCH - self.ready_event.set() - - def check_now(self, signum: int, frame) -> None: - cloudlog.info("caught SIGUSR1, checking for updates") - self.user_request = UserRequest.CHECK - self.ready_event.set() - - def sleep(self, t: float) -> None: - self.ready_event.wait(timeout=t) - -def write_time_to_param(params, param) -> None: - t = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - params.put(param, t) - -def run(cmd: list[str], cwd: str | None = None) -> str: - return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') - - -def set_consistent_flag(consistent: bool) -> None: - os.sync() - consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent")) - if consistent: - consistent_file.touch() - elif not consistent: - consistent_file.unlink(missing_ok=True) - os.sync() - -def parse_release_notes(basedir: str) -> bytes: - try: - with open(os.path.join(basedir, "RELEASES.md"), "rb") as f: - r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes - try: - return bytes(parse_markdown(r.decode("utf-8")), encoding="utf-8") - except Exception: - return r + b"\n" - except FileNotFoundError: - pass - except Exception: - cloudlog.exception("failed to parse release notes") - return b"" - -def setup_git_options(cwd: str) -> None: - # We sync FS object atimes (which NEOS doesn't use) and mtimes, but ctimes - # are outside user control. Make sure Git is set up to ignore system ctimes, - # because they change when we make hard links during finalize. Otherwise, - # there is a lot of unnecessary churn. This appears to be a common need on - # OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/ - - # We are using copytree to copy the directory, which also changes - # inode numbers. Ignore those changes too. - - # Set protocol to the new version (default after git 2.26) to reduce data - # usage on git fetch --dry-run from about 400KB to 18KB. - git_cfg = [ - ("core.trustctime", "false"), - ("core.checkStat", "minimal"), - ("protocol.version", "2"), - ("gc.auto", "0"), - ("gc.autoDetach", "false"), - ] - for option, value in git_cfg: - run(["git", "config", option, value], cwd) - - -def dismount_overlay() -> None: - if os.path.ismount(OVERLAY_MERGED): - cloudlog.info("unmounting existing overlay") - run(["sudo", "umount", "-l", OVERLAY_MERGED]) - - -def init_overlay() -> None: - - # Re-create the overlay if BASEDIR/.git has changed since we created the overlay - if OVERLAY_INIT.is_file() and os.path.ismount(OVERLAY_MERGED): - git_dir_path = os.path.join(BASEDIR, ".git") - new_files = run(["find", git_dir_path, "-newer", str(OVERLAY_INIT)]) - if not len(new_files.splitlines()): - # A valid overlay already exists - return - else: - cloudlog.info(".git directory changed, recreating overlay") - - cloudlog.info("preparing new safe staging area") - - params = Params() - params.put_bool("UpdateAvailable", False) - set_consistent_flag(False) - dismount_overlay() - run(["sudo", "rm", "-rf", STAGING_ROOT]) - if os.path.isdir(STAGING_ROOT): - shutil.rmtree(STAGING_ROOT) - - for dirname in [STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED]: - os.mkdir(dirname, 0o755) - - if os.lstat(BASEDIR).st_dev != os.lstat(OVERLAY_MERGED).st_dev: - raise RuntimeError("base and overlay merge directories are on different filesystems; not valid for overlay FS!") - - # Leave a timestamped canary in BASEDIR to check at startup. The device clock - # should be correct by the time we get here. If the init file disappears, or - # critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can - # assume that BASEDIR has used for local development or otherwise modified, - # and skips the update activation attempt. - consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent")) - if consistent_file.is_file(): - consistent_file.unlink() - OVERLAY_INIT.touch() - - os.sync() - overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}" - - mount_cmd = ["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED] - run(["sudo"] + mount_cmd) - run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")]) - - git_diff = run(["git", "diff", "--submodule=diff"], OVERLAY_MERGED) - params.put("GitDiff", git_diff) - cloudlog.info(f"git diff output:\n{git_diff}") - - -def finalize_update() -> None: - """Take the current OverlayFS merged view and finalize a copy outside of - OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree""" - - # Remove the update ready flag and any old updates - cloudlog.info("creating finalized version of the overlay") - set_consistent_flag(False) - - # Copy the merged overlay view and set the update ready flag - if os.path.exists(FINALIZED): - shutil.rmtree(FINALIZED) - shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True) - - run(["git", "reset", "--hard"], FINALIZED) - run(["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"], FINALIZED) - - cloudlog.info("Starting git cleanup in finalized update") - t = time.monotonic() - try: - run(["git", "gc"], FINALIZED) - run(["git", "lfs", "prune"], FINALIZED) - cloudlog.event("Done git cleanup", duration=time.monotonic() - t) - except subprocess.CalledProcessError: - cloudlog.exception(f"Failed git cleanup, took {time.monotonic() - t:.3f} s") - - set_consistent_flag(True) - cloudlog.info("done finalizing overlay") - - -def handle_agnos_update() -> None: - from openpilot.system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number - - cur_version = HARDWARE.get_os_version() - updated_version = run(["bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \ - echo -n $AGNOS_VERSION"], OVERLAY_MERGED).strip() - - cloudlog.info(f"AGNOS version check: {cur_version} vs {updated_version}") - if cur_version == updated_version: - return - - # prevent an openpilot getting swapped in with a mismatched or partially downloaded agnos - set_consistent_flag(False) - - cloudlog.info(f"Beginning background installation for AGNOS {updated_version}") - set_offroad_alert("Offroad_NeosUpdate", True) - - manifest_path = os.path.join(OVERLAY_MERGED, "system/hardware/tici/agnos.json") - target_slot_number = get_target_slot_number() - flash_agnos_update(manifest_path, target_slot_number, cloudlog) - set_offroad_alert("Offroad_NeosUpdate", False) - - - -class Updater: - def __init__(self): - self.params = Params() - self.branches = defaultdict(str) - self._has_internet: bool = False - - @property - def has_internet(self) -> bool: - return self._has_internet - - @property - def target_branch(self) -> str: - b: str | None = self.params.get("UpdaterTargetBranch") - if b is None: - b = self.get_branch(BASEDIR) - b = { - ("tizi", "release3"): "release-tizi", - }.get((HARDWARE.get_device_type(), b), b) - return b - - @property - def update_ready(self) -> bool: - consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent")) - if consistent_file.is_file(): - hash_mismatch = self.get_commit_hash(BASEDIR) != self.branches[self.target_branch] - branch_mismatch = self.get_branch(BASEDIR) != self.target_branch - on_target_branch = self.get_branch(FINALIZED) == self.target_branch - return ((hash_mismatch or branch_mismatch) and on_target_branch) - return False - - @property - def update_available(self) -> bool: - if os.path.isdir(OVERLAY_MERGED) and len(self.branches) > 0: - hash_mismatch = self.get_commit_hash(OVERLAY_MERGED) != self.branches[self.target_branch] - branch_mismatch = self.get_branch(OVERLAY_MERGED) != self.target_branch - return hash_mismatch or branch_mismatch - return False - - def get_branch(self, path: str) -> str: - return run(["git", "rev-parse", "--abbrev-ref", "HEAD"], path).rstrip() - - def get_commit_hash(self, path: str = OVERLAY_MERGED) -> str: - return run(["git", "rev-parse", "HEAD"], path).rstrip() - - def set_params(self, update_success: bool, failed_count: int, exception: str | None) -> None: - self.params.put("UpdateFailedCount", failed_count) - self.params.put("UpdaterTargetBranch", self.target_branch) - - self.params.put_bool("UpdaterFetchAvailable", self.update_available) - if len(self.branches): - self.params.put("UpdaterAvailableBranches", ','.join(self.branches.keys())) - - last_uptime_onroad = self.params.get("UptimeOnroad", return_default=True) - last_route_count = self.params.get("RouteCount", return_default=True) - if update_success: - self.params.put("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None)) - self.params.put("LastUpdateUptimeOnroad", last_uptime_onroad) - self.params.put("LastUpdateRouteCount", last_route_count) - else: - last_uptime_onroad = self.params.get("LastUpdateUptimeOnroad", return_default=True) - last_route_count = self.params.get("LastUpdateRouteCount", return_default=True) - - if exception is None: - self.params.remove("LastUpdateException") - else: - self.params.put("LastUpdateException", exception) - - # Write out current and new version info - def get_description(basedir: str) -> str: - if not os.path.exists(basedir): - return "" - - version = "" - branch = "" - commit = "" - commit_date = "" - try: - branch = self.get_branch(basedir) - commit = self.get_commit_hash(basedir)[:7] - with open(os.path.join(basedir, "common", "version.h")) as f: - version = f.read().split('"')[1] - - commit_unix_ts = run(["git", "show", "-s", "--format=%ct", "HEAD"], basedir).rstrip() - dt = datetime.datetime.fromtimestamp(int(commit_unix_ts)) - commit_date = dt.strftime("%b %d") - except Exception: - cloudlog.exception("updater.get_description") - return f"{version} / {branch} / {commit} / {commit_date}" - self.params.put("UpdaterCurrentDescription", get_description(BASEDIR)) - self.params.put("UpdaterCurrentReleaseNotes", parse_release_notes(BASEDIR)) - self.params.put("UpdaterNewDescription", get_description(FINALIZED)) - self.params.put("UpdaterNewReleaseNotes", parse_release_notes(FINALIZED)) - self.params.put_bool("UpdateAvailable", self.update_ready) - - # Handle user prompt - for alert in ("Offroad_UpdateFailed", "Offroad_ConnectivityNeeded", "Offroad_ConnectivityNeededPrompt"): - set_offroad_alert(alert, False) - - dt_uptime_onroad = (self.params.get("UptimeOnroad", return_default=True) - last_uptime_onroad) / (60*60) - dt_route_count = self.params.get("RouteCount", return_default=True) - last_route_count - build_metadata = get_build_metadata() - if failed_count > 15 and exception is not None and self.has_internet: - if build_metadata.tested_channel: - extra_text = "Ensure the software is correctly installed. Uninstall and re-install if this error persists." - else: - extra_text = exception - set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) - elif failed_count > 0: - if dt_uptime_onroad > HOURS_NO_CONNECTIVITY_MAX and dt_route_count > ROUTES_NO_CONNECTIVITY_MAX: - set_offroad_alert("Offroad_ConnectivityNeeded", True) - elif dt_uptime_onroad > HOURS_NO_CONNECTIVITY_PROMPT and dt_route_count > ROUTES_NO_CONNECTIVITY_PROMPT: - remaining = max(HOURS_NO_CONNECTIVITY_MAX - dt_uptime_onroad, 1) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} hour{'' if remaining == 1 else 's'}.") - - def check_for_update(self) -> None: - cloudlog.info("checking for updates") - - excluded_branches = ('release2', 'release2-staging') - - try: - run(["git", "ls-remote", "origin", "HEAD"], OVERLAY_MERGED) - self._has_internet = True - except subprocess.CalledProcessError: - self._has_internet = False - - setup_git_options(OVERLAY_MERGED) - output = run(["git", "ls-remote", "--heads"], OVERLAY_MERGED) - - self.branches = defaultdict(lambda: None) - for line in output.split('\n'): - ls_remotes_re = r'(?P\b[0-9a-f]{5,40}\b)(\s+)(refs\/heads\/)(?P.*$)' - x = re.fullmatch(ls_remotes_re, line.strip()) - if x is not None and x.group('branch_name') not in excluded_branches: - self.branches[x.group('branch_name')] = x.group('commit_sha') - - cur_branch = self.get_branch(OVERLAY_MERGED) - cur_commit = self.get_commit_hash(OVERLAY_MERGED) - new_branch = self.target_branch - new_commit = self.branches[new_branch] - if (cur_branch, cur_commit) != (new_branch, new_commit): - cloudlog.info(f"update available, {cur_branch} ({str(cur_commit)[:7]}) -> {new_branch} ({str(new_commit)[:7]})") - else: - cloudlog.info(f"up to date on {cur_branch} ({str(cur_commit)[:7]})") - - def fetch_update(self) -> None: - cloudlog.info("attempting git fetch inside staging overlay") - - self.params.put("UpdaterState", "downloading...") - - # TODO: cleanly interrupt this and invalidate old update - set_consistent_flag(False) - self.params.put_bool("UpdateAvailable", False) - - setup_git_options(OVERLAY_MERGED) - - run(["git", "config", "--replace-all", "remote.origin.fetch", "+refs/heads/*:refs/remotes/origin/*"], OVERLAY_MERGED) - - branch = self.target_branch - git_fetch_output = run(["git", "fetch", "origin", branch], OVERLAY_MERGED) - cloudlog.info("git fetch success: %s", git_fetch_output) - - cloudlog.info("git reset in progress") - cmds = [ - ["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"], - ["git", "branch", "--set-upstream-to", f"origin/{branch}"], - ["git", "reset", "--hard"], - ["git", "clean", "-xdff"], - ["git", "submodule", "sync"], - ["git", "submodule", "update", "--init", "--recursive"], - ["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"], - ] - r = [run(cmd, OVERLAY_MERGED) for cmd in cmds] - cloudlog.info("git reset success: %s", '\n'.join(r)) - - # TODO: show agnos download progress - if AGNOS: - handle_agnos_update() - - # Create the finalized, ready-to-swap update - self.params.put("UpdaterState", "finalizing update...") - finalize_update() - cloudlog.info("finalize success!") - - -def main() -> None: - params = Params() - - if params.get_bool("DisableUpdates"): - cloudlog.warning("updates are disabled by the DisableUpdates param") - exit(0) - - with open(LOCK_FILE, 'w') as ov_lock_fd: - try: - fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB) - except OSError as e: - raise RuntimeError("couldn't get overlay lock; is another instance running?") from e - - # Set low io priority - proc = psutil.Process() - if psutil.LINUX: - proc.ionice(psutil.IOPRIO_CLASS_BE, value=7) - - # Check if we just performed an update - if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir(): - cloudlog.event("update installed") - - if not params.get("InstallDate"): - t = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - params.put("InstallDate", t) - - updater = Updater() - update_failed_count = 0 # TODO: Load from param? - wait_helper = WaitTimeHelper() - - # invalidate old finalized update - set_consistent_flag(False) - - # set initial state - params.put("UpdaterState", "idle") - - # Run the update loop - first_run = True - while True: - wait_helper.ready_event.clear() - - # Attempt an update - exception = None - try: - # TODO: reuse overlay from previous updated instance if it looks clean - init_overlay() - - # ensure we have some params written soon after startup - updater.set_params(False, update_failed_count, exception) - - if not system_time_valid() or first_run: - first_run = False - wait_helper.sleep(60) - continue - - update_failed_count += 1 - - # check for update - params.put("UpdaterState", "checking...") - updater.check_for_update() - - # download update - last_fetch = params.get("UpdaterLastFetchTime") - timed_out = last_fetch is None or (datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - last_fetch > datetime.timedelta(days=3)) - user_requested_fetch = wait_helper.user_request == UserRequest.FETCH - if params.get_bool("NetworkMetered") and not timed_out and not user_requested_fetch: - cloudlog.info("skipping fetch, connection metered") - elif wait_helper.user_request == UserRequest.CHECK: - cloudlog.info("skipping fetch, only checking") - else: - updater.fetch_update() - write_time_to_param(params, "UpdaterLastFetchTime") - update_failed_count = 0 - except subprocess.CalledProcessError as e: - cloudlog.event( - "update process failed", - cmd=e.cmd, - output=e.output, - returncode=e.returncode - ) - exception = f"command failed: {e.cmd}\n{e.output}" - OVERLAY_INIT.unlink(missing_ok=True) - except Exception as e: - cloudlog.exception("uncaught updated exception, shouldn't happen") - exception = str(e) - OVERLAY_INIT.unlink(missing_ok=True) - - try: - params.put("UpdaterState", "idle") - update_successful = (update_failed_count == 0) - updater.set_params(update_successful, update_failed_count, exception) - except Exception: - cloudlog.exception("uncaught updated exception while setting params, shouldn't happen") - - # infrequent attempts if we successfully updated recently - wait_helper.user_request = UserRequest.NONE - wait_helper.sleep(5*60 if update_failed_count > 0 else 1.5*60*60) - - -if __name__ == "__main__": - main() diff --git a/system/version.py b/system/version.py old mode 100755 new mode 100644 index 0cea616d236f7f..f0817b3a9ff8dc --- a/system/version.py +++ b/system/version.py @@ -1,59 +1,120 @@ #!/usr/bin/env python3 -from dataclasses import dataclass -from functools import cache -import json import os -import pathlib import subprocess +from typing import List, Optional +from functools import lru_cache -from openpilot.common.basedir import BASEDIR -from openpilot.common.swaglog import cloudlog -from openpilot.common.git import get_commit, get_origin, get_branch, get_short_branch, get_commit_date +from common.basedir import BASEDIR +from system.swaglog import cloudlog -RELEASE_BRANCHES = ['release-tizi-staging', 'release-mici-staging', 'release-tizi', 'release-mici', 'nightly'] -TESTED_BRANCHES = RELEASE_BRANCHES + ['devel-staging', 'nightly-dev'] +TESTED_BRANCHES = ['devel', 'release3-staging', 'dashcam3-staging', 'release3', 'dashcam3'] -BUILD_METADATA_FILENAME = "build.json" +training_version: bytes = b"0.2.0" +terms_version: bytes = b"2" -training_version: str = "0.2.0" -terms_version: str = "2" +def cache(user_function, /): + return lru_cache(maxsize=None)(user_function) -def get_version(path: str = BASEDIR) -> str: - with open(os.path.join(path, "common", "version.h")) as _versionf: + +def run_cmd(cmd: List[str]) -> str: + return subprocess.check_output(cmd, encoding='utf8').strip() + + +def run_cmd_default(cmd: List[str], default: Optional[str] = None) -> Optional[str]: + try: + return run_cmd(cmd) + except subprocess.CalledProcessError: + return default + + +@cache +def get_commit(branch: str = "HEAD", default: Optional[str] = None) -> Optional[str]: + return run_cmd_default(["git", "rev-parse", branch], default=default) + + +@cache +def get_short_branch(default: Optional[str] = None) -> Optional[str]: + return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], default=default) + + +@cache +def get_branch(default: Optional[str] = None) -> Optional[str]: + return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], default=default) + + +@cache +def get_origin(default: Optional[str] = None) -> Optional[str]: + try: + local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"]) + tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"]) + return run_cmd(["git", "config", "remote." + tracking_remote + ".url"]) + except subprocess.CalledProcessError: # Not on a branch, fallback + return run_cmd_default(["git", "config", "--get", "remote.origin.url"], default=default) + + +@cache +def get_normalized_origin(default: Optional[str] = None) -> Optional[str]: + origin: Optional[str] = get_origin() + + if origin is None: + return default + + return origin.replace("git@", "", 1) \ + .replace(".git", "", 1) \ + .replace("https://", "", 1) \ + .replace(":", "/", 1) + + +@cache +def get_version() -> str: + with open(os.path.join(BASEDIR, "common", "version.h")) as _versionf: version = _versionf.read().split('"')[1] return version +@cache +def get_short_version() -> str: + return get_version().split('-')[0] # type: ignore + +@cache +def is_prebuilt() -> bool: + return os.path.exists(os.path.join(BASEDIR, 'prebuilt')) + -def get_release_notes(path: str = BASEDIR) -> str: - with open(os.path.join(path, "RELEASES.md")) as f: - return f.read().split('\n\n', 1)[0] +@cache +def is_comma_remote() -> bool: + # note to fork maintainers, this is used for release metrics. please do not + # touch this to get rid of the orange startup alert. there's better ways to do that + origin: Optional[str] = get_origin() + if origin is None: + return False + + return origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai') @cache -def is_prebuilt(path: str = BASEDIR) -> bool: - return os.path.exists(os.path.join(path, 'prebuilt')) +def is_tested_branch() -> bool: + return get_short_branch() in TESTED_BRANCHES @cache -def is_dirty(cwd: str = BASEDIR) -> bool: - if not get_origin() or not get_short_branch(): +def is_dirty() -> bool: + origin = get_origin() + branch = get_branch() + if (origin is None) or (branch is None): return True dirty = False try: # Actually check dirty files - if not is_prebuilt(cwd): + if not is_prebuilt(): # This is needed otherwise touched files might show up as modified try: - subprocess.check_call(["git", "update-index", "--refresh"], cwd=cwd) + subprocess.check_call(["git", "update-index", "--refresh"]) except subprocess.CalledProcessError: pass - branch = get_branch() - if not branch: - return True - dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"], cwd=cwd)) != 0 + dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0) except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") dirty = True @@ -61,100 +122,18 @@ def is_dirty(cwd: str = BASEDIR) -> bool: return dirty -@dataclass -class OpenpilotMetadata: - version: str - release_notes: str - git_commit: str - git_origin: str - git_commit_date: str - build_style: str - is_dirty: bool # whether there are local changes - - @property - def short_version(self) -> str: - return self.version.split('-')[0] - - @property - def comma_remote(self) -> bool: - # note to fork maintainers, this is used for release metrics. please do not - # touch this to get rid of the orange startup alert. there's better ways to do that - return self.git_normalized_origin == "github.com/commaai/openpilot" - - @property - def git_normalized_origin(self) -> str: - return self.git_origin \ - .replace("git@", "", 1) \ - .replace(".git", "", 1) \ - .replace("https://", "", 1) \ - .replace(":", "/", 1) - - -@dataclass -class BuildMetadata: - channel: str - openpilot: OpenpilotMetadata - - @property - def tested_channel(self) -> bool: - return self.channel in TESTED_BRANCHES - - @property - def release_channel(self) -> bool: - return self.channel in RELEASE_BRANCHES - - @property - def canonical(self) -> str: - return f"{self.openpilot.version}-{self.openpilot.git_commit}-{self.openpilot.build_style}" - - @property - def ui_description(self) -> str: - return f"{self.openpilot.version} / {self.openpilot.git_commit[:6]} / {self.channel}" - - -def build_metadata_from_dict(build_metadata: dict) -> BuildMetadata: - channel = build_metadata.get("channel", "unknown") - openpilot_metadata = build_metadata.get("openpilot", {}) - version = openpilot_metadata.get("version", "unknown") - release_notes = openpilot_metadata.get("release_notes", "unknown") - git_commit = openpilot_metadata.get("git_commit", "unknown") - git_origin = openpilot_metadata.get("git_origin", "unknown") - git_commit_date = openpilot_metadata.get("git_commit_date", "unknown") - build_style = openpilot_metadata.get("build_style", "unknown") - return BuildMetadata(channel, - OpenpilotMetadata( - version=version, - release_notes=release_notes, - git_commit=git_commit, - git_origin=git_origin, - git_commit_date=git_commit_date, - build_style=build_style, - is_dirty=False)) - - -def get_build_metadata(path: str = BASEDIR) -> BuildMetadata: - build_metadata_path = pathlib.Path(path) / BUILD_METADATA_FILENAME - - if build_metadata_path.exists(): - build_metadata = json.loads(build_metadata_path.read_text()) - return build_metadata_from_dict(build_metadata) - - git_folder = pathlib.Path(path) / ".git" - - if git_folder.exists(): - return BuildMetadata(get_short_branch(path), - OpenpilotMetadata( - version=get_version(path), - release_notes=get_release_notes(path), - git_commit=get_commit(path), - git_origin=get_origin(path), - git_commit_date=get_commit_date(path), - build_style="unknown", - is_dirty=is_dirty(path))) - - cloudlog.exception("unable to get build metadata") - raise Exception("invalid build metadata") - - if __name__ == "__main__": - print(get_build_metadata()) + from common.params import Params + + params = Params() + params.put("TermsVersion", terms_version) + params.put("TrainingVersion", training_version) + + print(f"Dirty: {is_dirty()}") + print(f"Version: {get_version()}") + print(f"Short version: {get_short_version()}") + print(f"Origin: {get_origin()}") + print(f"Normalized origin: {get_normalized_origin()}") + print(f"Branch: {get_branch()}") + print(f"Short branch: {get_short_branch()}") + print(f"Prebuilt: {is_prebuilt()}") diff --git a/system/webrtc/device/audio.py b/system/webrtc/device/audio.py deleted file mode 100644 index b1859518a17b37..00000000000000 --- a/system/webrtc/device/audio.py +++ /dev/null @@ -1,109 +0,0 @@ -import asyncio -import io - -import aiortc -import av -import numpy as np -import pyaudio - - -class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack): - PYAUDIO_TO_AV_FORMAT_MAP = { - pyaudio.paUInt8: 'u8', - pyaudio.paInt16: 's16', - pyaudio.paInt24: 's24', - pyaudio.paInt32: 's32', - pyaudio.paFloat32: 'flt', - } - - def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: int | None = None): - super().__init__() - - self.p = pyaudio.PyAudio() - chunk_size = int(packet_time * rate) - self.stream = self.p.open(format=audio_format, - channels=channels, - rate=rate, - frames_per_buffer=chunk_size, - input=True, - input_device_index=device_index) - self.format = audio_format - self.rate = rate - self.channels = channels - self.packet_time = packet_time - self.chunk_size = chunk_size - self.pts = 0 - - async def recv(self): - mic_data = self.stream.read(self.chunk_size) - mic_array = np.frombuffer(mic_data, dtype=np.int16) - mic_array = np.expand_dims(mic_array, axis=0) - layout = 'stereo' if self.channels > 1 else 'mono' - frame = av.AudioFrame.from_ndarray(mic_array, format=self.PYAUDIO_TO_AV_FORMAT_MAP[self.format], layout=layout) - frame.rate = self.rate - frame.pts = self.pts - self.pts += frame.samples - - return frame - - -class AudioOutputSpeaker: - def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: int | None = None): - - chunk_size = int(packet_time * rate) - self.p = pyaudio.PyAudio() - self.buffer = io.BytesIO() - self.channels = channels - self.stream = self.p.open(format=audio_format, - channels=channels, - rate=rate, - frames_per_buffer=chunk_size, - output=True, - output_device_index=device_index, - stream_callback=self.__pyaudio_callback) - self.tracks_and_tasks: list[tuple[aiortc.MediaStreamTrack, asyncio.Task | None]] = [] - - def __pyaudio_callback(self, in_data, frame_count, time_info, status): - if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2: - buff = b'\x00\x00' * frame_count * self.channels - elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size - self.buffer.seek(0) - buff = self.buffer.read(frame_count * self.channels * 4) - buff = buff[:frame_count * self.channels * 2] - self.buffer.seek(2) - else: - self.buffer.seek(0) - buff = self.buffer.read(frame_count * self.channels * 2) - self.buffer.seek(2) - return (buff, pyaudio.paContinue) - - async def __consume(self, track): - while True: - try: - frame = await track.recv() - except aiortc.MediaStreamError: - return - - self.buffer.write(bytes(frame.planes[0])) - - def hasTrack(self, track: aiortc.MediaStreamTrack) -> bool: - return any(t == track for t, _ in self.tracks_and_tasks) - - def addTrack(self, track: aiortc.MediaStreamTrack): - if not self.hasTrack(track): - self.tracks_and_tasks.append((track, None)) - - def start(self): - for index, (track, task) in enumerate(self.tracks_and_tasks): - if task is None: - self.tracks_and_tasks[index] = (track, asyncio.create_task(self.__consume(track))) - - def stop(self): - for _, task in self.tracks_and_tasks: - if task is not None: - task.cancel() - - self.tracks_and_tasks = [] - self.stream.stop_stream() - self.stream.close() - self.p.terminate() diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py deleted file mode 100644 index 50feab4f4a910d..00000000000000 --- a/system/webrtc/device/video.py +++ /dev/null @@ -1,45 +0,0 @@ -import asyncio -import time - -import av -from teleoprtc.tracks import TiciVideoStreamTrack - -from cereal import messaging -from openpilot.common.realtime import DT_MDL, DT_DMON - - -class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): - camera_to_sock_mapping = { - "driver": "livestreamDriverEncodeData", - "wideRoad": "livestreamWideRoadEncodeData", - "road": "livestreamRoadEncodeData", - } - - def __init__(self, camera_type: str): - dt = DT_DMON if camera_type == "driver" else DT_MDL - super().__init__(camera_type, dt) - - self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) - self._pts = 0 - self._t0_ns = time.monotonic_ns() - - async def recv(self): - while True: - msg = messaging.recv_one_or_none(self._sock) - if msg is not None: - break - await asyncio.sleep(0.005) - - evta = getattr(msg, msg.which()) - - packet = av.Packet(evta.header + evta.data) - packet.time_base = self._time_base - - self._pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000 - packet.pts = self._pts - self.log_debug("track sending frame %d", self._pts) - - return packet - - def codec_preference(self) -> str | None: - return "H264" diff --git a/system/webrtc/schema.py b/system/webrtc/schema.py deleted file mode 100644 index d80986ebf2599e..00000000000000 --- a/system/webrtc/schema.py +++ /dev/null @@ -1,43 +0,0 @@ -import capnp -from typing import Any - - -def generate_type(type_walker, schema_walker) -> str | list[Any] | dict[str, Any]: - data_type = next(type_walker) - if data_type.which() == 'struct': - return generate_struct(next(schema_walker)) - elif data_type.which() == 'list': - _ = next(schema_walker) - return [generate_type(type_walker, schema_walker)] - elif data_type.which() == 'enum': - return "text" - else: - return str(data_type.which()) - - -def generate_struct(schema: capnp.lib.capnp._StructSchema) -> dict[str, Any]: - return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")} - - -def generate_field(field: capnp.lib.capnp._StructSchemaField) -> str | list[Any] | dict[str, Any]: - def schema_walker(field): - yield field.schema - - s = field.schema - while hasattr(s, 'elementType'): - s = s.elementType - yield s - - def type_walker(field): - yield field.proto.slot.type - - t = field.proto.slot.type - while hasattr(getattr(t, t.which()), 'elementType'): - t = getattr(t, t.which()).elementType - yield t - - if field.proto.which() == "slot": - schema_gen, type_gen = schema_walker(field), type_walker(field) - return generate_type(type_gen, schema_gen) - else: - return generate_struct(field.schema) diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py deleted file mode 100644 index e31fda37286a20..00000000000000 --- a/system/webrtc/tests/test_stream_session.py +++ /dev/null @@ -1,104 +0,0 @@ -import asyncio -import json -import time -# for aiortc and its dependencies -import warnings -warnings.filterwarnings("ignore", category=DeprecationWarning) -warnings.filterwarnings("ignore", category=RuntimeWarning) # TODO: remove this when google-crc32c publish a python3.12 wheel - -from aiortc import RTCDataChannel -from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE -import capnp -import pyaudio -from cereal import messaging, log - -from openpilot.system.webrtc.webrtcd import CerealOutgoingMessageProxy, CerealIncomingMessageProxy -from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack -from openpilot.system.webrtc.device.audio import AudioInputStreamTrack - - -class TestStreamSession: - def setup_method(self): - self.loop = asyncio.new_event_loop() - - def teardown_method(self): - self.loop.stop() - self.loop.close() - - def test_outgoing_proxy(self, mocker): - test_msg = log.Event.new_message() - test_msg.logMonoTime = 123 - test_msg.valid = True - test_msg.customReservedRawData0 = b"test" - expected_dict = {"type": "customReservedRawData0", "logMonoTime": 123, "valid": True, "data": "test"} - expected_json = json.dumps(expected_dict).encode() - - channel = mocker.Mock(spec=RTCDataChannel) - mocked_submaster = messaging.SubMaster(["customReservedRawData0"]) - def mocked_update(t): - mocked_submaster.update_msgs(0, [test_msg]) - - mocker.patch.object(messaging.SubMaster, "update", side_effect=mocked_update) - proxy = CerealOutgoingMessageProxy(mocked_submaster) - proxy.add_channel(channel) - - proxy.update() - - channel.send.assert_called_once_with(expected_json) - - def test_incoming_proxy(self, mocker): - tested_msgs = [ - {"type": "customReservedRawData0", "data": "test"}, # primitive - {"type": "can", "data": [{"address": 0, "dat": "", "src": 0}]}, # list - {"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict - ] - - mocked_pubmaster = mocker.MagicMock(spec=messaging.PubMaster) - - proxy = CerealIncomingMessageProxy(mocked_pubmaster) - - for msg in tested_msgs: - proxy.send(json.dumps(msg).encode()) - - mocked_pubmaster.send.assert_called_once() - mt, md = mocked_pubmaster.send.call_args.args - assert mt == msg["type"] - assert isinstance(md, capnp._DynamicStructBuilder) - assert hasattr(md, msg["type"]) - - mocked_pubmaster.reset_mock() - - def test_livestream_track(self, mocker): - fake_msg = messaging.new_message("livestreamDriverEncodeData") - - config = {"receive.return_value": fake_msg.to_bytes()} - mocker.patch("msgq.SubSocket", spec=True, **config) - track = LiveStreamVideoStreamTrack("driver") - - assert track.id.startswith("driver") - assert track.codec_preference() == "H264" - - for i in range(5): - packet = self.loop.run_until_complete(track.recv()) - assert packet.time_base == VIDEO_TIME_BASE - if i == 0: - start_ns = time.monotonic_ns() - start_pts = packet.pts - assert abs(i + packet.pts - (start_pts + (((time.monotonic_ns() - start_ns) * VIDEO_CLOCK_RATE) // 1_000_000_000))) < 450 #5ms - assert packet.size == 0 - - def test_input_audio_track(self, mocker): - packet_time, rate = 0.02, 16000 - sample_count = int(packet_time * rate) - mocked_stream = mocker.MagicMock(spec=pyaudio.Stream) - mocked_stream.read.return_value = b"\x00" * 2 * sample_count - - config = {"open.side_effect": lambda *args, **kwargs: mocked_stream} - mocker.patch("pyaudio.PyAudio", spec=True, **config) - track = AudioInputStreamTrack(audio_format=pyaudio.paInt16, packet_time=packet_time, rate=rate) - - for i in range(5): - frame = self.loop.run_until_complete(track.recv()) - assert frame.rate == rate - assert frame.samples == sample_count - assert frame.pts == i * sample_count diff --git a/system/webrtc/tests/test_webrtcd.py b/system/webrtc/tests/test_webrtcd.py deleted file mode 100644 index 4fa6d8953f7312..00000000000000 --- a/system/webrtc/tests/test_webrtcd.py +++ /dev/null @@ -1,65 +0,0 @@ -import pytest -import asyncio -import json -# for aiortc and its dependencies -import warnings -warnings.filterwarnings("ignore", category=DeprecationWarning) -warnings.filterwarnings("ignore", category=RuntimeWarning) # TODO: remove this when google-crc32c publish a python3.12 wheel - -from openpilot.system.webrtc.webrtcd import get_stream - -import aiortc -from teleoprtc import WebRTCOfferBuilder -from parameterized import parameterized_class - - -@parameterized_class(("in_services", "out_services"), [ - (["testJoystick"], ["carState"]), - ([], ["carState"]), - (["testJoystick"], []), - ([], []), -]) -@pytest.mark.asyncio -class TestWebrtcdProc: - async def assertCompletesWithTimeout(self, awaitable, timeout=1): - try: - async with asyncio.timeout(timeout): - await awaitable - except TimeoutError: - pytest.fail("Timeout while waiting for awaitable to complete") - - async def test_webrtcd(self, mocker): - mock_request = mocker.MagicMock() - async def connect(offer): - body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services} - mock_request.json.side_effect = mocker.AsyncMock(return_value=body) - response = await get_stream(mock_request) - response_json = json.loads(response.text) - return aiortc.RTCSessionDescription(**response_json) - - builder = WebRTCOfferBuilder(connect) - builder.offer_to_receive_video_stream("road") - builder.offer_to_receive_audio_stream() - if len(self.in_services) > 0 or len(self.out_services) > 0: - builder.add_messaging() - - stream = builder.stream() - - await self.assertCompletesWithTimeout(stream.start()) - await self.assertCompletesWithTimeout(stream.wait_for_connection()) - - assert stream.has_incoming_video_track("road") - assert stream.has_incoming_audio_track() - assert stream.has_messaging_channel() == (len(self.in_services) > 0 or len(self.out_services) > 0) - - video_track, audio_track = stream.get_incoming_video_track("road"), stream.get_incoming_audio_track() - await self.assertCompletesWithTimeout(video_track.recv()) - await self.assertCompletesWithTimeout(audio_track.recv()) - - await self.assertCompletesWithTimeout(stream.stop()) - - # cleanup, very implementation specific, test may break if it changes - assert mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test" - _, session = mock_request.app["streams"].__setitem__.call_args.args - await self.assertCompletesWithTimeout(session.post_run_cleanup()) - diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py deleted file mode 100755 index c19f1bf9dd6115..00000000000000 --- a/system/webrtc/webrtcd.py +++ /dev/null @@ -1,292 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -import asyncio -import json -import uuid -import logging -from dataclasses import dataclass, field -from typing import Any, TYPE_CHECKING - -# aiortc and its dependencies have lots of internal warnings :( -import warnings -warnings.filterwarnings("ignore", category=DeprecationWarning) -warnings.filterwarnings("ignore", category=RuntimeWarning) # TODO: remove this when google-crc32c publish a python3.12 wheel - -import capnp -from aiohttp import web -if TYPE_CHECKING: - from aiortc.rtcdatachannel import RTCDataChannel - -from openpilot.system.webrtc.schema import generate_field -from cereal import messaging, log - - -class CerealOutgoingMessageProxy: - def __init__(self, sm: messaging.SubMaster): - self.sm = sm - self.channels: list[RTCDataChannel] = [] - - def add_channel(self, channel: 'RTCDataChannel'): - self.channels.append(channel) - - def to_json(self, msg_content: Any): - if isinstance(msg_content, capnp._DynamicStructReader): - msg_dict = msg_content.to_dict() - elif isinstance(msg_content, capnp._DynamicListReader): - msg_dict = [self.to_json(msg) for msg in msg_content] - elif isinstance(msg_content, bytes): - msg_dict = msg_content.decode() - else: - msg_dict = msg_content - - return msg_dict - - def update(self): - # this is blocking in async context... - self.sm.update(0) - for service, updated in self.sm.updated.items(): - if not updated: - continue - msg_dict = self.to_json(self.sm[service]) - mono_time, valid = self.sm.logMonoTime[service], self.sm.valid[service] - outgoing_msg = {"type": service, "logMonoTime": mono_time, "valid": valid, "data": msg_dict} - encoded_msg = json.dumps(outgoing_msg).encode() - for channel in self.channels: - channel.send(encoded_msg) - - -class CerealIncomingMessageProxy: - def __init__(self, pm: messaging.PubMaster): - self.pm = pm - - def send(self, message: bytes): - msg_json = json.loads(message) - msg_type, msg_data = msg_json["type"], msg_json["data"] - size = None - if not isinstance(msg_data, dict): - size = len(msg_data) - - msg = messaging.new_message(msg_type, size=size) - setattr(msg, msg_type, msg_data) - self.pm.send(msg_type, msg) - - -class CerealProxyRunner: - def __init__(self, proxy: CerealOutgoingMessageProxy): - self.proxy = proxy - self.is_running = False - self.task = None - self.logger = logging.getLogger("webrtcd") - - def start(self): - assert self.task is None - self.task = asyncio.create_task(self.run()) - - def stop(self): - if self.task is None or self.task.done(): - return - self.task.cancel() - self.task = None - - async def run(self): - from aiortc.exceptions import InvalidStateError - - while True: - try: - self.proxy.update() - except InvalidStateError: - self.logger.warning("Cereal outgoing proxy invalid state (connection closed)") - break - except Exception: - self.logger.exception("Cereal outgoing proxy failure") - await asyncio.sleep(0.01) - - -class DynamicPubMaster(messaging.PubMaster): - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - self.lock = asyncio.Lock() - - async def add_services_if_needed(self, services): - async with self.lock: - for service in services: - if service not in self.sock: - self.sock[service] = messaging.pub_sock(service) - - -class StreamSession: - shared_pub_master = DynamicPubMaster([]) - - def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], debug_mode: bool = False): - from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack - from aiortc.contrib.media import MediaBlackhole - from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack - from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker - from teleoprtc import WebRTCAnswerBuilder - from teleoprtc.info import parse_info_from_offer - - config = parse_info_from_offer(sdp) - builder = WebRTCAnswerBuilder(sdp) - - assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks" - for cam in cameras: - builder.add_video_stream(cam, LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()) - if config.expected_audio_track: - builder.add_audio_stream(AudioInputStreamTrack() if not debug_mode else AudioStreamTrack()) - if config.incoming_audio_track: - self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole - builder.offer_to_receive_audio_stream() - - self.stream = builder.stream() - self.identifier = str(uuid.uuid4()) - - self.incoming_bridge: CerealIncomingMessageProxy | None = None - self.incoming_bridge_services = incoming_services - self.outgoing_bridge: CerealOutgoingMessageProxy | None = None - self.outgoing_bridge_runner: CerealProxyRunner | None = None - if len(incoming_services) > 0: - self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) - if len(outgoing_services) > 0: - self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) - self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) - - self.audio_output: AudioOutputSpeaker | MediaBlackhole | None = None - self.run_task: asyncio.Task | None = None - self.logger = logging.getLogger("webrtcd") - self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s", - self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services) - - def start(self): - self.run_task = asyncio.create_task(self.run()) - - def stop(self): - if self.run_task.done(): - return - self.run_task.cancel() - self.run_task = None - asyncio.run(self.post_run_cleanup()) - - async def get_answer(self): - return await self.stream.start() - - async def message_handler(self, message: bytes): - assert self.incoming_bridge is not None - try: - self.incoming_bridge.send(message) - except Exception: - self.logger.exception("Cereal incoming proxy failure") - - async def run(self): - try: - await self.stream.wait_for_connection() - if self.stream.has_messaging_channel(): - if self.incoming_bridge is not None: - await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services) - self.stream.set_message_handler(self.message_handler) - if self.outgoing_bridge_runner is not None: - channel = self.stream.get_messaging_channel() - self.outgoing_bridge_runner.proxy.add_channel(channel) - self.outgoing_bridge_runner.start() - if self.stream.has_incoming_audio_track(): - track = self.stream.get_incoming_audio_track(buffered=False) - self.audio_output = self.audio_output_cls() - self.audio_output.addTrack(track) - self.audio_output.start() - self.logger.info("Stream session (%s) connected", self.identifier) - - await self.stream.wait_for_disconnection() - await self.post_run_cleanup() - - self.logger.info("Stream session (%s) ended", self.identifier) - except Exception: - self.logger.exception("Stream session failure") - - async def post_run_cleanup(self): - await self.stream.stop() - if self.outgoing_bridge is not None: - self.outgoing_bridge_runner.stop() - if self.audio_output: - self.audio_output.stop() - - -@dataclass -class StreamRequestBody: - sdp: str - cameras: list[str] - bridge_services_in: list[str] = field(default_factory=list) - bridge_services_out: list[str] = field(default_factory=list) - - -async def get_stream(request: 'web.Request'): - stream_dict, debug_mode = request.app['streams'], request.app['debug'] - raw_body = await request.json() - body = StreamRequestBody(**raw_body) - - session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode) - answer = await session.get_answer() - session.start() - - stream_dict[session.identifier] = session - - return web.json_response({"sdp": answer.sdp, "type": answer.type}) - - -async def get_schema(request: 'web.Request'): - services = request.query["services"].split(",") - services = [s for s in services if s] - assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name" - schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} - return web.json_response(schema_dict) - -async def post_notify(request: 'web.Request'): - try: - payload = await request.json() - except Exception as e: - raise web.HTTPBadRequest(text="Invalid JSON") from e - - for session in list(request.app.get('streams', {}).values()): - try: - ch = session.stream.get_messaging_channel() - ch.send(json.dumps(payload)) - except Exception: - continue - - return web.Response(status=200, text="OK") - -async def on_shutdown(app: 'web.Application'): - for session in app['streams'].values(): - session.stop() - del app['streams'] - - -def webrtcd_thread(host: str, port: int, debug: bool): - logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()]) - logging_level = logging.DEBUG if debug else logging.INFO - logging.getLogger("WebRTCStream").setLevel(logging_level) - logging.getLogger("webrtcd").setLevel(logging_level) - - app = web.Application() - - app['streams'] = dict() - app['debug'] = debug - app.on_shutdown.append(on_shutdown) - app.router.add_post("/stream", get_stream) - app.router.add_post("/notify", post_notify) - app.router.add_get("/schema", get_schema) - - web.run_app(app, host=host, port=port) - - -def main(): - parser = argparse.ArgumentParser(description="WebRTC daemon") - parser.add_argument("--host", type=str, default="0.0.0.0", help="Host to listen on") - parser.add_argument("--port", type=int, default=5001, help="Port to listen on") - parser.add_argument("--debug", action="store_true", help="Enable debug mode") - args = parser.parse_args() - - webrtcd_thread(args.host, args.port, args.debug) - - -if __name__=="__main__": - main() diff --git a/teleoprtc b/teleoprtc deleted file mode 120000 index 3d3dbc8dea1ac6..00000000000000 --- a/teleoprtc +++ /dev/null @@ -1 +0,0 @@ -teleoprtc_repo/teleoprtc \ No newline at end of file diff --git a/teleoprtc_repo b/teleoprtc_repo deleted file mode 160000 index 389815b8ca5302..00000000000000 --- a/teleoprtc_repo +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 389815b8ca5302ce7c1504b7841d4eb61a8cd51b diff --git a/third_party/SConscript b/third_party/SConscript index 3a7497d1623207..e5bbfaa07a8d67 100644 --- a/third_party/SConscript +++ b/third_party/SConscript @@ -1,3 +1,6 @@ Import('env') env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call']) +env.Append(CPPPATH=[Dir('json11')]) + +env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/third_party/acados/.gitignore b/third_party/acados/.gitignore index 68858c62e4321e..9787bd1b55c25d 100644 --- a/third_party/acados/.gitignore +++ b/third_party/acados/.gitignore @@ -1,5 +1,4 @@ acados_repo/ -lib !x86_64/ !larch64/ !aarch64/ diff --git a/third_party/acados/Darwin/lib/libacados.dylib b/third_party/acados/Darwin/lib/libacados.dylib index 8cd5a41f14410b..6d133f693d706c 100755 Binary files a/third_party/acados/Darwin/lib/libacados.dylib and b/third_party/acados/Darwin/lib/libacados.dylib differ diff --git a/third_party/acados/Darwin/lib/libblasfeo.dylib b/third_party/acados/Darwin/lib/libblasfeo.dylib index 442a6c736d9ec0..7ba6a8b32d34a2 100755 Binary files a/third_party/acados/Darwin/lib/libblasfeo.dylib and b/third_party/acados/Darwin/lib/libblasfeo.dylib differ diff --git a/third_party/acados/Darwin/lib/libhpipm.dylib b/third_party/acados/Darwin/lib/libhpipm.dylib index 019ec8019ed70a..69141c81781c5d 100755 Binary files a/third_party/acados/Darwin/lib/libhpipm.dylib and b/third_party/acados/Darwin/lib/libhpipm.dylib differ diff --git a/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib b/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib index caaa8ec7756d90..b0cf93b060e11b 100755 Binary files a/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib and b/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib differ diff --git a/third_party/acados/Darwin/t_renderer b/third_party/acados/Darwin/t_renderer index 83a74ea5a30417..1afbd815199505 100755 Binary files a/third_party/acados/Darwin/t_renderer and b/third_party/acados/Darwin/t_renderer differ diff --git a/third_party/acados/aarch64 b/third_party/acados/aarch64 deleted file mode 120000 index 062c65e8d99c64..00000000000000 --- a/third_party/acados/aarch64 +++ /dev/null @@ -1 +0,0 @@ -larch64/ \ No newline at end of file diff --git a/third_party/acados/acados_template/__init__.py b/third_party/acados/acados_template/__init__.py deleted file mode 100644 index bfbe907990b8be..00000000000000 --- a/third_party/acados/acados_template/__init__.py +++ /dev/null @@ -1,40 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -from .acados_model import AcadosModel -from .acados_ocp import AcadosOcp, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpDims, AcadosOcpOptions -from .acados_sim import AcadosSim, AcadosSimDims, AcadosSimOpts -from .acados_ocp_solver import AcadosOcpSolver, get_simulink_default_opts, ocp_get_default_cmake_builder -from .acados_sim_solver import AcadosSimSolver, sim_get_default_cmake_builder -from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \ - get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \ - casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts - -from .zoro_description import ZoroDescription, process_zoro_description diff --git a/third_party/acados/acados_template/acados_model.py b/third_party/acados/acados_template/acados_model.py deleted file mode 100644 index b7c6945442c422..00000000000000 --- a/third_party/acados/acados_template/acados_model.py +++ /dev/null @@ -1,154 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - - -class AcadosModel(): - """ - Class containing all the information to code generate the external CasADi functions - that are needed when creating an acados ocp solver or acados integrator. - Thus, this class contains: - - a) the :py:attr:`name` of the model, - b) all CasADi variables/expressions needed in the CasADi function generation process. - """ - def __init__(self): - ## common for OCP and Integrator - self.name = None - """ - The model name is used for code generation. Type: string. Default: :code:`None` - """ - self.x = None #: CasADi variable describing the state of the system; Default: :code:`None` - self.xdot = None #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None` - self.u = None #: CasADi variable describing the input of the system; Default: :code:`None` - self.z = [] #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty` - self.p = [] #: CasADi variable describing parameters of the DAE; Default: :code:`empty` - # dynamics - self.f_impl_expr = None - """ - CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`. - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'. - Default: :code:`None` - """ - self.f_expl_expr = None - """ - CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`. - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'. - Default: :code:`None` - """ - self.disc_dyn_expr = None - """ - CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`. - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'. - Default: :code:`None` - """ - - self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi' - self.dyn_generic_source = None #: name of source file for discrete dyanamics; Default: :code:`None` - self.dyn_disc_fun_jac_hess = None #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None` - self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None` - self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None` - - # for GNSF models - self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0} - """ - dictionary containing information on GNSF structure needed when rendering templates. - Contains integers `nontrivial_f_LO`, `purely_linear`. - """ - - ## for OCP - # constraints - # BGH(default): lh <= h(x, u) <= uh - self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None` - # BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi - self.con_phi_expr = None #: CasADi expression for the constraint phi; Default: :code:`None` - self.con_r_expr = None #: CasADi expression for the constraint phi(r); Default: :code:`None` - self.con_r_in_phi = None - # terminal - self.con_h_expr_e = None #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None` - self.con_r_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` - self.con_phi_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None` - self.con_r_in_phi_e = None - # cost - self.cost_y_expr = None #: CasADi expression for nonlinear least squares; Default: :code:`None` - self.cost_y_expr_e = None #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None` - self.cost_y_expr_0 = None #: CasADi expression for nonlinear least squares, initial; Default: :code:`None` - self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None` - self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None` - self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None` - self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None` - self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None` - self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None` - - ## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p) - self.cost_psi_expr_0 = None - """ - CasADi expression for the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_psi_expr = None - """ - CasADi expression for the outer loss function :math:`\psi(r, p)`; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_psi_expr_e = None - """ - CasADi expression for the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_r_in_psi_expr_0 = None - """ - CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_r_in_psi_expr = None - """ - CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_r_in_psi_expr_e = None - """ - CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_conl_custom_outer_hess_0 = None - """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_conl_custom_outer_hess = None - """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'. - """ - self.cost_conl_custom_outer_hess_e = None - """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`None` - Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'. - """ diff --git a/third_party/acados/acados_template/acados_sim_solver.py b/third_party/acados/acados_template/acados_sim_solver.py deleted file mode 100644 index de5ee1070944f3..00000000000000 --- a/third_party/acados/acados_template/acados_sim_solver.py +++ /dev/null @@ -1,558 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import sys -import os -import json -import importlib - -import numpy as np - -from subprocess import DEVNULL, call, STDOUT - -from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_bool, byref -from copy import deepcopy - -from .casadi_function_generation import generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_explicit_ode -from .acados_sim import AcadosSim -from .acados_ocp import AcadosOcp -from .utils import is_column, render_template, format_class_dict, make_object_json_dumpable,\ - make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path, get_lib_ext,\ - casadi_length, is_empty, check_casadi_version -from .builders import CMakeBuilder -from .gnsf.detect_gnsf_structure import detect_gnsf_structure - - - -def make_sim_dims_consistent(acados_sim: AcadosSim): - dims = acados_sim.dims - model = acados_sim.model - # nx - if is_column(model.x): - dims.nx = casadi_length(model.x) - else: - raise Exception('model.x should be column vector!') - - # nu - if is_empty(model.u): - dims.nu = 0 - else: - dims.nu = casadi_length(model.u) - - # nz - if is_empty(model.z): - dims.nz = 0 - else: - dims.nz = casadi_length(model.z) - - # np - if is_empty(model.p): - dims.np = 0 - else: - dims.np = casadi_length(model.p) - if acados_sim.parameter_values.shape[0] != dims.np: - raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ - f'\nGot np = {dims.np}, acados_sim.parameter_values.shape = {acados_sim.parameter_values.shape[0]}\n') - - -def get_sim_layout(): - python_interface_path = get_python_interface_path() - abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json') - with open(abs_path, 'r') as f: - sim_layout = json.load(f) - return sim_layout - - -def sim_formulation_json_dump(acados_sim: AcadosSim, json_file='acados_sim.json'): - # Load acados_sim structure description - sim_layout = get_sim_layout() - - # Copy input sim object dictionary - sim_dict = dict(deepcopy(acados_sim).__dict__) - - for key, v in sim_layout.items(): - # skip non dict attributes - if not isinstance(v, dict): continue - # Copy sim object attributes dictionaries - sim_dict[key]=dict(getattr(acados_sim, key).__dict__) - - sim_json = format_class_dict(sim_dict) - - with open(json_file, 'w') as f: - json.dump(sim_json, f, default=make_object_json_dumpable, indent=4, sort_keys=True) - - -def sim_get_default_cmake_builder() -> CMakeBuilder: - """ - If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting. - :return: default :py:class:`~acados_template.builders.CMakeBuilder` - """ - cmake_builder = CMakeBuilder() - cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB'] - return cmake_builder - - -def sim_render_templates(json_file, model_name: str, code_export_dir, cmake_options: CMakeBuilder = None): - # setting up loader and environment - json_path = os.path.join(os.getcwd(), json_file) - - if not os.path.exists(json_path): - raise Exception(f"{json_path} not found!") - - # Render templates - in_file = 'acados_sim_solver.in.c' - out_file = f'acados_sim_solver_{model_name}.c' - render_template(in_file, out_file, code_export_dir, json_path) - - in_file = 'acados_sim_solver.in.h' - out_file = f'acados_sim_solver_{model_name}.h' - render_template(in_file, out_file, code_export_dir, json_path) - - in_file = 'acados_sim_solver.in.pxd' - out_file = f'acados_sim_solver.pxd' - render_template(in_file, out_file, code_export_dir, json_path) - - # Builder - if cmake_options is not None: - in_file = 'CMakeLists.in.txt' - out_file = 'CMakeLists.txt' - render_template(in_file, out_file, code_export_dir, json_path) - else: - in_file = 'Makefile.in' - out_file = 'Makefile' - render_template(in_file, out_file, code_export_dir, json_path) - - in_file = 'main_sim.in.c' - out_file = f'main_sim_{model_name}.c' - render_template(in_file, out_file, code_export_dir, json_path) - - # folder model - model_dir = os.path.join(code_export_dir, model_name + '_model') - - in_file = 'model.in.h' - out_file = f'{model_name}_model.h' - render_template(in_file, out_file, model_dir, json_path) - - -def sim_generate_external_functions(acados_sim: AcadosSim): - model = acados_sim.model - model = make_model_consistent(model) - - integrator_type = acados_sim.solver_options.integrator_type - - opts = dict(generate_hess = acados_sim.solver_options.sens_hess, - code_export_directory = acados_sim.code_export_directory) - - # create code_export_dir, model_dir - code_export_dir = acados_sim.code_export_directory - opts['code_export_directory'] = code_export_dir - model_dir = os.path.join(code_export_dir, model.name + '_model') - if not os.path.exists(model_dir): - os.makedirs(model_dir) - - # generate external functions - check_casadi_version() - if integrator_type == 'ERK': - generate_c_code_explicit_ode(model, opts) - elif integrator_type == 'IRK': - generate_c_code_implicit_ode(model, opts) - elif integrator_type == 'GNSF': - generate_c_code_gnsf(model, opts) - - -class AcadosSimSolver: - """ - Class to interact with the acados integrator C object. - - :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim` - :param json_file: Default: 'acados_sim.json' - :param build: Default: True - :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use - the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with - `MS Visual Studio`); default: `None` - """ - if sys.platform=="win32": - from ctypes import wintypes - from ctypes import WinDLL - dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary - dlclose.argtypes = [wintypes.HMODULE] - else: - dlclose = CDLL(None).dlclose - dlclose.argtypes = [c_void_p] - - - @classmethod - def generate(cls, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None): - """ - Generates the code for an acados sim solver, given the description in acados_sim - """ - - acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory) - - # make dims consistent - make_sim_dims_consistent(acados_sim) - - # module dependent post processing - if acados_sim.solver_options.integrator_type == 'GNSF': - if acados_sim.solver_options.sens_hess == True: - raise Exception("AcadosSimSolver: GNSF does not support sens_hess = True.") - if 'gnsf_model' in acados_sim.__dict__: - set_up_imported_gnsf_model(acados_sim) - else: - detect_gnsf_structure(acados_sim) - - # generate external functions - sim_generate_external_functions(acados_sim) - - # dump to json - sim_formulation_json_dump(acados_sim, json_file) - - # render templates - sim_render_templates(json_file, acados_sim.model.name, acados_sim.code_export_directory, cmake_builder) - - - @classmethod - def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): - # Compile solver - cwd = os.getcwd() - os.chdir(code_export_dir) - if with_cython: - call( - ['make', 'clean_sim_cython'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) - call( - ['make', 'sim_cython'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) - else: - if cmake_builder is not None: - cmake_builder.exec(code_export_dir, verbose=verbose) - else: - call( - ['make', 'sim_shared_lib'], - stdout=None if verbose else DEVNULL, - stderr=None if verbose else STDOUT - ) - os.chdir(cwd) - - - @classmethod - def create_cython_solver(cls, json_file): - """ - """ - with open(json_file, 'r') as f: - acados_sim_json = json.load(f) - code_export_directory = acados_sim_json['code_export_directory'] - - importlib.invalidate_caches() - rel_code_export_directory = os.path.relpath(code_export_directory) - acados_sim_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_sim_solver_pyx') - - AcadosSimSolverCython = getattr(acados_sim_solver_pyx, 'AcadosSimSolverCython') - return AcadosSimSolverCython(acados_sim_json['model']['name']) - - def __init__(self, acados_sim, json_file='acados_sim.json', generate=True, build=True, cmake_builder: CMakeBuilder = None, verbose: bool = True): - - self.solver_created = False - self.acados_sim = acados_sim - model_name = acados_sim.model.name - self.model_name = model_name - - code_export_dir = os.path.abspath(acados_sim.code_export_directory) - - # reuse existing json and casadi functions, when creating integrator from ocp - if generate and not isinstance(acados_sim, AcadosOcp): - self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder) - - if build: - self.build(code_export_dir, cmake_builder=cmake_builder, verbose=True) - - # prepare library loading - lib_prefix = 'lib' - lib_ext = get_lib_ext() - if os.name == 'nt': - lib_prefix = '' - - # Load acados library to avoid unloading the library. - # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed. - # Unloading a library which uses OpenMP results in a segfault (on any platform?). - # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp] - # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html] - libacados_name = f'{lib_prefix}acados{lib_ext}' - libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name) - self.__acados_lib = CDLL(libacados_filepath) - # find out if acados was compiled with OpenMP - try: - self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None - except AttributeError as e: - self.__acados_lib_uses_omp = False - if self.__acados_lib_uses_omp: - print('acados was compiled with OpenMP.') - else: - print('acados was compiled without OpenMP.') - libacados_sim_solver_name = f'{lib_prefix}acados_sim_solver_{self.model_name}{lib_ext}' - self.shared_lib_name = os.path.join(code_export_dir, libacados_sim_solver_name) - - # get shared_lib - self.shared_lib = CDLL(self.shared_lib_name) - - # create capsule - getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p - self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")() - - # create solver - getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int - assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0 - self.solver_created = True - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p - self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule) - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p - self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule) - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p - self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule) - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p - self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule) - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p - self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule) - - getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p - self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule) - - self.gettable_vectors = ['x', 'u', 'z', 'S_adj'] - self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic'] - self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] - - - def simulate(self, x=None, u=None, z=None, p=None): - """ - Simulate the system forward for the given x, u, z, p and return x_next. - Wrapper around `solve()` taking care of setting/getting inputs/outputs. - """ - if x is not None: - self.set('x', x) - if u is not None: - self.set('u', u) - if z is not None: - self.set('z', z) - if p is not None: - self.set('p', p) - - status = self.solve() - - if status == 2: - print("Warning: acados_sim_solver reached maximum iterations.") - elif status != 0: - raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.') - - x_next = self.get('x') - return x_next - - - def solve(self): - """ - Solve the simulation problem with current input. - """ - getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p] - getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int - - status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule) - return status - - - def get(self, field_): - """ - Get the last solution of the solver. - - :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] - """ - field = field_.encode('utf-8') - - if field_ in self.gettable_vectors: - # get dims - dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) - dims_data = cast(dims.ctypes.data, POINTER(c_int)) - - self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] - self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) - - # allocate array - out = np.ascontiguousarray(np.zeros((dims[0],)), dtype=np.float64) - out_data = cast(out.ctypes.data, POINTER(c_double)) - - self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] - self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data) - - elif field_ in self.gettable_matrices: - # get dims - dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) - dims_data = cast(dims.ctypes.data, POINTER(c_int)) - - self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] - self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) - - out = np.zeros((dims[0], dims[1]), order='F') - out_data = cast(out.ctypes.data, POINTER(c_double)) - - self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] - self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data) - - elif field_ in self.gettable_scalars: - scalar = c_double() - scalar_data = byref(scalar) - self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] - self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, scalar_data) - - out = scalar.value - else: - raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ - f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}') - - return out - - - - def set(self, field_: str, value_): - """ - Set numerical data inside the solver. - - :param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T'] - :param value: the value with appropriate size. - """ - settable = ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T'] # S_forw - - # TODO: check and throw error here. then remove checks in Cython for speed - # cast value_ to avoid conversion issues - if isinstance(value_, (float, int)): - value_ = np.array([value_]) - - value_ = value_.astype(float) - value_data = cast(value_.ctypes.data, POINTER(c_double)) - value_data_p = cast((value_data), c_void_p) - - field = field_.encode('utf-8') - - # treat parameters separately - if field_ == 'p': - model_name = self.acados_sim.model.name - getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int] - value_data = cast(value_.ctypes.data, POINTER(c_double)) - getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0]) - return - else: - # dimension check - dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) - dims_data = cast(dims.ctypes.data, POINTER(c_int)) - - self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)] - self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data) - - value_ = np.ravel(value_, order='F') - - value_shape = value_.shape - if len(value_shape) == 1: - value_shape = (value_shape[0], 0) - - if value_shape != tuple(dims): - raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \ - f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).') - - # set - if field_ in ['xdot', 'z']: - self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p] - self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p) - elif field_ in settable: - self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p] - self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p) - else: - raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \ - f' available fields are {", ".join(settable)}') - - return - - - def options_set(self, field_: str, value_: bool): - """ - Set solver options - - :param field: string in ['sens_forw', 'sens_adj', 'sens_hess'] - :param value: Boolean - """ - fields = ['sens_forw', 'sens_adj', 'sens_hess'] - if field_ not in fields: - raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") - - field = field_.encode('utf-8') - value_ctypes = c_bool(value_) - - if not isinstance(value_, bool): - raise TypeError("options_set: expected boolean for value") - - # only allow setting - if getattr(self.acados_sim.solver_options, field_) or value_ == False: - self.shared_lib.sim_opts_set.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_bool)] - self.shared_lib.sim_opts_set(self.sim_config, self.sim_opts, field, value_ctypes) - else: - raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n") - - return - - - def __del__(self): - - if self.solver_created: - getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p] - getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int - getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule) - - getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p] - getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int - getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule) - - try: - self.dlclose(self.shared_lib._handle) - except: - print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosSimSolver {self.model_name}.\n", - "Attempting to create a new one with the same name will likely result in the old one being used!") - pass diff --git a/third_party/acados/acados_template/acados_sim_solver_common.pxd b/third_party/acados/acados_template/acados_sim_solver_common.pxd deleted file mode 100644 index 7c20a6d24de137..00000000000000 --- a/third_party/acados/acados_template/acados_sim_solver_common.pxd +++ /dev/null @@ -1,63 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - - -cdef extern from "acados/sim/sim_common.h": - ctypedef struct sim_config: - pass - - ctypedef struct sim_opts: - pass - - ctypedef struct sim_in: - pass - - ctypedef struct sim_out: - pass - - -cdef extern from "acados_c/sim_interface.h": - - ctypedef struct sim_plan: - pass - - ctypedef struct sim_solver: - pass - - # out - void sim_out_get(sim_config *config, void *dims, sim_out *out, const char *field, void *value) - int sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, void *dims_data) - - # opts - void sim_opts_set(sim_config *config, void *opts_, const char *field, void *value) - - # get/set - void sim_in_set(sim_config *config, void *dims, sim_in *sim_in, const char *field, void *value) - void sim_solver_set(sim_solver *solver, const char *field, void *value) \ No newline at end of file diff --git a/third_party/acados/acados_template/acados_sim_solver_pyx.pyx b/third_party/acados/acados_template/acados_sim_solver_pyx.pyx deleted file mode 100644 index 01964fd7a0b9e3..00000000000000 --- a/third_party/acados/acados_template/acados_sim_solver_pyx.pyx +++ /dev/null @@ -1,255 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# cython: language_level=3 -# cython: profile=False -# distutils: language=c - -cimport cython -from libc cimport string -# from libc cimport bool as bool_t - -cimport acados_sim_solver_common -cimport acados_sim_solver - -cimport numpy as cnp - -import os -from datetime import datetime -import numpy as np - - -cdef class AcadosSimSolverCython: - """ - Class to interact with the acados sim solver C object. - """ - - cdef acados_sim_solver.sim_solver_capsule *capsule - cdef void *sim_dims - cdef acados_sim_solver_common.sim_opts *sim_opts - cdef acados_sim_solver_common.sim_config *sim_config - cdef acados_sim_solver_common.sim_out *sim_out - cdef acados_sim_solver_common.sim_in *sim_in - cdef acados_sim_solver_common.sim_solver *sim_solver - - cdef bint solver_created - - cdef str model_name - - cdef str sim_solver_type - - cdef list gettable_vectors - cdef list gettable_matrices - cdef list gettable_scalars - - def __cinit__(self, model_name): - - self.solver_created = False - - self.model_name = model_name - - # create capsule - self.capsule = acados_sim_solver.acados_sim_solver_create_capsule() - - # create solver - assert acados_sim_solver.acados_sim_create(self.capsule) == 0 - self.solver_created = True - - # get pointers solver - self.__get_pointers_solver() - - self.gettable_vectors = ['x', 'u', 'z', 'S_adj'] - self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic'] - self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] - - def __get_pointers_solver(self): - """ - Private function to get the pointers for solver - """ - # get pointers solver - self.sim_opts = acados_sim_solver.acados_get_sim_opts(self.capsule) - self.sim_dims = acados_sim_solver.acados_get_sim_dims(self.capsule) - self.sim_config = acados_sim_solver.acados_get_sim_config(self.capsule) - self.sim_out = acados_sim_solver.acados_get_sim_out(self.capsule) - self.sim_in = acados_sim_solver.acados_get_sim_in(self.capsule) - self.sim_solver = acados_sim_solver.acados_get_sim_solver(self.capsule) - - - def simulate(self, x=None, u=None, z=None, p=None): - """ - Simulate the system forward for the given x, u, z, p and return x_next. - Wrapper around `solve()` taking care of setting/getting inputs/outputs. - """ - if x is not None: - self.set('x', x) - if u is not None: - self.set('u', u) - if z is not None: - self.set('z', z) - if p is not None: - self.set('p', p) - - status = self.solve() - - if status == 2: - print("Warning: acados_sim_solver reached maximum iterations.") - elif status != 0: - raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.') - - x_next = self.get('x') - return x_next - - - def solve(self): - """ - Solve the sim with current input. - """ - return acados_sim_solver.acados_sim_solve(self.capsule) - - - def get(self, field_): - """ - Get the last solution of the solver. - - :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] - """ - field = field_.encode('utf-8') - - if field_ in self.gettable_vectors: - return self.__get_vector(field) - elif field_ in self.gettable_matrices: - return self.__get_matrix(field) - elif field_ in self.gettable_scalars: - return self.__get_scalar(field) - else: - raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ - f' available fields are {", ".join(self.gettable.keys())}') - - - def __get_scalar(self, field): - cdef double scalar - acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, &scalar) - return scalar - - - def __get_vector(self, field): - cdef int[2] dims - acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) - # cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64)) - cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),) - acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out.data) - return out - - - def __get_matrix(self, field): - cdef int[2] dims - acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) - cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64) - acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out.data) - return out - - - def set(self, field_: str, value_): - """ - Set numerical data inside the solver. - - :param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z'] - :param value: the value with appropriate size. - """ - settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw - - # cast value_ to avoid conversion issues - if isinstance(value_, (float, int)): - value_ = np.array([value_]) - # if len(value_.shape) > 1: - # raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional') - - cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten() - - field = field_.encode('utf-8') - cdef int[2] dims - - # treat parameters separately - if field_ == 'p': - assert acados_sim_solver.acados_sim_update_params(self.capsule, value.data, value.shape[0]) == 0 - return - else: - acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) - - value_ = np.ravel(value_, order='F') - - value_shape = value_.shape - if len(value_shape) == 1: - value_shape = (value_shape[0], 0) - - if value_shape != tuple(dims): - raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \ - f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).') - - # set - if field_ in ['xdot', 'z']: - acados_sim_solver_common.sim_solver_set(self.sim_solver, field, value.data) - elif field_ in settable: - acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value.data) - else: - raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \ - f' available fields are {", ".join(settable)}') - - - def options_set(self, field_: str, value_: bool): - """ - Set solver options - - :param field: string in ['sens_forw', 'sens_adj', 'sens_hess'] - :param value: Boolean - """ - fields = ['sens_forw', 'sens_adj', 'sens_hess'] - if field_ not in fields: - raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") - - field = field_.encode('utf-8') - - if not isinstance(value_, bool): - raise TypeError("options_set: expected boolean for value") - - cdef bint bool_value = value_ - acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, &bool_value) - # TODO: only allow setting - # if getattr(self.acados_sim.solver_options, field_) or value_ == False: - # acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, &bool_value) - # else: - # raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n") - - return - - - def __del__(self): - if self.solver_created: - acados_sim_solver.acados_sim_free(self.capsule) - acados_sim_solver.acados_sim_solver_free_capsule(self.capsule) diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd deleted file mode 100644 index 153f98d13a62e1..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd +++ /dev/null @@ -1,51 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -cimport acados_sim_solver_common - -cdef extern from "acados_sim_solver_{{ model.name }}.h": - ctypedef struct sim_solver_capsule "sim_solver_capsule": - pass - - sim_solver_capsule * acados_sim_solver_create_capsule "{{ model.name }}_acados_sim_solver_create_capsule"() - int acados_sim_solver_free_capsule "{{ model.name }}_acados_sim_solver_free_capsule"(sim_solver_capsule *capsule) - - int acados_sim_create "{{ model.name }}_acados_sim_create"(sim_solver_capsule * capsule) - int acados_sim_solve "{{ model.name }}_acados_sim_solve"(sim_solver_capsule * capsule) - int acados_sim_free "{{ model.name }}_acados_sim_free"(sim_solver_capsule * capsule) - int acados_sim_update_params "{{ model.name }}_acados_sim_update_params"(sim_solver_capsule * capsule, double *value, int np_) - # int acados_sim_update_params_sparse "{{ model.name }}_acados_sim_update_params_sparse"(sim_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) - - acados_sim_solver_common.sim_in *acados_get_sim_in "{{ model.name }}_acados_get_sim_in"(sim_solver_capsule * capsule) - acados_sim_solver_common.sim_out *acados_get_sim_out "{{ model.name }}_acados_get_sim_out"(sim_solver_capsule * capsule) - acados_sim_solver_common.sim_solver *acados_get_sim_solver "{{ model.name }}_acados_get_sim_solver"(sim_solver_capsule * capsule) - acados_sim_solver_common.sim_config *acados_get_sim_config "{{ model.name }}_acados_get_sim_config"(sim_solver_capsule * capsule) - acados_sim_solver_common.sim_opts *acados_get_sim_opts "{{ model.name }}_acados_get_sim_opts"(sim_solver_capsule * capsule) - void *acados_get_sim_dims "{{ model.name }}_acados_get_sim_dims"(sim_solver_capsule * capsule) diff --git a/third_party/acados/acados_template/c_templates_tera/constraints.in.h b/third_party/acados/acados_template/c_templates_tera/constraints.in.h deleted file mode 100644 index d71ce5cc221f1b..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/constraints.in.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef {{ model.name }}_CONSTRAINTS -#define {{ model.name }}_CONSTRAINTS - -#ifdef __cplusplus -extern "C" { -#endif - -{% if dims.nphi > 0 %} -int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); -const int *{{ model.name }}_phi_constraint_sparsity_in(int); -const int *{{ model.name }}_phi_constraint_sparsity_out(int); -int {{ model.name }}_phi_constraint_n_in(void); -int {{ model.name }}_phi_constraint_n_out(void); -{% endif %} - -{% if dims.nphi_e > 0 %} -int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); -const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); -const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); -int {{ model.name }}_phi_e_constraint_n_in(void); -int {{ model.name }}_phi_e_constraint_n_out(void); -{% endif %} - -{% if dims.nh > 0 %} -int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); - -int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_sparsity_out(int); -int {{ model.name }}_constr_h_fun_n_in(void); -int {{ model.name }}_constr_h_fun_n_out(void); - -{% if solver_options.hessian_approx == "EXACT" -%} -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); -{% endif %} -{% endif %} - -{% if dims.nh_e > 0 %} -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); -const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void); - -int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); -const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_n_in(void); -int {{ model.name }}_constr_h_e_fun_n_out(void); - -{% if solver_options.hessian_approx == "EXACT" -%} -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); -const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); -{% endif %} -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_CONSTRAINTS diff --git a/third_party/acados/acados_template/c_templates_tera/cost.in.h b/third_party/acados/acados_template/c_templates_tera/cost.in.h deleted file mode 100644 index 45eb09c12e2b3b..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/cost.in.h +++ /dev/null @@ -1,238 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_COST -#define {{ model.name }}_COST - -#ifdef __cplusplus -extern "C" { -#endif - - -// Cost at initial shooting node -{% if cost.cost_type_0 == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_n_in(void); -int {{ model.name }}_cost_y_0_fun_n_out(void); - -int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); -int {{ model.name }}_cost_y_0_hess_n_in(void); -int {{ model.name }}_cost_y_0_hess_n_out(void); -{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - -int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int); -const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int); -int {{ model.name }}_conl_cost_0_fun_n_in(void); -int {{ model.name }}_conl_cost_0_fun_n_out(void); - -int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void); -int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void); - -{% elif cost.cost_type_0 == "EXTERNAL" %} - {%- if cost.cost_ext_fun_type_0 == "casadi" %} -int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); - {%- else %} -int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); - {%- endif %} -{% endif %} - - -// Cost at path shooting node -{% if cost.cost_type == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_fun_sparsity_out(int); -int {{ model.name }}_cost_y_fun_n_in(void); -int {{ model.name }}_cost_y_fun_n_out(void); - -int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_hess_sparsity_out(int); -int {{ model.name }}_cost_y_hess_n_in(void); -int {{ model.name }}_cost_y_hess_n_out(void); - -{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} -int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_fun_sparsity_in(int); -const int *{{ model.name }}_conl_cost_fun_sparsity_out(int); -int {{ model.name }}_conl_cost_fun_n_in(void); -int {{ model.name }}_conl_cost_fun_n_out(void); - -int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void); -int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void); -{% elif cost.cost_type == "EXTERNAL" %} - {%- if cost.cost_ext_fun_type == "casadi" %} -int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); - {%- else %} -int {{ cost.cost_function_ext_cost }}(void **, void **, void *); - {%- endif %} -{% endif %} - -// Cost at terminal shooting node -{% if cost.cost_type_e == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_n_in(void); -int {{ model.name }}_cost_y_e_fun_n_out(void); - -int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); -int {{ model.name }}_cost_y_e_hess_n_in(void); -int {{ model.name }}_cost_y_e_hess_n_out(void); -{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} -int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int); -const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int); -int {{ model.name }}_conl_cost_e_fun_n_in(void); -int {{ model.name }}_conl_cost_e_fun_n_out(void); - -int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void); -int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void); -{% elif cost.cost_type_e == "EXTERNAL" %} - {%- if cost.cost_ext_fun_type_e == "casadi" %} -int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); - {%- else %} -int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); - {%- endif %} -{% endif %} - - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_COST diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m deleted file mode 100644 index 5d74c523f88bd0..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m +++ /dev/null @@ -1,432 +0,0 @@ -% -% Copyright (c) The acados authors. -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; - -% - -SOURCES = { ... - {%- if solver_options.integrator_type == 'ERK' %} - '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ... - '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',... - {%- if solver_options.hessian_approx == 'EXACT' %} - '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',... - {%- endif %} - {%- elif solver_options.integrator_type == "IRK" %} - '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ... - '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ... - '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ... - {%- if solver_options.hessian_approx == 'EXACT' %} - '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',... - {%- endif %} - {%- elif solver_options.integrator_type == "GNSF" %} - {% if model.gnsf.purely_linear != 1 %} - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',... - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',... - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',... - {% if model.gnsf.nontrivial_f_LO == 1 %} - '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',... - {%- endif %} - {%- endif %} - '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',... - {%- elif solver_options.integrator_type == "DISCRETE" %} - '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',... - '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',... - {%- if solver_options.hessian_approx == "EXACT" %} - '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',... - {%- endif %} - {%- endif %} - {%- if cost.cost_type_0 == "NONLINEAR_LS" %} - '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',... - {%- elif cost.cost_type_0 == "EXTERNAL" %} - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',... - {%- endif %} - - {%- if cost.cost_type == "NONLINEAR_LS" %} - '{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',... - {%- elif cost.cost_type == "EXTERNAL" %} - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',... - {%- endif %} - {%- if cost.cost_type_e == "NONLINEAR_LS" %} - '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',... - {%- elif cost.cost_type_e == "EXTERNAL" %} - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',... - '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',... - {%- endif %} - {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} - '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ... - '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ... - '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ... - {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} - '{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ... - {%- endif %} - {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} - '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ... - '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ... - '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ... - {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} - '{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ... - {%- endif %} - 'acados_solver_sfunction_{{ model.name }}.c', ... - 'acados_solver_{{ model.name }}.c' - }; - -INC_PATH = '{{ acados_include_path }}'; - -INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... - ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... - ['-I', fullfile(INC_PATH, 'acados')], ... - ['-I', fullfile(INC_PATH)]}; - -{% if solver_options.qp_solver is containing("QPOASES") %} -INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')]; -{% endif %} - -CFLAGS = 'CFLAGS=$CFLAGS'; -LDFLAGS = 'LDFLAGS=$LDFLAGS'; -COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; -COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; - -{% if solver_options.qp_solver is containing("QPOASES") %} -CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ]; -COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ]; -{%- elif solver_options.qp_solver is containing("OSQP") %} -CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ]; -COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ]; -{%- elif solver_options.qp_solver is containing("QPDUNES") %} -CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; -COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; -{%- elif solver_options.qp_solver is containing("DAQP") %} -CFLAGS = [ CFLAGS, ' -DACADOS_WITH_DAQP' ]; -COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_DAQP' ]; -{%- elif solver_options.qp_solver is containing("HPMPC") %} -CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ]; -COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ]; -{% endif %} - -LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')]; - -LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; - -% acados linking libraries and flags -{%- if acados_link_libs and os and os == "pc" %} -LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}']; -COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}']; -LIBS{end+1} = '{{ acados_link_libs.qpoases }}'; -LIBS{end+1} = '{{ acados_link_libs.hpmpc }}'; -LIBS{end+1} = '{{ acados_link_libs.osqp }}'; -{%- else %} - {% if solver_options.qp_solver is containing("QPOASES") %} -LIBS{end+1} = '-lqpOASES_e'; - {% endif %} - {% if solver_options.qp_solver is containing("DAQP") %} -LIBS{end+1} = '-ldaqp'; - {% endif %} -{%- endif %} - - -try - % mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - mex('-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - LIB_PATH, LIBS{:}, SOURCES{:}, ... - '-output', 'acados_solver_sfunction_{{ model.name }}' ); -catch exception - disp('make_sfun failed with the following exception:') - disp(exception); - disp('Try adding -v to the mex command above to get more information.') - keyboard -end - -fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ... - eval('mexext')] ); - - -%% print note on usage of s-function, and create I/O port names vectors -fprintf('\n\nNote: Usage of Sfunction is as follows:\n') -input_note = 'Inputs are:\n'; -i_in = 1; - -global sfun_input_names -sfun_input_names = {}; - -{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #} -input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... - ' size [{{ dims.nbx_0 }}]\n '); -sfun_input_names = [sfun_input_names; 'lbx_0 [{{ dims.nbx_0 }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} -input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... - ' size [{{ dims.nbx_0 }}]\n '); -sfun_input_names = [sfun_input_names; 'ubx_0 [{{ dims.nbx_0 }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} -input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N,',... - ' size [{{ (dims.N+1)*dims.np }}]\n '); -sfun_input_names = [sfun_input_names; 'parameter_traj [{{ (dims.N+1)*dims.np }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} -input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n '); -sfun_input_names = [sfun_input_names; 'y_ref_0 [{{ dims.ny_0 }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} -input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... - ' size [{{ (dims.N-1) * dims.ny }}]\n '); -sfun_input_names = [sfun_input_names; 'y_ref [{{ (dims.N-1) * dims.ny }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} -input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n '); -sfun_input_names = [sfun_input_names; 'y_ref_e [{{ dims.ny_e }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #} -input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); -sfun_input_names = [sfun_input_names; 'lbx [{{ (dims.N-1) * dims.nbx }}]']; -i_in = i_in + 1; -{%- endif %} -{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #} -input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n '); -sfun_input_names = [sfun_input_names; 'ubx [{{ (dims.N-1) * dims.nbx }}]']; -i_in = i_in + 1; -{%- endif %} - - -{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #} -input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n '); -sfun_input_names = [sfun_input_names; 'lbx_e [{{ dims.nbx_e }}]']; -i_in = i_in + 1; -{%- endif %} -{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} -input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n '); -sfun_input_names = [sfun_input_names; 'ubx_e [{{ dims.nbx_e }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #} -input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); -sfun_input_names = [sfun_input_names; 'lbu [{{ dims.N*dims.nbu }}]']; -i_in = i_in + 1; -{%- endif -%} -{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #} -input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n '); -sfun_input_names = [sfun_input_names; 'ubu [{{ dims.N*dims.nbu }}]']; -i_in = i_in + 1; -{%- endif -%} - -{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} -input_note = strcat(input_note, num2str(i_in), ') lg for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n '); -sfun_input_names = [sfun_input_names; 'lg [{{ dims.N*dims.ng }}]']; -i_in = i_in + 1; -{%- endif %} -{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} -input_note = strcat(input_note, num2str(i_in), ') ug for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n '); -sfun_input_names = [sfun_input_names; 'ug [{{ dims.N*dims.ng }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #} -input_note = strcat(input_note, num2str(i_in), ') lh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n '); -sfun_input_names = [sfun_input_names; 'lh [{{ dims.N*dims.nh }}]']; -i_in = i_in + 1; -{%- endif %} -{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #} -input_note = strcat(input_note, num2str(i_in), ') uh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n '); -sfun_input_names = [sfun_input_names; 'uh [{{ dims.N*dims.nh }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} -input_note = strcat(input_note, num2str(i_in), ') lh_e, size [{{ dims.nh_e }}]\n '); -sfun_input_names = [sfun_input_names; 'lh_e [{{ dims.nh_e }}]']; -i_in = i_in + 1; -{%- endif %} -{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} -input_note = strcat(input_note, num2str(i_in), ') uh_e, size [{{ dims.nh_e }}]\n '); -sfun_input_names = [sfun_input_names; 'uh_e [{{ dims.nh_e }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #} -input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n '); -sfun_input_names = [sfun_input_names; 'cost_W_0 [{{ dims.ny_0 * dims.ny_0 }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #} -input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n '); -sfun_input_names = [sfun_input_names; 'cost_W [{{ dims.ny * dims.ny }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #} -input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n '); -sfun_input_names = [sfun_input_names; 'cost_W_e [{{ dims.ny_e * dims.ny_e }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} -input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n '); -sfun_input_names = [sfun_input_names; 'reset_solver [1]']; -i_in = i_in + 1; -{%- endif %} - -{%- if simulink_opts.inputs.x_init %} {#- x_init #} -input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n '); -sfun_input_names = [sfun_input_names; 'x_init [{{ dims.nx * (dims.N+1) }}]']; -i_in = i_in + 1; -{%- endif %} - -{%- if simulink_opts.inputs.u_init %} {#- u_init #} -input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n '); -sfun_input_names = [sfun_input_names; 'u_init [{{ dims.nu * (dims.N) }}]']; -i_in = i_in + 1; -{%- endif %} - -fprintf(input_note) - -disp(' ') - -output_note = 'Outputs are:\n'; -i_out = 0; - -global sfun_output_names -sfun_output_names = {}; - -{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n '); -sfun_output_names = [sfun_output_names; 'u0 [{{ dims.nu }}]']; -{%- endif %} - -{%- if simulink_opts.outputs.utraj == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n '); -sfun_output_names = [sfun_output_names; 'utraj [{{ dims.nu * dims.N }}]']; -{%- endif %} - -{%- if simulink_opts.outputs.xtraj == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n '); -sfun_output_names = [sfun_output_names; 'xtraj [{{ dims.nx * (dims.N + 1) }}]']; -{%- endif %} - -{%- if simulink_opts.outputs.solver_status == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); -sfun_output_names = [sfun_output_names; 'solver_status']; -{%- endif %} - -{%- if simulink_opts.outputs.cost_value == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') cost function value\n '); -sfun_output_names = [sfun_output_names; 'cost_value']; -{%- endif %} - - -{%- if simulink_opts.outputs.KKT_residual == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); -sfun_output_names = [sfun_output_names; 'KKT_residual']; -{%- endif %} - -{%- if simulink_opts.outputs.KKT_residuals == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') KKT residuals, size [4] (stat, eq, ineq, comp)\n '); -sfun_output_names = [sfun_output_names; 'KKT_residuals [4]']; -{%- endif %} - -{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); -sfun_output_names = [sfun_output_names; 'x1']; -{%- endif %} - -{%- if simulink_opts.outputs.CPU_time == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); -sfun_output_names = [sfun_output_names; 'CPU_time']; -{%- endif %} - -{%- if simulink_opts.outputs.CPU_time_sim == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n '); -sfun_output_names = [sfun_output_names; 'CPU_time_sim']; -{%- endif %} - -{%- if simulink_opts.outputs.CPU_time_qp == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n '); -sfun_output_names = [sfun_output_names; 'CPU_time_qp']; -{%- endif %} - -{%- if simulink_opts.outputs.CPU_time_lin == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n '); -sfun_output_names = [sfun_output_names; 'CPU_time_lin']; -{%- endif %} - -{%- if simulink_opts.outputs.sqp_iter == 1 %} -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); -sfun_output_names = [sfun_output_names; 'sqp_iter']; -{%- endif %} - -fprintf(output_note) - -% The mask drawing command is: -% --- -% global sfun_input_names sfun_output_names -% for i = 1:length(sfun_input_names) -% port_label('input', i, sfun_input_names{i}) -% end -% for i = 1:length(sfun_output_names) -% port_label('output', i, sfun_output_names{i}) -% end -% --- -% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands -% (you can access it wirth ctrl+M on the s-function) \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m deleted file mode 100644 index e4c32a8c19a9d2..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m +++ /dev/null @@ -1,137 +0,0 @@ -% -% Copyright (c) The acados authors. -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; - -% - -SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ... - 'acados_sim_solver_{{ model.name }}.c ', ... - {%- if solver_options.integrator_type == 'ERK' %} - '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ',... - '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',... - '{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c ',... - {%- if solver_options.hessian_approx == 'EXACT' %} - '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',... - {%- endif %} - {%- elif solver_options.integrator_type == "IRK" %} - '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ... - '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ... - '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ... - {%- if solver_options.hessian_approx == 'EXACT' %} - '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',... - {%- endif %} - {%- elif solver_options.integrator_type == "GNSF" %} - {%- if model.gnsf.purely_linear != 1 %} - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c ',... - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c ',... - '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ',... - {%- if model.gnsf.nontrivial_f_LO == 1 %} - '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ',... - {%- endif %} - {%- endif %} - '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ',... - {%- endif %} - ]; - -INC_PATH = '{{ acados_include_path }}'; - -INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ... - ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ... - ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' ']; - -CFLAGS = ' -O'; - -LIB_PATH = '{{ acados_lib_path }}'; - -LIBS = '-lacados -lblasfeo -lhpipm'; - -try - % eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... - eval( [ 'mex -output acados_sim_solver_sfunction_{{ model.name }} ', ... - CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); - -catch exception - disp('make_sfun failed with the following exception:') - disp(exception); - disp('Try adding -v to the mex command above to get more information.') - keyboard -end - - -fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ... - eval('mexext')] ); - - -global sfun_sim_input_names -sfun_sim_input_names = {}; - -%% print note on usage of s-function -fprintf('\n\nNote: Usage of Sfunction is as follows:\n') -input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n '; -i_in = 2; -sfun_sim_input_names = [sfun_sim_input_names; 'x0 [{{ dims.nx }}]']; - -{%- if dims.nu > 0 %} -input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n '); -i_in = i_in + 1; -sfun_sim_input_names = [sfun_sim_input_names; 'u [{{ dims.nu }}]']; -{%- endif %} - -{%- if dims.np > 0 %} -input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n '); -i_in = i_in + 1; -sfun_sim_input_names = [sfun_sim_input_names; 'p [{{ dims.np }}]']; -{%- endif %} - - -fprintf(input_note) - -disp(' ') - -global sfun_sim_output_names -sfun_sim_output_names = {}; - -output_note = strcat('Outputs are:\n', ... - '1) x1 - simulated state, size [{{ dims.nx }}]\n'); -sfun_sim_output_names = [sfun_sim_output_names; 'x1 [{{ dims.nx }}]']; - -fprintf(output_note) - - -% The mask drawing command is: -% --- -% global sfun_sim_input_names sfun_sim_output_names -% for i = 1:length(sfun_sim_input_names) -% port_label('input', i, sfun_sim_input_names{i}) -% end -% for i = 1:length(sfun_sim_output_names) -% port_label('output', i, sfun_sim_output_names{i}) -% end -% --- -% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands -% (you can access it wirth ctrl+M on the s-function) \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m deleted file mode 100644 index 374321283099bd..00000000000000 --- a/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m +++ /dev/null @@ -1,270 +0,0 @@ -% -% Copyright (c) The acados authors. -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; - -% - -classdef {{ model.name }}_mex_solver < handle - - properties - C_ocp - C_ocp_ext_fun - cost_ext_fun_type - cost_ext_fun_type_e - N - name - code_gen_dir - end % properties - - - - methods - - % constructor - function obj = {{ model.name }}_mex_solver() - make_mex_{{ model.name }}(); - [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}(); - % to have path to destructor when changing directory - addpath('.') - obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}'; - obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}'; - obj.N = {{ dims.N }}; - obj.name = '{{ model.name }}'; - obj.code_gen_dir = pwd(); - end - - % destructor - function delete(obj) - disp("delete template..."); - return_dir = pwd(); - cd(obj.code_gen_dir); - if ~isempty(obj.C_ocp) - acados_mex_free_{{ model.name }}(obj.C_ocp); - end - cd(return_dir); - disp("done."); - end - - % solve - function solve(obj) - acados_mex_solve_{{ model.name }}(obj.C_ocp); - end - - % set -- borrowed from MEX interface - function set(varargin) - obj = varargin{1}; - field = varargin{2}; - value = varargin{3}; - if ~isa(field, 'char') - error('field must be a char vector, use '' '''); - end - if nargin==3 - acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value); - elseif nargin==4 - stage = varargin{4}; - acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage); - else - disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)'); - end - end - - function value = get_cost(obj) - value = ocp_get_cost(obj.C_ocp); - end - - % get -- borrowed from MEX interface - function value = get(varargin) - % usage: - % obj.get(field, value, [stage]) - obj = varargin{1}; - field = varargin{2}; - if any(strfind('sens', field)) - error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') - end - if ~isa(field, 'char') - error('field must be a char vector, use '' '''); - end - - if nargin==2 - value = ocp_get(obj.C_ocp, field); - elseif nargin==3 - stage = varargin{3}; - value = ocp_get(obj.C_ocp, field, stage); - else - disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); - end - end - - - function [] = store_iterate(varargin) - %%% Stores the current iterate of the ocp solver in a json file. - %%% param1: filename: if not set, use model_name + timestamp + '.json' - %%% param2: overwrite: if false and filename exists add timestamp to filename - - obj = varargin{1}; - filename = ''; - overwrite = false; - - if nargin>=2 - filename = varargin{2}; - if ~isa(filename, 'char') - error('filename must be a char vector, use '' '''); - end - end - - if nargin==3 - overwrite = varargin{3}; - end - - if nargin > 3 - disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); - end - - if strcmp(filename,'') - filename = [obj.name '_iterate.json']; - end - if ~overwrite - % append timestamp - if exist(filename, 'file') - filename = filename(1:end-5); - filename = [filename '_' datestr(now,'yyyy-mm-dd-HH:MM:SS') '.json']; - end - end - filename = fullfile(pwd, filename); - - % get iterate: - solution = struct(); - for i=0:obj.N - solution.(['x_' num2str(i)]) = obj.get('x', i); - solution.(['lam_' num2str(i)]) = obj.get('lam', i); - solution.(['t_' num2str(i)]) = obj.get('t', i); - solution.(['sl_' num2str(i)]) = obj.get('sl', i); - solution.(['su_' num2str(i)]) = obj.get('su', i); - end - for i=0:obj.N-1 - solution.(['z_' num2str(i)]) = obj.get('z', i); - solution.(['u_' num2str(i)]) = obj.get('u', i); - solution.(['pi_' num2str(i)]) = obj.get('pi', i); - end - - acados_folder = getenv('ACADOS_INSTALL_DIR'); - addpath(fullfile(acados_folder, 'external', 'jsonlab')); - savejson('', solution, filename); - - json_string = savejson('', solution, 'ForceRootName', 0); - - fid = fopen(filename, 'w'); - if fid == -1, error('store_iterate: Cannot create JSON file'); end - fwrite(fid, json_string, 'char'); - fclose(fid); - - disp(['stored current iterate in ' filename]); - end - - - function [] = load_iterate(obj, filename) - %%% Loads the iterate stored in json file with filename into the ocp solver. - acados_folder = getenv('ACADOS_INSTALL_DIR'); - addpath(fullfile(acados_folder, 'external', 'jsonlab')); - filename = fullfile(pwd, filename); - - if ~exist(filename, 'file') - error(['load_iterate: failed, file does not exist: ' filename]) - end - - solution = loadjson(filename); - keys = fieldnames(solution); - - for k = 1:numel(keys) - key = keys{k}; - key_parts = strsplit(key, '_'); - field = key_parts{1}; - stage = key_parts{2}; - - val = solution.(key); - - % check if array is empty (can happen for z) - if numel(val) > 0 - obj.set(field, val, str2num(stage)) - end - end - end - - - % print - function print(varargin) - if nargin < 2 - field = 'stat'; - else - field = varargin{2}; - end - - obj = varargin{1}; - - if strcmp(field, 'stat') - stat = obj.get('stat'); - {%- if solver_options.nlp_solver_type == "SQP" %} - fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha'); - if size(stat,2)>8 - fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); - end - fprintf('\n'); - for jj=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8)); - if size(stat,2)>8 - fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12)); - end - fprintf('\n'); - end - fprintf('\n'); - {%- else %} - fprintf('\niter\tqp_status\tqp_iter'); - if size(stat,2)>3 - fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp'); - end - fprintf('\n'); - for jj=1:size(stat,1) - fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3)); - if size(stat,2)>3 - fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7)); - end - fprintf('\n'); - end - {% endif %} - - else - fprintf('unsupported field in function print of acados_ocp.print, got %s', field); - keyboard - end - - end - - end % methods - -end % class - diff --git a/third_party/acados/acados_template/casadi_function_generation.py b/third_party/acados/acados_template/casadi_function_generation.py deleted file mode 100644 index 6373a2809dbc6b..00000000000000 --- a/third_party/acados/acados_template/casadi_function_generation.py +++ /dev/null @@ -1,708 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -import casadi as ca -from .utils import is_empty, casadi_length - - -def get_casadi_symbol(x): - if isinstance(x, ca.MX): - return ca.MX.sym - elif isinstance(x, ca.SX): - return ca.SX.sym - else: - raise TypeError("Expected casadi SX or MX.") - -################ -# Dynamics -################ - - -def generate_c_code_discrete_dynamics( model, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - # load model - x = model.x - u = model.u - p = model.p - phi = model.disc_dyn_expr - model_name = model.name - nx = casadi_length(x) - - symbol = get_casadi_symbol(x) - # assume nx1 = nx !!! - lam = symbol('lam', nx, 1) - - # generate jacobians - ux = ca.vertcat(u,x) - jac_ux = ca.jacobian(phi, ux) - # generate adjoint - adj_ux = ca.jtimes(phi, ux, lam, True) - # generate hessian - hess_ux = ca.jacobian(adj_ux, ux) - - # change directory - cwd = os.getcwd() - model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) - if not os.path.exists(model_dir): - os.makedirs(model_dir) - os.chdir(model_dir) - - # set up & generate ca.Functions - fun_name = model_name + '_dyn_disc_phi_fun' - phi_fun = ca.Function(fun_name, [x, u, p], [phi]) - phi_fun.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_dyn_disc_phi_fun_jac' - phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) - phi_fun_jac_ut_xt.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' - phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) - phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_codegen_opts) - - os.chdir(cwd) - return - - - -def generate_c_code_explicit_ode( model, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - generate_hess = opts["generate_hess"] - - # load model - x = model.x - u = model.u - p = model.p - f_expl = model.f_expl_expr - model_name = model.name - - ## get model dimensions - nx = x.size()[0] - nu = u.size()[0] - - symbol = get_casadi_symbol(x) - - ## set up functions to be exported - Sx = symbol('Sx', nx, nx) - Sp = symbol('Sp', nx, nu) - lambdaX = symbol('lambdaX', nx, 1) - - fun_name = model_name + '_expl_ode_fun' - - ## Set up functions - expl_ode_fun = ca.Function(fun_name, [x, u, p], [f_expl]) - - vdeX = ca.jtimes(f_expl,x,Sx) - vdeP = ca.jacobian(f_expl,u) + ca.jtimes(f_expl,x,Sp) - - fun_name = model_name + '_expl_vde_forw' - - expl_vde_forw = ca.Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) - - adj = ca.jtimes(f_expl, ca.vertcat(x, u), lambdaX, True) - - fun_name = model_name + '_expl_vde_adj' - expl_vde_adj = ca.Function(fun_name, [x, lambdaX, u, p], [adj]) - - if generate_hess: - S_forw = ca.vertcat(ca.horzcat(Sx, Sp), ca.horzcat(ca.DM.zeros(nu,nx), ca.DM.eye(nu))) - hess = ca.mtimes(ca.transpose(S_forw),ca.jtimes(adj, ca.vertcat(x,u), S_forw)) - hess2 = [] - for j in range(nx+nu): - for i in range(j,nx+nu): - hess2 = ca.vertcat(hess2, hess[i,j]) - - fun_name = model_name + '_expl_ode_hess' - expl_ode_hess = ca.Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) - - # change directory - cwd = os.getcwd() - model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) - if not os.path.exists(model_dir): - os.makedirs(model_dir) - os.chdir(model_dir) - - # generate C code - fun_name = model_name + '_expl_ode_fun' - expl_ode_fun.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_expl_vde_forw' - expl_vde_forw.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_expl_vde_adj' - expl_vde_adj.generate(fun_name, casadi_codegen_opts) - - if generate_hess: - fun_name = model_name + '_expl_ode_hess' - expl_ode_hess.generate(fun_name, casadi_codegen_opts) - os.chdir(cwd) - - return - - -def generate_c_code_implicit_ode( model, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - # load model - x = model.x - xdot = model.xdot - u = model.u - z = model.z - p = model.p - f_impl = model.f_impl_expr - model_name = model.name - - # get model dimensions - nx = casadi_length(x) - nz = casadi_length(z) - - # generate jacobians - jac_x = ca.jacobian(f_impl, x) - jac_xdot = ca.jacobian(f_impl, xdot) - jac_u = ca.jacobian(f_impl, u) - jac_z = ca.jacobian(f_impl, z) - - # Set up functions - p = model.p - fun_name = model_name + '_impl_dae_fun' - impl_dae_fun = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - impl_dae_fun_jac_x_xdot_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' - impl_dae_fun_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' - impl_dae_fun_jac_x_xdot_u = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) - - fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' - impl_dae_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) - - if opts["generate_hess"]: - x_xdot_z_u = ca.vertcat(x, xdot, z, u) - symbol = get_casadi_symbol(x) - multiplier = symbol('multiplier', nx + nz) - ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True) - HESS = ca.jacobian(ADJ, x_xdot_z_u) - fun_name = model_name + '_impl_dae_hess' - impl_dae_hess = ca.Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) - - # change directory - cwd = os.getcwd() - model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) - if not os.path.exists(model_dir): - os.makedirs(model_dir) - os.chdir(model_dir) - - # generate C code - fun_name = model_name + '_impl_dae_fun' - impl_dae_fun.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' - impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' - impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' - impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_codegen_opts) - - if opts["generate_hess"]: - fun_name = model_name + '_impl_dae_hess' - impl_dae_hess.generate(fun_name, casadi_codegen_opts) - - os.chdir(cwd) - return - - -def generate_c_code_gnsf( model, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - model_name = model.name - - # set up directory - cwd = os.getcwd() - model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) - if not os.path.exists(model_dir): - os.makedirs(model_dir) - os.chdir(model_dir) - - # obtain gnsf dimensions - get_matrices_fun = model.get_matrices_fun - phi_fun = model.phi_fun - - size_gnsf_A = get_matrices_fun.size_out(0) - gnsf_nx1 = size_gnsf_A[1] - gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] - gnsf_nuhat = max(phi_fun.size_in(1)) - gnsf_ny = max(phi_fun.size_in(0)) - gnsf_nout = max(phi_fun.size_out(0)) - - # set up expressions - # if the model uses ca.MX because of cost/constraints - # the DAE can be exported as ca.SX -> detect GNSF in Matlab - # -> evaluated ca.SX GNSF functions with ca.MX. - u = model.u - symbol = get_casadi_symbol(u) - - y = symbol("y", gnsf_ny, 1) - uhat = symbol("uhat", gnsf_nuhat, 1) - p = model.p - x1 = symbol("gnsf_x1", gnsf_nx1, 1) - x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) - z1 = symbol("gnsf_z1", gnsf_nz1, 1) - dummy = symbol("gnsf_dummy", 1, 1) - empty_var = symbol("gnsf_empty_var", 0, 0) - - ## generate C code - fun_name = model_name + '_gnsf_phi_fun' - phi_fun_ = ca.Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) - phi_fun_.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_gnsf_phi_fun_jac_y' - phi_fun_jac_y = model.phi_fun_jac_y - phi_fun_jac_y_ = ca.Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) - phi_fun_jac_y_.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_gnsf_phi_jac_y_uhat' - phi_jac_y_uhat = model.phi_jac_y_uhat - phi_jac_y_uhat_ = ca.Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) - phi_jac_y_uhat_.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' - f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz - f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) - - # avoid codegeneration issue - if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): - f_lo_fun_jac_x1k1uz_eval = [empty_var] - - f_lo_fun_jac_x1k1uz_ = ca.Function(fun_name, [x1, x1dot, z1, u, p], - f_lo_fun_jac_x1k1uz_eval) - f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_codegen_opts) - - fun_name = model_name + '_gnsf_get_matrices_fun' - get_matrices_fun_ = ca.Function(fun_name, [dummy], get_matrices_fun(1)) - get_matrices_fun_.generate(fun_name, casadi_codegen_opts) - - # remove fields for json dump - del model.phi_fun - del model.phi_fun_jac_y - del model.phi_jac_y_uhat - del model.f_lo_fun_jac_x1k1uz - del model.get_matrices_fun - - os.chdir(cwd) - - return - - -################ -# Cost -################ - -def generate_c_code_external_cost(model, stage_type, opts): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - x = model.x - p = model.p - u = model.u - z = model.z - symbol = get_casadi_symbol(x) - - if stage_type == 'terminal': - suffix_name = "_cost_ext_cost_e_fun" - suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_e_fun_jac" - ext_cost = model.cost_expr_ext_cost_e - custom_hess = model.cost_expr_ext_cost_custom_hess_e - # Last stage cannot depend on u and z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) - - elif stage_type == 'path': - suffix_name = "_cost_ext_cost_fun" - suffix_name_hess = "_cost_ext_cost_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_fun_jac" - ext_cost = model.cost_expr_ext_cost - custom_hess = model.cost_expr_ext_cost_custom_hess - - elif stage_type == 'initial': - suffix_name = "_cost_ext_cost_0_fun" - suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_0_fun_jac" - ext_cost = model.cost_expr_ext_cost_0 - custom_hess = model.cost_expr_ext_cost_custom_hess_0 - - nunx = x.shape[0] + u.shape[0] - - # set up functions to be exported - fun_name = model.name + suffix_name - fun_name_hess = model.name + suffix_name_hess - fun_name_jac = model.name + suffix_name_jac - - # generate expression for full gradient and Hessian - hess_uxz, grad_uxz = ca.hessian(ext_cost, ca.vertcat(u, x, z)) - - hess_ux = hess_uxz[:nunx, :nunx] - hess_z = hess_uxz[nunx:, nunx:] - hess_z_ux = hess_uxz[nunx:, :nunx] - - if custom_hess is not None: - hess_ux = custom_hess - - ext_cost_fun = ca.Function(fun_name, [x, u, z, p], [ext_cost]) - - ext_cost_fun_jac_hess = ca.Function( - fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux] - ) - ext_cost_fun_jac = ca.Function( - fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz] - ) - - # change directory - cwd = os.getcwd() - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) - if not os.path.exists(cost_dir): - os.makedirs(cost_dir) - os.chdir(cost_dir) - - ext_cost_fun.generate(fun_name, casadi_codegen_opts) - ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_codegen_opts) - ext_cost_fun_jac.generate(fun_name_jac, casadi_codegen_opts) - - os.chdir(cwd) - return - - -def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - x = model.x - z = model.z - p = model.p - u = model.u - - symbol = get_casadi_symbol(x) - - if stage_type == 'terminal': - middle_name = '_cost_y_e' - u = symbol('u', 0, 0) - y_expr = model.cost_y_expr_e - - elif stage_type == 'initial': - middle_name = '_cost_y_0' - y_expr = model.cost_y_expr_0 - - elif stage_type == 'path': - middle_name = '_cost_y' - y_expr = model.cost_y_expr - - # change directory - cwd = os.getcwd() - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) - if not os.path.exists(cost_dir): - os.makedirs(cost_dir) - os.chdir(cost_dir) - - # set up expressions - cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x))) - dy_dz = ca.jacobian(y_expr, z) - ny = casadi_length(y_expr) - - y = symbol('y', ny, 1) - - y_adj = ca.jtimes(y_expr, ca.vertcat(u, x), y, True) - y_hess = ca.jacobian(y_adj, ca.vertcat(u, x)) - - ## generate C code - suffix_name = '_fun' - fun_name = cost_name + middle_name + suffix_name - y_fun = ca.Function( fun_name, [x, u, z, p], [ y_expr ]) - y_fun.generate( fun_name, casadi_codegen_opts ) - - suffix_name = '_fun_jac_ut_xt' - fun_name = cost_name + middle_name + suffix_name - y_fun_jac_ut_xt = ca.Function(fun_name, [x, u, z, p], [ y_expr, cost_jac_expr, dy_dz ]) - y_fun_jac_ut_xt.generate( fun_name, casadi_codegen_opts ) - - suffix_name = '_hess' - fun_name = cost_name + middle_name + suffix_name - y_hess = ca.Function(fun_name, [x, u, z, y, p], [ y_hess ]) - y_hess.generate( fun_name, casadi_codegen_opts ) - - os.chdir(cwd) - - return - - - -def generate_c_code_conl_cost(model, cost_name, stage_type, opts): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - x = model.x - z = model.z - p = model.p - - symbol = get_casadi_symbol(x) - - if stage_type == 'terminal': - u = symbol('u', 0, 0) - - yref = model.cost_r_in_psi_expr_e - inner_expr = model.cost_y_expr_e - yref - outer_expr = model.cost_psi_expr_e - res_expr = model.cost_r_in_psi_expr_e - - suffix_name_fun = '_conl_cost_e_fun' - suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' - - custom_hess = model.cost_conl_custom_outer_hess_e - - elif stage_type == 'initial': - u = model.u - - yref = model.cost_r_in_psi_expr_0 - inner_expr = model.cost_y_expr_0 - yref - outer_expr = model.cost_psi_expr_0 - res_expr = model.cost_r_in_psi_expr_0 - - suffix_name_fun = '_conl_cost_0_fun' - suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess' - - custom_hess = model.cost_conl_custom_outer_hess_0 - - elif stage_type == 'path': - u = model.u - - yref = model.cost_r_in_psi_expr - inner_expr = model.cost_y_expr - yref - outer_expr = model.cost_psi_expr - res_expr = model.cost_r_in_psi_expr - - suffix_name_fun = '_conl_cost_fun' - suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess' - - custom_hess = model.cost_conl_custom_outer_hess - - # set up function names - fun_name_cost_fun = model.name + suffix_name_fun - fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess - - # set up functions to be exported - outer_loss_fun = ca.Function('psi', [res_expr, p], [outer_expr]) - cost_expr = outer_loss_fun(inner_expr, p) - - outer_loss_grad_fun = ca.Function('outer_loss_grad', [res_expr, p], [ca.jacobian(outer_expr, res_expr).T]) - - if custom_hess is None: - outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [ca.hessian(outer_loss_fun(res_expr, p), res_expr)[0]]) - else: - outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [custom_hess]) - - Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T - Jt_z_expr = ca.jacobian(inner_expr, z).T - - cost_fun = ca.Function( - fun_name_cost_fun, - [x, u, z, yref, p], - [cost_expr]) - - cost_fun_jac_hess = ca.Function( - fun_name_cost_fun_jac_hess, - [x, u, z, yref, p], - [cost_expr, outer_loss_grad_fun(inner_expr, p), Jt_ux_expr, Jt_z_expr, outer_hess_fun(inner_expr, p)] - ) - # change directory - cwd = os.getcwd() - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) - if not os.path.exists(cost_dir): - os.makedirs(cost_dir) - os.chdir(cost_dir) - - # generate C code - cost_fun.generate(fun_name_cost_fun, casadi_codegen_opts) - cost_fun_jac_hess.generate(fun_name_cost_fun_jac_hess, casadi_codegen_opts) - - os.chdir(cwd) - - return - - -################ -# Constraints -################ -def generate_c_code_constraint( model, con_name, is_terminal, opts ): - - casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - # load constraint variables and expression - x = model.x - p = model.p - - symbol = get_casadi_symbol(x) - - if is_terminal: - con_h_expr = model.con_h_expr_e - con_phi_expr = model.con_phi_expr_e - # create dummy u, z - u = symbol('u', 0, 0) - z = symbol('z', 0, 0) - else: - con_h_expr = model.con_h_expr - con_phi_expr = model.con_phi_expr - u = model.u - z = model.z - - if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): - raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") - - if (is_empty(con_h_expr) and is_empty(con_phi_expr)): - # both empty -> nothing to generate - return - - if is_empty(con_h_expr): - constr_type = 'BGP' - else: - constr_type = 'BGH' - - if is_empty(p): - p = symbol('p', 0, 0) - - if is_empty(z): - z = symbol('z', 0, 0) - - if not (is_empty(con_h_expr)) and opts['generate_hess']: - # multipliers for hessian - nh = casadi_length(con_h_expr) - lam_h = symbol('lam_h', nh, 1) - - # set up & change directory - cwd = os.getcwd() - constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints')) - if not os.path.exists(constraints_dir): - os.makedirs(constraints_dir) - os.chdir(constraints_dir) - - # export casadi functions - if constr_type == 'BGH': - if is_terminal: - fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' - else: - fun_name = con_name + '_constr_h_fun_jac_uxt_zt' - - jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x))) - jac_z_t = ca.jacobian(con_h_expr, z) - constraint_fun_jac_tran = ca.Function(fun_name, [x, u, z, p], \ - [con_h_expr, jac_ux_t, jac_z_t]) - - constraint_fun_jac_tran.generate(fun_name, casadi_codegen_opts) - if opts['generate_hess']: - - if is_terminal: - fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' - else: - fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' - - # adjoint - adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True) - # hessian - hess_ux = ca.jacobian(adj_ux, ca.vertcat(u, x)) - - adj_z = ca.jtimes(con_h_expr, z, lam_h, True) - hess_z = ca.jacobian(adj_z, z) - - # set up functions - constraint_fun_jac_tran_hess = \ - ca.Function(fun_name, [x, u, lam_h, z, p], \ - [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) - - # generate C code - constraint_fun_jac_tran_hess.generate(fun_name, casadi_codegen_opts) - - if is_terminal: - fun_name = con_name + '_constr_h_e_fun' - else: - fun_name = con_name + '_constr_h_fun' - h_fun = ca.Function(fun_name, [x, u, z, p], [con_h_expr]) - h_fun.generate(fun_name, casadi_codegen_opts) - - else: # BGP constraint - if is_terminal: - fun_name = con_name + '_phi_e_constraint' - r = model.con_r_in_phi_e - con_r_expr = model.con_r_expr_e - else: - fun_name = con_name + '_phi_constraint' - r = model.con_r_in_phi - con_r_expr = model.con_r_expr - - nphi = casadi_length(con_phi_expr) - con_phi_expr_x_u_z = ca.substitute(con_phi_expr, r, con_r_expr) - phi_jac_u = ca.jacobian(con_phi_expr_x_u_z, u) - phi_jac_x = ca.jacobian(con_phi_expr_x_u_z, x) - phi_jac_z = ca.jacobian(con_phi_expr_x_u_z, z) - - hess = ca.hessian(con_phi_expr[0], r)[0] - for i in range(1, nphi): - hess = ca.vertcat(hess, ca.hessian(con_phi_expr[i], r)[0]) - - r_jac_u = ca.jacobian(con_r_expr, u) - r_jac_x = ca.jacobian(con_r_expr, x) - - constraint_phi = \ - ca.Function(fun_name, [x, u, z, p], \ - [con_phi_expr_x_u_z, \ - ca.vertcat(ca.transpose(phi_jac_u), ca.transpose(phi_jac_x)), \ - ca.transpose(phi_jac_z), \ - hess, - ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))]) - - constraint_phi.generate(fun_name, casadi_codegen_opts) - - # change directory back - os.chdir(cwd) - - return - diff --git a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c deleted file mode 100644 index b39ff2e23baf15..00000000000000 --- a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c +++ /dev/null @@ -1,819 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -// This is a template based custom_update function -#include -#include -#include -#include - -#include "custom_update_function.h" -#include "acados_solver_{{ model.name }}.h" -#include "acados_c/ocp_nlp_interface.h" -#include "acados/utils/mem.h" - -#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" -#include "blasfeo/include/blasfeo_d_blasfeo_api.h" - - -typedef struct custom_memory -{ - // covariance matrics - struct blasfeo_dmat *uncertainty_matrix_buffer; // shape = (N+1, nx, nx) - // covariance matrix of the additive disturbance - struct blasfeo_dmat W_mat; // shape = (nw, nw) - struct blasfeo_dmat unc_jac_G_mat; // shape = (nx, nw) - struct blasfeo_dmat temp_GW_mat; // shape = (nx, nw) - struct blasfeo_dmat GWG_mat; // shape = (nx, nx) - // sensitivity matrices - struct blasfeo_dmat A_mat; // shape = (nx, nx) - struct blasfeo_dmat B_mat; // shape = (nx, nu) - // matrix in linear constraints - struct blasfeo_dmat Cg_mat; // shape = (ng, nx) - struct blasfeo_dmat Dg_mat; // shape = (ng, nu) - struct blasfeo_dmat Cg_e_mat; // shape = (ng_e, nx) - struct blasfeo_dmat dummy_Dgh_e_mat; // shape = (ngh_e_max, nu) - // matrix in nonlinear constraints - struct blasfeo_dmat Ch_mat; // shape = (nh, nx) - struct blasfeo_dmat Dh_mat; // shape = (nh, nu) - struct blasfeo_dmat Ch_e_mat; // shape = (nh_e, nx) - // feedback gain matrix - struct blasfeo_dmat K_mat; // shape = (nu, nx) - // AK = A - B@K - struct blasfeo_dmat AK_mat; // shape = (nx, nx) - // A@P_k - struct blasfeo_dmat temp_AP_mat; // shape = (nx, nx) - // K@P_k, K@P_k@K^T - struct blasfeo_dmat temp_KP_mat; // shape = (nu, nx) - struct blasfeo_dmat temp_KPK_mat; // shape = (nu, nu) - // C + D @ K, (C + D @ K) @ P_k - struct blasfeo_dmat temp_CaDK_mat; // shape = (ngh_me_max, nx) - struct blasfeo_dmat temp_CaDKmP_mat; // shape = (ngh_me_max, nx) - struct blasfeo_dmat temp_beta_mat; // shape = (ngh_me_max, ngh_me_max) - - double *d_A_mat; // shape = (nx, nx) - double *d_B_mat; // shape = (nx, nu) - double *d_Cg_mat; // shape = (ng, nx) - double *d_Dg_mat; // shape = (ng, nu) - double *d_Cg_e_mat; // shape = (ng_e, nx) - double *d_Cgh_mat; // shape = (ng+nh, nx) - double *d_Dgh_mat; // shape = (ng+nh, nu) - double *d_Cgh_e_mat; // shape = (ng_e+nh_e, nx) - double *d_state_vec; - // upper and lower bounds on state variables - double *d_lbx; // shape = (nbx,) - double *d_ubx; // shape = (nbx,) - double *d_lbx_e; // shape = (nbx_e,) - double *d_ubx_e; // shape = (nbx_e,) - // tightened upper and lower bounds on state variables - double *d_lbx_tightened; // shape = (nbx,) - double *d_ubx_tightened; // shape = (nbx,) - double *d_lbx_e_tightened; // shape = (nbx_e,) - double *d_ubx_e_tightened; // shape = (nbx_e,) - // upper and lower bounds on control inputs - double *d_lbu; // shape = (nbu,) - double *d_ubu; // shape = (nbu,) - // tightened upper and lower bounds on control inputs - double *d_lbu_tightened; // shape = (nbu,) - double *d_ubu_tightened; // shape = (nbu,) - // upper and lower bounds on polytopic constraints - double *d_lg; // shape = (ng,) - double *d_ug; // shape = (ng,) - double *d_lg_e; // shape = (ng_e,) - double *d_ug_e; // shape = (ng_e,) - // tightened lower bounds on polytopic constraints - double *d_lg_tightened; // shape = (ng,) - double *d_ug_tightened; // shape = (ng,) - double *d_lg_e_tightened; // shape = (ng_e,) - double *d_ug_e_tightened; // shape = (ng_e,) - // upper and lower bounds on nonlinear constraints - double *d_lh; // shape = (nh,) - double *d_uh; // shape = (nh,) - double *d_lh_e; // shape = (nh_e,) - double *d_uh_e; // shape = (nh_e,) - // tightened upper and lower bounds on nonlinear constraints - double *d_lh_tightened; // shape = (nh,) - double *d_uh_tightened; // shape = (nh,) - double *d_lh_e_tightened; // shape = (nh_e,) - double *d_uh_e_tightened; // shape = (nh_e,) - - int *idxbx; // shape = (nbx,) - int *idxbu; // shape = (nbu,) - int *idxbx_e; // shape = (nbx_e,) - - void *raw_memory; // Pointer to allocated memory, to be used for freeing -} custom_memory; - -static int int_max(int num1, int num2) -{ - return (num1 > num2 ) ? num1 : num2; -} - - -static int custom_memory_calculate_size(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims) -{ - int N = nlp_dims->N; - int nx = {{ dims.nx }}; - int nu = {{ dims.nu }}; - int nw = {{ zoro_description.nw }}; - - int ng = {{ dims.ng }}; - int nh = {{ dims.nh }}; - int nbx = {{ dims.nbx }}; - int nbu = {{ dims.nbu }}; - - int ng_e = {{ dims.ng_e }}; - int nh_e = {{ dims.nh_e }}; - int ngh_e_max = int_max(ng_e, nh_e); - int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh)); - int nbx_e = {{ dims.nbx_e }}; - - assert({{zoro_description.nlbx_t}} <= nbx); - assert({{zoro_description.nubx_t}} <= nbx); - assert({{zoro_description.nlbu_t}} <= nbu); - assert({{zoro_description.nubu_t}} <= nbu); - assert({{zoro_description.nlg_t}} <= ng); - assert({{zoro_description.nug_t}} <= ng); - assert({{zoro_description.nlh_t}} <= nh); - assert({{zoro_description.nuh_t}} <= nh); - assert({{zoro_description.nlbx_e_t}} <= nbx_e); - assert({{zoro_description.nubx_e_t}} <= nbx_e); - assert({{zoro_description.nlg_e_t}} <= ng_e); - assert({{zoro_description.nug_e_t}} <= ng_e); - assert({{zoro_description.nlh_e_t}} <= nh_e); - assert({{zoro_description.nuh_e_t}} <= nh_e); - - acados_size_t size = sizeof(custom_memory); - size += nbx * sizeof(int); - /* blasfeo structs */ - size += (N + 1) * sizeof(struct blasfeo_dmat); - /* blasfeo mem: mat */ - size += (N + 1) * blasfeo_memsize_dmat(nx, nx); // uncertainty_matrix_buffer - size += blasfeo_memsize_dmat(nw, nw); // W_mat - size += 2 * blasfeo_memsize_dmat(nx, nw); // unc_jac_G_mat, temp_GW_mat - size += 4 * blasfeo_memsize_dmat(nx, nx); // GWG_mat, A_mat, AK_mat, temp_AP_mat - size += blasfeo_memsize_dmat(nx, nu); // B_mat - size += 2 * blasfeo_memsize_dmat(nu, nx); // K_mat, temp_KP_mat - size += blasfeo_memsize_dmat(nu, nu); // temp_KPK_mat - size += blasfeo_memsize_dmat(ng, nx); // Cg_mat - size += blasfeo_memsize_dmat(ng, nu); // Dg_mat - size += blasfeo_memsize_dmat(ng_e, nx); // Cg_e_mat - size += blasfeo_memsize_dmat(ngh_e_max, nu); // dummy_Dgh_e_mat - size += blasfeo_memsize_dmat(nh, nx); // Ch_mat - size += blasfeo_memsize_dmat(nh, nu); // Dh_mat - size += blasfeo_memsize_dmat(nh_e, nx); // Ch_e_mat - size += 2 * blasfeo_memsize_dmat(ngh_me_max, nx); // temp_CaDK_mat, temp_CaDKmP_mat - size += blasfeo_memsize_dmat(ngh_me_max, ngh_me_max); // temp_beta_mat - /* blasfeo mem: vec */ - /* Arrays */ - size += nx*nx *sizeof(double); // d_A_mat - size += nx*nu *sizeof(double); // d_B_mat - size += (ng + ng_e) * nx * sizeof(double); // d_Cg_mat, d_Cg_e_mat - size += (ng) * nu * sizeof(double); // d_Dg_mat - size += (nh + nh_e + ng + ng_e) * nx * sizeof(double); // d_Cgh_mat, d_Cgh_e_mat - size += (nh + ng) * nu * sizeof(double); // d_Dgh_mat - // d_state_vec - size += nx *sizeof(double); - // constraints and tightened constraints - size += 4 * (nbx + nbu + ng + nh)*sizeof(double); - size += 4 * (nbx_e + ng_e + nh_e)*sizeof(double); - size += (nbx + nbu + nbx_e)*sizeof(int); // idxbx, idxbu, idxbx_e - - size += 1 * 8; // initial alignment - make_int_multiple_of(64, &size); - size += 1 * 64; - - return size; -} - - -static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims, void *raw_memory) -{ - int N = nlp_dims->N; - int nx = {{ dims.nx }}; - int nu = {{ dims.nu }}; - int nw = {{ zoro_description.nw }}; - - int ng = {{ dims.ng }}; - int nh = {{ dims.nh }}; - int nbx = {{ dims.nbx }}; - int nbu = {{ dims.nbu }}; - - int ng_e = {{ dims.ng_e }}; - int nh_e = {{ dims.nh_e }}; - int ngh_e_max = int_max(ng_e, nh_e); - int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh)); - int nbx_e = {{ dims.nbx_e }}; - - char *c_ptr = (char *) raw_memory; - custom_memory *mem = (custom_memory *) c_ptr; - c_ptr += sizeof(custom_memory); - - align_char_to(8, &c_ptr); - assign_and_advance_blasfeo_dmat_structs(N+1, &mem->uncertainty_matrix_buffer, &c_ptr); - - align_char_to(64, &c_ptr); - - for (int ii = 0; ii <= N; ii++) - { - assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->uncertainty_matrix_buffer[ii], &c_ptr); - } - // Disturbance Dynamics - assign_and_advance_blasfeo_dmat_mem(nw, nw, &mem->W_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->unc_jac_G_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->temp_GW_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->GWG_mat, &c_ptr); - // System Dynamics - assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->A_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nu, &mem->B_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ng, nx, &mem->Cg_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ng, nu, &mem->Dg_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ng_e, nx, &mem->Cg_e_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ngh_e_max, nu, &mem->dummy_Dgh_e_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nh, nx, &mem->Ch_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nh, nu, &mem->Dh_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nh_e, nx, &mem->Ch_e_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->K_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->AK_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->temp_AP_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->temp_KP_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(nu, nu, &mem->temp_KPK_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDK_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDKmP_mat, &c_ptr); - assign_and_advance_blasfeo_dmat_mem(ngh_me_max, ngh_me_max, &mem->temp_beta_mat, &c_ptr); - - assign_and_advance_double(nx*nx, &mem->d_A_mat, &c_ptr); - assign_and_advance_double(nx*nu, &mem->d_B_mat, &c_ptr); - assign_and_advance_double(ng*nx, &mem->d_Cg_mat, &c_ptr); - assign_and_advance_double(ng*nu, &mem->d_Dg_mat, &c_ptr); - assign_and_advance_double(ng_e*nx, &mem->d_Cg_e_mat, &c_ptr); - assign_and_advance_double((ng + nh)*nx, &mem->d_Cgh_mat, &c_ptr); - assign_and_advance_double((ng + nh)*nu, &mem->d_Dgh_mat, &c_ptr); - assign_and_advance_double((ng_e + nh_e)*nx, &mem->d_Cgh_e_mat, &c_ptr); - assign_and_advance_double(nx, &mem->d_state_vec, &c_ptr); - assign_and_advance_double(nbx, &mem->d_lbx, &c_ptr); - assign_and_advance_double(nbx, &mem->d_ubx, &c_ptr); - assign_and_advance_double(nbx_e, &mem->d_lbx_e, &c_ptr); - assign_and_advance_double(nbx_e, &mem->d_ubx_e, &c_ptr); - assign_and_advance_double(nbx, &mem->d_lbx_tightened, &c_ptr); - assign_and_advance_double(nbx, &mem->d_ubx_tightened, &c_ptr); - assign_and_advance_double(nbx_e, &mem->d_lbx_e_tightened, &c_ptr); - assign_and_advance_double(nbx_e, &mem->d_ubx_e_tightened, &c_ptr); - assign_and_advance_double(nbu, &mem->d_lbu, &c_ptr); - assign_and_advance_double(nbu, &mem->d_ubu, &c_ptr); - assign_and_advance_double(nbu, &mem->d_lbu_tightened, &c_ptr); - assign_and_advance_double(nbu, &mem->d_ubu_tightened, &c_ptr); - assign_and_advance_double(ng, &mem->d_lg, &c_ptr); - assign_and_advance_double(ng, &mem->d_ug, &c_ptr); - assign_and_advance_double(ng_e, &mem->d_lg_e, &c_ptr); - assign_and_advance_double(ng_e, &mem->d_ug_e, &c_ptr); - assign_and_advance_double(ng, &mem->d_lg_tightened, &c_ptr); - assign_and_advance_double(ng, &mem->d_ug_tightened, &c_ptr); - assign_and_advance_double(ng_e, &mem->d_lg_e_tightened, &c_ptr); - assign_and_advance_double(ng_e, &mem->d_ug_e_tightened, &c_ptr); - assign_and_advance_double(nh, &mem->d_lh, &c_ptr); - assign_and_advance_double(nh, &mem->d_uh, &c_ptr); - assign_and_advance_double(nh_e, &mem->d_lh_e, &c_ptr); - assign_and_advance_double(nh_e, &mem->d_uh_e, &c_ptr); - assign_and_advance_double(nh, &mem->d_lh_tightened, &c_ptr); - assign_and_advance_double(nh, &mem->d_uh_tightened, &c_ptr); - assign_and_advance_double(nh_e, &mem->d_lh_e_tightened, &c_ptr); - assign_and_advance_double(nh_e, &mem->d_uh_e_tightened, &c_ptr); - - assign_and_advance_int(nbx, &mem->idxbx, &c_ptr); - assign_and_advance_int(nbu, &mem->idxbu, &c_ptr); - assign_and_advance_int(nbx_e, &mem->idxbx_e, &c_ptr); - - assert((char *) raw_memory + custom_memory_calculate_size(nlp_config, nlp_dims) >= c_ptr); - mem->raw_memory = raw_memory; - - return mem; -} - - - -static void *custom_memory_create({{ model.name }}_solver_capsule* capsule) -{ - printf("\nin custom_memory_create_function\n"); - - ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); - ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); - acados_size_t bytes = custom_memory_calculate_size(nlp_config, nlp_dims); - - void *ptr = acados_calloc(1, bytes); - - custom_memory *custom_mem = custom_memory_assign(nlp_config, nlp_dims, ptr); - custom_mem->raw_memory = ptr; - - return custom_mem; -} - - -static void custom_val_init_function(ocp_nlp_dims *nlp_dims, ocp_nlp_in *nlp_in, ocp_nlp_solver *nlp_solver, custom_memory *custom_mem) -{ - int N = nlp_dims->N; - int nx = {{ dims.nx }}; - int nu = {{ dims.nu }}; - int nw = {{ zoro_description.nw }}; - - int ng = {{ dims.ng }}; - int nh = {{ dims.nh }}; - int nbx = {{ dims.nbx }}; - int nbu = {{ dims.nbu }}; - - int ng_e = {{ dims.ng_e }}; - int nh_e = {{ dims.nh_e }}; - int ngh_e_max = int_max(ng_e, nh_e); - int nbx_e = {{ dims.nbx_e }}; - - /* Get the state constraint bounds */ - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbx", custom_mem->idxbx); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "idxbx", custom_mem->idxbx_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbu", custom_mem->idxbu); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu); - // Get the Jacobians and the bounds of the linear constraints - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "C", custom_mem->d_Cg_mat); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "D", custom_mem->d_Dg_mat); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "C", custom_mem->d_Cg_e_mat); - blasfeo_pack_dmat(ng, nx, custom_mem->d_Cg_mat, ng, &custom_mem->Cg_mat, 0, 0); - blasfeo_pack_dmat(ng, nu, custom_mem->d_Dg_mat, ng, &custom_mem->Dg_mat, 0, 0); - blasfeo_pack_dmat(ng_e, nx, custom_mem->d_Cg_e_mat, ng_e, &custom_mem->Cg_e_mat, 0, 0); - blasfeo_dgese(ngh_e_max, nu, 0., &custom_mem->dummy_Dgh_e_mat, 0, 0); //fill with zeros - // NOTE: fixed lower and upper bounds of nonlinear constraints - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e); - - /* Initilize tightened constraints*/ - // NOTE: tightened constraints are only initialized once - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); - ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); - - /* Initialize the W matrix */ - // blasfeo_dgese(nw, nw, 0., &custom_mem->W_mat, 0, 0); -{%- for ir in range(end=zoro_description.nw) %} - {%- for ic in range(end=zoro_description.nw) %} - blasfeo_dgein1({{zoro_description.W_mat[ir][ic]}}, &custom_mem->W_mat, {{ir}}, {{ic}}); - {%- endfor %} -{%- endfor %} - -{%- for ir in range(end=dims.nx) %} - {%- for ic in range(end=zoro_description.nw) %} - blasfeo_dgein1({{zoro_description.unc_jac_G_mat[ir][ic]}}, &custom_mem->unc_jac_G_mat, {{ir}}, {{ic}}); - {%- endfor %} -{%- endfor %} - - // NOTE: if G is changing this is not in init! - // temp_GW_mat = unc_jac_G_mat * W_mat - blasfeo_dgemm_nn(nx, nw, nw, 1.0, &custom_mem->unc_jac_G_mat, 0, 0, - &custom_mem->W_mat, 0, 0, 0.0, - &custom_mem->temp_GW_mat, 0, 0, &custom_mem->temp_GW_mat, 0, 0); - // GWG_mat = temp_GW_mat * unc_jac_G_mat^T - blasfeo_dgemm_nt(nx, nx, nw, 1.0, &custom_mem->temp_GW_mat, 0, 0, - &custom_mem->unc_jac_G_mat, 0, 0, 0.0, - &custom_mem->GWG_mat, 0, 0, &custom_mem->GWG_mat, 0, 0); - - /* Initialize the uncertainty_matrix_buffer[0] */ -{%- for ir in range(end=dims.nx) %} - {%- for ic in range(end=dims.nx) %} - blasfeo_dgein1({{zoro_description.P0_mat[ir][ic]}}, &custom_mem->uncertainty_matrix_buffer[0], {{ir}}, {{ic}}); - {%- endfor %} -{%- endfor %} - - /* Initialize the feedback gain matrix */ -{%- for ir in range(end=dims.nu) %} - {%- for ic in range(end=dims.nx) %} - blasfeo_dgein1({{zoro_description.fdbk_K_mat[ir][ic]}}, &custom_mem->K_mat, {{ir}}, {{ic}}); - {%- endfor %} -{%- endfor %} -} - - -int custom_update_init_function({{ model.name }}_solver_capsule* capsule) -{ - capsule->custom_update_memory = custom_memory_create(capsule); - ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); - - ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); - ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule); - custom_val_init_function(nlp_dims, nlp_in, nlp_solver, capsule->custom_update_memory); - return 1; -} - -static void compute_gh_beta(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* C_mat, - struct blasfeo_dmat* D_mat, struct blasfeo_dmat* CaDK_mat, - struct blasfeo_dmat* CaDKmP_mat, struct blasfeo_dmat* beta_mat, - struct blasfeo_dmat* P_mat, - int n_cstr, int nx, int nu) -{ - // (C+DK)@P@(C^T+K^TD^T) - // CaDK_mat = C_mat + D_mat @ K_mat - blasfeo_dgemm_nn(n_cstr, nx, nu, 1.0, D_mat, 0, 0, - K_mat, 0, 0, 1.0, - C_mat, 0, 0, CaDK_mat, 0, 0); - // CaDKmP_mat = CaDK_mat @ P_mat - blasfeo_dgemm_nn(n_cstr, nx, nx, 1.0, CaDK_mat, 0, 0, - P_mat, 0, 0, 0.0, - CaDKmP_mat, 0, 0, CaDKmP_mat, 0, 0); - // beta_mat = CaDKmP_mat @ CaDK_mat^T - blasfeo_dgemm_nt(n_cstr, n_cstr, nx, 1.0, CaDKmP_mat, 0, 0, - CaDK_mat, 0, 0, 0.0, - beta_mat, 0, 0, beta_mat, 0, 0); -} - -static void compute_KPK(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* temp_KP_mat, - struct blasfeo_dmat* temp_KPK_mat, struct blasfeo_dmat* P_mat, - int nx, int nu) -{ - // K @ P_k @ K^T - // temp_KP_mat = K_mat @ P_mat - blasfeo_dgemm_nn(nu, nx, nx, 1.0, K_mat, 0, 0, - P_mat, 0, 0, 0.0, - temp_KP_mat, 0, 0, temp_KP_mat, 0, 0); - // temp_KPK_mat = temp_KP_mat @ K_mat^T - blasfeo_dgemm_nt(nu, nu, nx, 1.0, temp_KP_mat, 0, 0, - K_mat, 0, 0, 0.0, - temp_KPK_mat, 0, 0, temp_KPK_mat, 0, 0); -} - -static void compute_next_P_matrix(struct blasfeo_dmat* P_mat, struct blasfeo_dmat* P_next_mat, - struct blasfeo_dmat* A_mat, struct blasfeo_dmat* B_mat, - struct blasfeo_dmat* K_mat, struct blasfeo_dmat* W_mat, - struct blasfeo_dmat* AK_mat, struct blasfeo_dmat* temp_AP_mat, int nx, int nu) -{ - // AK_mat = -B@K + A - blasfeo_dgemm_nn(nx, nx, nu, -1.0, B_mat, 0, 0, K_mat, 0, 0, - 1.0, A_mat, 0, 0, AK_mat, 0, 0); - // temp_AP_mat = AK_mat @ P_k - blasfeo_dgemm_nn(nx, nx, nx, 1.0, AK_mat, 0, 0, - P_mat, 0, 0, 0.0, - temp_AP_mat, 0, 0, temp_AP_mat, 0, 0); - // P_{k+1} = temp_AP_mat @ AK_mat^T + GWG_mat - blasfeo_dgemm_nt(nx, nx, nx, 1.0, temp_AP_mat, 0, 0, - AK_mat, 0, 0, 1.0, - W_mat, 0, 0, P_next_mat, 0, 0); -} - -static void reset_P0_matrix(ocp_nlp_dims *nlp_dims, struct blasfeo_dmat* P_mat, double* data) -{ - int nx = nlp_dims->nx[0]; - blasfeo_pack_dmat(nx, nx, data, nx, P_mat, 0, 0); -} - -static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, custom_memory *custom_mem) -{ - ocp_nlp_config *nlp_config = solver->config; - ocp_nlp_dims *nlp_dims = solver->dims; - - int N = nlp_dims->N; - int nx = nlp_dims->nx[0]; - int nu = nlp_dims->nu[0]; - int nx_sqr = nx*nx; - int nbx = {{ dims.nbx }}; - int nbu = {{ dims.nbu }}; - int ng = {{ dims.ng }}; - int nh = {{ dims.nh }}; - int ng_e = {{ dims.ng_e }}; - int nh_e = {{ dims.nh_e }}; - int nbx_e = {{ dims.nbx_e }}; - double backoff_scaling_gamma = {{ zoro_description.backoff_scaling_gamma }}; - - // First Stage - // NOTE: lbx_0 and ubx_0 should not be tightened. - // NOTE: lg_0 and ug_0 are not tightened. - // NOTE: lh_0 and uh_0 are not tightened. -{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %} - compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat, - &custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[0]), nx, nu); - -{%- if zoro_description.nlbu_t > 0 %} - // backoff lbu - {%- for it in zoro_description.idx_lbu_t %} - custom_mem->d_lbu_tightened[{{it}}] - = custom_mem->d_lbu[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, - custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened); -{%- endif %} -{%- if zoro_description.nubu_t > 0 %} - // backoff ubu - {%- for it in zoro_description.idx_ubu_t %} - custom_mem->d_ubu_tightened[{{it}}] - = custom_mem->d_ubu[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, - custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened); -{%- endif %} -{%- endif %} - // Middle Stages - // constraint tightening: for next stage based on dynamics of ii stage - // P[ii+1] = (A-B@K) @ P[ii] @ (A-B@K).T + G@W@G.T - for (int ii = 0; ii < N-1; ii++) - { - // get and pack: A, B - ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "A", custom_mem->d_A_mat); - blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0); - ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "B", custom_mem->d_B_mat); - blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0); - - compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[ii]), - &(custom_mem->uncertainty_matrix_buffer[ii+1]), - &custom_mem->A_mat, &custom_mem->B_mat, - &custom_mem->K_mat, &custom_mem->GWG_mat, - &custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu); - - // state constraints -{%- if zoro_description.nlbx_t + zoro_description.nubx_t> 0 %} - {%- if zoro_description.nlbx_t > 0 %} - // lbx - {%- for it in zoro_description.idx_lbx_t %} - custom_mem->d_lbx_tightened[{{it}}] - = custom_mem->d_lbx[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], - custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened); - {%- endif %} - {% if zoro_description.nubx_t > 0 %} - // ubx - {%- for it in zoro_description.idx_ubx_t %} - custom_mem->d_ubx_tightened[{{it}}] = custom_mem->d_ubx[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], - custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened); - {%- endif %} -{%- endif %} - -{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %} - // input constraints - compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat, - &custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[ii+1]), nx, nu); - - {%- if zoro_description.nlbu_t > 0 %} - {%- for it in zoro_description.idx_lbu_t %} - custom_mem->d_lbu_tightened[{{it}}] = custom_mem->d_lbu[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, - custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened); - {%- endif %} - {%- if zoro_description.nubu_t > 0 %} - {%- for it in zoro_description.idx_ubu_t %} - custom_mem->d_ubu_tightened[{{it}}] = custom_mem->d_ubu[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, - custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened); - {%- endif %} -{%- endif %} - -{%- if zoro_description.nlg_t + zoro_description.nug_t > 0 %} - // Linear constraints: g - compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat, - &custom_mem->Dg_mat, &custom_mem->temp_CaDK_mat, - &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, - &custom_mem->uncertainty_matrix_buffer[ii+1], ng, nx, nu); - - {%- if zoro_description.nlg_t > 0 %} - {%- for it in zoro_description.idx_lg_t %} - custom_mem->d_lg_tightened[{{it}}] - = custom_mem->d_lg[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened); - {%- endif %} - {%- if zoro_description.nug_t > 0 %} - {%- for it in zoro_description.idx_ug_t %} - custom_mem->d_ug_tightened[{{it}}] - = custom_mem->d_ug[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened); - {%- endif %} -{%- endif %} - - -{%- if zoro_description.nlh_t + zoro_description.nuh_t > 0 %} - // nonlinear constraints: h - // Get C_{k+1} and D_{k+1} - ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "C", custom_mem->d_Cgh_mat); - ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "D", custom_mem->d_Dgh_mat); - // NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints - blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0); - blasfeo_pack_dmat(nh, nu, custom_mem->d_Dgh_mat+ng, ng+nh, &custom_mem->Dh_mat, 0, 0); - - compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat, - &custom_mem->Dh_mat, &custom_mem->temp_CaDK_mat, - &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, - &custom_mem->uncertainty_matrix_buffer[ii+1], nh, nx, nu); - - {%- if zoro_description.nlh_t > 0 %} - {%- for it in zoro_description.idx_lh_t %} - custom_mem->d_lh_tightened[{{it}}] - = custom_mem->d_lh[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened); - {%- endif %} - {%- if zoro_description.nuh_t > 0 %} - {%- for it in zoro_description.idx_uh_t %} - custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened); - {%- endif %} -{%- endif %} - } - - // Last stage - // get and pack: A, B - ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "A", custom_mem->d_A_mat); - blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0); - ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "B", custom_mem->d_B_mat); - blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0); - // AK_mat = -B*K + A - compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[N-1]), - &(custom_mem->uncertainty_matrix_buffer[N]), - &custom_mem->A_mat, &custom_mem->B_mat, - &custom_mem->K_mat, &custom_mem->GWG_mat, - &custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu); - - // state constraints nlbx_e_t -{%- if zoro_description.nlbx_e_t + zoro_description.nubx_e_t> 0 %} -{%- if zoro_description.nlbx_e_t > 0 %} - // lbx_e - {%- for it in zoro_description.idx_lbx_e_t %} - custom_mem->d_lbx_e_tightened[{{it}}] - = custom_mem->d_lbx_e[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], - custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); -{%- endif %} -{% if zoro_description.nubx_e_t > 0 %} - // ubx_e - {%- for it in zoro_description.idx_ubx_e_t %} - custom_mem->d_ubx_e_tightened[{{it}}] = custom_mem->d_ubx_e[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], - custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); -{%- endif %} -{%- endif %} - -{%- if zoro_description.nlg_e_t + zoro_description.nug_e_t > 0 %} - // Linear constraints: g - compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat, - &custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat, - &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, - &custom_mem->uncertainty_matrix_buffer[N], ng, nx, nu); - -{%- if zoro_description.nlg_e_t > 0 %} - {%- for it in zoro_description.idx_lg_e_t %} - custom_mem->d_lg_e_tightened[{{it}}] - = custom_mem->d_lg_e[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); -{%- endif %} -{%- if zoro_description.nug_e_t > 0 %} - {%- for it in zoro_description.idx_ug_e_t %} - custom_mem->d_ug_e_tightened[{{it}}] - = custom_mem->d_ug_e[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); -{%- endif %} -{%- endif %} - - -{%- if zoro_description.nlh_e_t + zoro_description.nuh_e_t > 0 %} - // nonlinear constraints: h - // Get C_{k+1} and D_{k+1} - ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, N, "C", custom_mem->d_Cgh_mat); - // NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints - blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0); - - compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat, - &custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat, - &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, - &custom_mem->uncertainty_matrix_buffer[N], nh, nx, nu); - - {%- if zoro_description.nlh_e_t > 0 %} - {%- for it in zoro_description.idx_lh_e_t %} - custom_mem->d_lh_e_tightened[{{it}}] - = custom_mem->d_lh_e[{{it}}] - + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); - {%- endif %} - {%- if zoro_description.nuh_e_t > 0 %} - {%- for it in zoro_description.idx_uh_e_t %} - custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}] - - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); - {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); - {%- endif %} -{%- endif %} - -} - - -int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len) -{ - custom_memory *custom_mem = (custom_memory *) capsule->custom_update_memory; - ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); - ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); - ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); - ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule); - ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule); - void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(capsule); - - if (data_len > 0) - { - reset_P0_matrix(nlp_dims, &custom_mem->uncertainty_matrix_buffer[0], data); - } - uncertainty_propagate_and_update(nlp_solver, nlp_in, nlp_out, custom_mem); - - return 1; -} - - -int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule) -{ - custom_memory *mem = capsule->custom_update_memory; - - free(mem->raw_memory); - return 1; -} - -// useful prints for debugging - -/* -printf("A_mat:\n"); -blasfeo_print_exp_dmat(nx, nx, &custom_mem->A_mat, 0, 0); -printf("B_mat:\n"); -blasfeo_print_exp_dmat(nx, nu, &custom_mem->B_mat, 0, 0); -printf("K_mat:\n"); -blasfeo_print_exp_dmat(nu, nx, &custom_mem->K_mat, 0, 0); -printf("AK_mat:\n"); -blasfeo_print_exp_dmat(nx, nx, &custom_mem->AK_mat, 0, 0); -printf("temp_AP_mat:\n"); -blasfeo_print_exp_dmat(nx, nx, &custom_mem->temp_AP_mat, 0, 0); -printf("W_mat:\n"); -blasfeo_print_exp_dmat(nx, nx, &custom_mem->W_mat, 0, 0); -printf("P_k+1:\n"); -blasfeo_print_exp_dmat(nx, nx, &(custom_mem->uncertainty_matrix_buffer[ii+1]), 0, 0);*/ \ No newline at end of file diff --git a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h deleted file mode 100644 index 9611ea210c9578..00000000000000 --- a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#include "acados_solver_{{ model.name }}.h" - -// Called at the end of solver creation. -// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory. -int custom_update_init_function({{ model.name }}_solver_capsule* capsule); - - -// Custom update function that can be called between solver calls -int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len); - - -// Called just before destroying the solver. -// Responsible to free allocated memory, stored at capsule->custom_update_memory. -int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule); diff --git a/third_party/acados/acados_template/gnsf/check_reformulation.py b/third_party/acados/acados_template/gnsf/check_reformulation.py deleted file mode 100644 index 2bdfbbc3363a61..00000000000000 --- a/third_party/acados/acados_template/gnsf/check_reformulation.py +++ /dev/null @@ -1,216 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -from acados_template.utils import casadi_length -from casadi import * -import numpy as np - - -def check_reformulation(model, gnsf, print_info): - - ## Description: - # this function takes the implicit ODE/ index-1 DAE and a gnsf structure - # to evaluate both models at num_eval random points x0, x0dot, z0, u0 - # if for all points the relative error is <= TOL, the function will return:: - # 1, otherwise it will give an error. - - TOL = 1e-14 - num_eval = 10 - - # get dimensions - nx = gnsf["nx"] - nu = gnsf["nu"] - nz = gnsf["nz"] - nx1 = gnsf["nx1"] - nx2 = gnsf["nx2"] - nz1 = gnsf["nz1"] - nz2 = gnsf["nz2"] - n_out = gnsf["n_out"] - - # get model matrices - A = gnsf["A"] - B = gnsf["B"] - C = gnsf["C"] - E = gnsf["E"] - c = gnsf["c"] - - L_x = gnsf["L_x"] - L_xdot = gnsf["L_xdot"] - L_z = gnsf["L_z"] - L_u = gnsf["L_u"] - - A_LO = gnsf["A_LO"] - E_LO = gnsf["E_LO"] - B_LO = gnsf["B_LO"] - c_LO = gnsf["c_LO"] - - I_x1 = range(nx1) - I_x2 = range(nx1, nx) - - I_z1 = range(nz1) - I_z2 = range(nz1, nz) - - idx_perm_f = gnsf["idx_perm_f"] - - # get casadi variables - x = gnsf["x"] - xdot = gnsf["xdot"] - z = gnsf["z"] - u = gnsf["u"] - y = gnsf["y"] - uhat = gnsf["uhat"] - p = gnsf["p"] - - # create functions - impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr]) - phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]]) - f_lo_fun = Function( - "f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]] - ) - - # print(gnsf) - # print(gnsf["n_out"]) - - for i_check in range(num_eval): - # generate random values - x0 = np.random.rand(nx, 1) - x0dot = np.random.rand(nx, 1) - z0 = np.random.rand(nz, 1) - u0 = np.random.rand(nu, 1) - - if gnsf["ny"] > 0: - y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1] - else: - y0 = [] - if gnsf["nuhat"] > 0: - uhat0 = L_u @ u0 - else: - uhat0 = [] - - # eval functions - p0 = np.random.rand(gnsf["np"], 1) - f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full() - phi_val = phi_fun(y0, uhat0, p0) - f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0) - - f_impl_val = f_impl_val[idx_perm_f] - # eval gnsf - if n_out > 0: - C_phi = C @ phi_val - else: - C_phi = np.zeros((nx1 + nz1, 1)) - try: - gnsf_val1 = ( - A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1]) - ) - # gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1])) - except: - import pdb - - pdb.set_trace() - - if nx2 > 0: # eval LOS: - gnsf_val2 = ( - A_LO @ x0[I_x2] - + B_LO @ u0 - + c_LO - + f_lo_val - - E_LO @ vertcat(x0dot[I_x2], z0[I_z2]) - ) - gnsf_val = vertcat(gnsf_val1, gnsf_val2).full() - else: - gnsf_val = gnsf_val1.full() - # compute error and check - rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val) - - if rel_error > TOL: - print("transcription failed rel_error > TOL") - print("you are in debug mode now: import pdb; pdb.set_trace()") - abs_error = gnsf_val - f_impl_val - # T = table(f_impl_val, gnsf_val, abs_error) - # print(T) - print("abs_error:", abs_error) - # error('transcription failed rel_error > TOL') - # check = 0 - import pdb - - pdb.set_trace() - if print_info: - print(" ") - print("model reformulation checked: relative error <= TOL = ", str(TOL)) - print(" ") - check = 1 - ## helpful for debugging: - # # use in calling function and compare - # # compare f_impl(i) with gnsf_val1(i) - # - - # nx = gnsf['nx'] - # nu = gnsf['nu'] - # nz = gnsf['nz'] - # nx1 = gnsf['nx1'] - # nx2 = gnsf['nx2'] - # - # A = gnsf['A'] - # B = gnsf['B'] - # C = gnsf['C'] - # E = gnsf['E'] - # c = gnsf['c'] - # - # L_x = gnsf['L_x'] - # L_z = gnsf['L_z'] - # L_xdot = gnsf['L_xdot'] - # L_u = gnsf['L_u'] - # - # A_LO = gnsf['A_LO'] - # - # x0 = rand(nx, 1) - # x0dot = rand(nx, 1) - # z0 = rand(nz, 1) - # u0 = rand(nu, 1) - # I_x1 = range(nx1) - # I_x2 = nx1+range(nx) - # - # y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0 - # uhat0 = L_u @ u0 - # - # gnsf_val1 = (A @ x[I_x1] + B @ u + # C @ phi_current + c) - E @ [xdot[I_x1] z] - # gnsf_val1 = gnsf_val1.simplify() - # - # # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2] - # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2] - # - # - # gnsf_val = [gnsf_val1 gnsf_val2] - # gnsf_val = gnsf_val.simplify() - # dyn_expr_f = dyn_expr_f.simplify() - # import pdb; pdb.set_trace() - - return check diff --git a/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py b/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py deleted file mode 100644 index ebf1f373a44665..00000000000000 --- a/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py +++ /dev/null @@ -1,278 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -from casadi import * -from .check_reformulation import check_reformulation -from .determine_input_nonlinearity_function import determine_input_nonlinearity_function -from ..utils import casadi_length, print_casadi_expression - - -def detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info): - - ## Description - # this function takes a gnsf structure with trivial model matrices (A, B, - # E, c are zeros, and C is eye). - # It detects all affine linear terms and sets up an equivalent model in the - # GNSF structure, where all affine linear terms are modeled through the - # matrices A, B, E, c and the linear output system (LOS) is empty. - # NOTE: model is just taken as an argument to check equivalence of the - # models within the function. - - model = acados_ocp.model - if print_info: - print(" ") - print("====================================================================") - print(" ") - print("============ Detect affine-linear dependencies ==================") - print(" ") - print("====================================================================") - print(" ") - # symbolics - x = gnsf["x"] - xdot = gnsf["xdot"] - u = gnsf["u"] - z = gnsf["z"] - - # dimensions - nx = gnsf["nx"] - nu = gnsf["nu"] - nz = gnsf["nz"] - - ny_old = gnsf["ny"] - nuhat_old = gnsf["nuhat"] - - ## Represent all affine dependencies through the model matrices A, B, E, c - ## determine A - n_nodes_current = n_nodes(gnsf["phi_expr"]) - - for ii in range(casadi_length(gnsf["phi_expr"])): - fii = gnsf["phi_expr"][ii] - for ix in range(nx): - var = x[ix] - varname = var.name - # symbolic jacobian of fii w.r.t. xi - jac_fii_xi = jacobian(fii, var) - if jac_fii_xi.is_constant(): - # jacobian value - jac_fii_xi_fun = Function("jac_fii_xi_fun", [x[1]], [jac_fii_xi]) - # x[1] as input just to have a scalar input and call the function as follows: - gnsf["A"][ii, ix] = jac_fii_xi_fun(0).full() - else: - gnsf["A"][ii, ix] = 0 - if print_info: - print( - "phi(", - str(ii), - ") is nonlinear in x(", - str(ix), - ") = ", - varname, - ) - print(fii) - print("-----------------------------------------------------") - f_next = gnsf["phi_expr"] - gnsf["A"] @ x - f_next = simplify(f_next) - n_nodes_next = n_nodes(f_next) - - if print_info: - print("\n") - print(f"determined matrix A:") - print(gnsf["A"]) - print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") - # assert(n_nodes_current >= n_nodes_next,'n_nodes_current >= n_nodes_next FAILED') - gnsf["phi_expr"] = f_next - - check_reformulation(model, gnsf, print_info) - - ## determine B - n_nodes_current = n_nodes(gnsf["phi_expr"]) - - for ii in range(casadi_length(gnsf["phi_expr"])): - fii = gnsf["phi_expr"][ii] - for iu in range(nu): - var = u[iu] - varname = var.name - # symbolic jacobian of fii w.r.t. ui - jac_fii_ui = jacobian(fii, var) - if jac_fii_ui.is_constant(): # i.e. hessian is structural zero: - # jacobian value - jac_fii_ui_fun = Function("jac_fii_ui_fun", [x[1]], [jac_fii_ui]) - gnsf["B"][ii, iu] = jac_fii_ui_fun(0).full() - else: - gnsf["B"][ii, iu] = 0 - if print_info: - print(f"phi({ii}) is nonlinear in u(", str(iu), ") = ", varname) - print(fii) - print("-----------------------------------------------------") - f_next = gnsf["phi_expr"] - gnsf["B"] @ u - f_next = simplify(f_next) - n_nodes_next = n_nodes(f_next) - - if print_info: - print("\n") - print(f"determined matrix B:") - print(gnsf["B"]) - print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") - - gnsf["phi_expr"] = f_next - - check_reformulation(model, gnsf, print_info) - - ## determine E - n_nodes_current = n_nodes(gnsf["phi_expr"]) - k = vertcat(xdot, z) - - for ii in range(casadi_length(gnsf["phi_expr"])): - fii = gnsf["phi_expr"][ii] - for ik in range(casadi_length(k)): - # symbolic jacobian of fii w.r.t. ui - var = k[ik] - varname = var.name - jac_fii_ki = jacobian(fii, var) - if jac_fii_ki.is_constant(): - # jacobian value - jac_fii_ki_fun = Function("jac_fii_ki_fun", [x[1]], [jac_fii_ki]) - gnsf["E"][ii, ik] = -jac_fii_ki_fun(0).full() - else: - gnsf["E"][ii, ik] = 0 - if print_info: - print(f"phi( {ii}) is nonlinear in xdot_z({ik}) = ", varname) - print(fii) - print("-----------------------------------------------------") - f_next = gnsf["phi_expr"] + gnsf["E"] @ k - f_next = simplify(f_next) - n_nodes_next = n_nodes(f_next) - - if print_info: - print("\n") - print(f"determined matrix E:") - print(gnsf["E"]) - print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") - - gnsf["phi_expr"] = f_next - check_reformulation(model, gnsf, print_info) - - ## determine constant term c - - n_nodes_current = n_nodes(gnsf["phi_expr"]) - for ii in range(casadi_length(gnsf["phi_expr"])): - fii = gnsf["phi_expr"][ii] - if fii.is_constant(): - # function value goes into c - fii_fun = Function("fii_fun", [x[1]], [fii]) - gnsf["c"][ii] = fii_fun(0).full() - else: - gnsf["c"][ii] = 0 - if print_info: - print(f"phi(", str(ii), ") is NOT constant") - print(fii) - print("-----------------------------------------------------") - gnsf["phi_expr"] = gnsf["phi_expr"] - gnsf["c"] - gnsf["phi_expr"] = simplify(gnsf["phi_expr"]) - n_nodes_next = n_nodes(gnsf["phi_expr"]) - - if print_info: - print("\n") - print(f"determined vector c:") - print(gnsf["c"]) - print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") - - check_reformulation(model, gnsf, print_info) - - ## determine nonlinearity & corresponding matrix C - ## Reduce dimension of phi - n_nodes_current = n_nodes(gnsf["phi_expr"]) - ind_non_zero = [] - for ii in range(casadi_length(gnsf["phi_expr"])): - fii = gnsf["phi_expr"][ii] - fii = simplify(fii) - if not fii.is_zero(): - ind_non_zero = list(set.union(set(ind_non_zero), set([ii]))) - gnsf["phi_expr"] = gnsf["phi_expr"][ind_non_zero] - - # C - gnsf["C"] = np.zeros((nx + nz, len(ind_non_zero))) - for ii in range(len(ind_non_zero)): - gnsf["C"][ind_non_zero[ii], ii] = 1 - gnsf = determine_input_nonlinearity_function(gnsf) - n_nodes_next = n_nodes(gnsf["phi_expr"]) - - if print_info: - print(" ") - print("determined matrix C:") - print(gnsf["C"]) - print( - "---------------------------------------------------------------------------------" - ) - print( - "------------- Success: Affine linear terms detected -----------------------------" - ) - print( - "---------------------------------------------------------------------------------" - ) - print( - f'reduced nonlinearity dimension n_out from {nx+nz} to {gnsf["n_out"]}' - ) - print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") - print(" ") - print("phi now reads as:") - print_casadi_expression(gnsf["phi_expr"]) - - ## determine input of nonlinearity function - check_reformulation(model, gnsf, print_info) - - gnsf["ny"] = casadi_length(gnsf["y"]) - gnsf["nuhat"] = casadi_length(gnsf["uhat"]) - - if print_info: - print( - "-----------------------------------------------------------------------------------" - ) - print(" ") - print( - f"reduced input ny of phi from ", - str(ny_old), - " to ", - str(gnsf["ny"]), - ) - print( - f"reduced input nuhat of phi from ", - str(nuhat_old), - " to ", - str(gnsf["nuhat"]), - ) - print( - "-----------------------------------------------------------------------------------" - ) - - # if print_info: - # print(f"gnsf: {gnsf}") - - return gnsf diff --git a/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py b/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py deleted file mode 100644 index 24ffe643b8ac2a..00000000000000 --- a/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py +++ /dev/null @@ -1,240 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com - -from casadi import Function, jacobian, SX, vertcat, horzcat - -from .determine_trivial_gnsf_transcription import determine_trivial_gnsf_transcription -from .detect_affine_terms_reduce_nonlinearity import ( - detect_affine_terms_reduce_nonlinearity, -) -from .reformulate_with_LOS import reformulate_with_LOS -from .reformulate_with_invertible_E_mat import reformulate_with_invertible_E_mat -from .structure_detection_print_summary import structure_detection_print_summary -from .check_reformulation import check_reformulation - - -def detect_gnsf_structure(acados_ocp, transcribe_opts=None): - - ## Description - # This function takes a CasADi implicit ODE or index-1 DAE model "model" - # consisting of a CasADi expression f_impl in the symbolic CasADi - # variables x, xdot, u, z, (and possibly parameters p), which are also part - # of the model, as well as a model name. - # It will create a struct "gnsf" containing all information needed to use - # it with the gnsf integrator in acados. - # Additionally it will create the struct "reordered_model" which contains - # the permuted state vector and permuted f_impl, in which additionally some - # functions, which were made part of the linear output system of the gnsf, - # have changed signs. - - # Options: transcribe_opts is a Matlab struct consisting of booleans: - # print_info: if extensive information on how the model is processed - # is printed to the console. - # generate_gnsf_model: if the neccessary C functions to simulate the gnsf - # model with the acados implementation of the GNSF exploiting - # integrator should be generated. - # generate_gnsf_model: if the neccessary C functions to simulate the - # reordered model with the acados implementation of the IRK - # integrator should be generated. - # check_E_invertibility: if the transcription method should check if the - # assumption that the main blocks of the matrix gnsf.E are invertible - # holds. If not, the method will try to reformulate the gnsf model - # with a different model, such that the assumption holds. - - # acados_root_dir = getenv('ACADOS_INSTALL_DIR') - - ## load transcribe_opts - if transcribe_opts is None: - print("WARNING: GNSF structure detection called without transcribe_opts") - print(" using default settings") - print("") - transcribe_opts = dict() - - if "print_info" in transcribe_opts: - print_info = transcribe_opts["print_info"] - else: - print_info = 1 - print("print_info option was not set - default is true") - - if "detect_LOS" in transcribe_opts: - detect_LOS = transcribe_opts["detect_LOS"] - else: - detect_LOS = 1 - if print_info: - print("detect_LOS option was not set - default is true") - - if "check_E_invertibility" in transcribe_opts: - check_E_invertibility = transcribe_opts["check_E_invertibility"] - else: - check_E_invertibility = 1 - if print_info: - print("check_E_invertibility option was not set - default is true") - - ## Reformulate implicit index-1 DAE into GNSF form - # (Generalized nonlinear static feedback) - gnsf = determine_trivial_gnsf_transcription(acados_ocp, print_info) - gnsf = detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info) - - if detect_LOS: - gnsf = reformulate_with_LOS(acados_ocp, gnsf, print_info) - - if check_E_invertibility: - gnsf = reformulate_with_invertible_E_mat(gnsf, acados_ocp, print_info) - - # detect purely linear model - if gnsf["nx1"] == 0 and gnsf["nz1"] == 0 and gnsf["nontrivial_f_LO"] == 0: - gnsf["purely_linear"] = 1 - else: - gnsf["purely_linear"] = 0 - - structure_detection_print_summary(gnsf, acados_ocp) - check_reformulation(acados_ocp.model, gnsf, print_info) - - ## copy relevant fields from gnsf to model - acados_ocp.model.get_matrices_fun = Function() - dummy = acados_ocp.model.x[0] - model_name = acados_ocp.model.name - - get_matrices_fun = Function( - f"{model_name}_gnsf_get_matrices_fun", - [dummy], - [ - gnsf["A"], - gnsf["B"], - gnsf["C"], - gnsf["E"], - gnsf["L_x"], - gnsf["L_xdot"], - gnsf["L_z"], - gnsf["L_u"], - gnsf["A_LO"], - gnsf["c"], - gnsf["E_LO"], - gnsf["B_LO"], - gnsf["nontrivial_f_LO"], - gnsf["purely_linear"], - gnsf["ipiv_x"] + 1, - gnsf["ipiv_z"] + 1, - gnsf["c_LO"], - ], - ) - - phi = gnsf["phi_expr"] - y = gnsf["y"] - uhat = gnsf["uhat"] - p = gnsf["p"] - - jac_phi_y = jacobian(phi, y) - jac_phi_uhat = jacobian(phi, uhat) - - phi_fun = Function(f"{model_name}_gnsf_phi_fun", [y, uhat, p], [phi]) - acados_ocp.model.phi_fun = phi_fun - acados_ocp.model.phi_fun_jac_y = Function( - f"{model_name}_gnsf_phi_fun_jac_y", [y, uhat, p], [phi, jac_phi_y] - ) - acados_ocp.model.phi_jac_y_uhat = Function( - f"{model_name}_gnsf_phi_jac_y_uhat", [y, uhat, p], [jac_phi_y, jac_phi_uhat] - ) - - x1 = acados_ocp.model.x[gnsf["idx_perm_x"][: gnsf["nx1"]]] - x1dot = acados_ocp.model.xdot[gnsf["idx_perm_x"][: gnsf["nx1"]]] - if gnsf["nz1"] > 0: - z1 = acados_ocp.model.z[gnsf["idx_perm_z"][: gnsf["nz1"]]] - else: - z1 = SX.sym("z1", 0, 0) - f_lo = gnsf["f_lo_expr"] - u = acados_ocp.model.u - acados_ocp.model.f_lo_fun_jac_x1k1uz = Function( - f"{model_name}_gnsf_f_lo_fun_jac_x1k1uz", - [x1, x1dot, z1, u, p], - [ - f_lo, - horzcat( - jacobian(f_lo, x1), - jacobian(f_lo, x1dot), - jacobian(f_lo, u), - jacobian(f_lo, z1), - ), - ], - ) - - acados_ocp.model.get_matrices_fun = get_matrices_fun - - size_gnsf_A = gnsf["A"].shape - acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1] - acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] - acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1)) - acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0)) - acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0)) - - # # dim - # model['dim_gnsf_nx1'] = gnsf['nx1'] - # model['dim_gnsf_nx2'] = gnsf['nx2'] - # model['dim_gnsf_nz1'] = gnsf['nz1'] - # model['dim_gnsf_nz2'] = gnsf['nz2'] - # model['dim_gnsf_nuhat'] = gnsf['nuhat'] - # model['dim_gnsf_ny'] = gnsf['ny'] - # model['dim_gnsf_nout'] = gnsf['n_out'] - - # # sym - # model['sym_gnsf_y'] = gnsf['y'] - # model['sym_gnsf_uhat'] = gnsf['uhat'] - - # # data - # model['dyn_gnsf_A'] = gnsf['A'] - # model['dyn_gnsf_A_LO'] = gnsf['A_LO'] - # model['dyn_gnsf_B'] = gnsf['B'] - # model['dyn_gnsf_B_LO'] = gnsf['B_LO'] - # model['dyn_gnsf_E'] = gnsf['E'] - # model['dyn_gnsf_E_LO'] = gnsf['E_LO'] - # model['dyn_gnsf_C'] = gnsf['C'] - # model['dyn_gnsf_c'] = gnsf['c'] - # model['dyn_gnsf_c_LO'] = gnsf['c_LO'] - # model['dyn_gnsf_L_x'] = gnsf['L_x'] - # model['dyn_gnsf_L_xdot'] = gnsf['L_xdot'] - # model['dyn_gnsf_L_z'] = gnsf['L_z'] - # model['dyn_gnsf_L_u'] = gnsf['L_u'] - # model['dyn_gnsf_idx_perm_x'] = gnsf['idx_perm_x'] - # model['dyn_gnsf_ipiv_x'] = gnsf['ipiv_x'] - # model['dyn_gnsf_idx_perm_z'] = gnsf['idx_perm_z'] - # model['dyn_gnsf_ipiv_z'] = gnsf['ipiv_z'] - # model['dyn_gnsf_idx_perm_f'] = gnsf['idx_perm_f'] - # model['dyn_gnsf_ipiv_f'] = gnsf['ipiv_f'] - - # # flags - # model['dyn_gnsf_nontrivial_f_LO'] = gnsf['nontrivial_f_LO'] - # model['dyn_gnsf_purely_linear'] = gnsf['purely_linear'] - - # # casadi expr - # model['dyn_gnsf_expr_phi'] = gnsf['phi_expr'] - # model['dyn_gnsf_expr_f_lo'] = gnsf['f_lo_expr'] - - return acados_ocp diff --git a/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py b/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py deleted file mode 100644 index 94aa001c79528d..00000000000000 --- a/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py +++ /dev/null @@ -1,110 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com - -from casadi import * -from ..utils import casadi_length, is_empty - - -def determine_input_nonlinearity_function(gnsf): - - ## Description - # this function takes a structure gnsf and updates the matrices L_x, - # L_xdot, L_z, L_u and CasADi vectors y, uhat of this structure as follows: - - # given a CasADi expression phi_expr, which may depend on the variables - # (x1, x1dot, z, u), this function determines a vector y (uhat) consisting - # of all components of (x1, x1dot, z) (respectively u) that enter phi_expr. - # Additionally matrices L_x, L_xdot, L_z, L_u are determined such that - # y = L_x * x + L_xdot * xdot + L_z * z - # uhat = L_u * u - # Furthermore the dimensions ny, nuhat, n_out are updated - - ## y - y = SX.sym('y', 0, 0) - # components of x1 - for ii in range(gnsf["nx1"]): - if which_depends(gnsf["phi_expr"], gnsf["x"][ii])[0]: - y = vertcat(y, gnsf["x"][ii]) - # else: - # x[ii] is not part of y - # components of x1dot - for ii in range(gnsf["nx1"]): - if which_depends(gnsf["phi_expr"], gnsf["xdot"][ii])[0]: - print(gnsf["phi_expr"], "depends on", gnsf["xdot"][ii]) - y = vertcat(y, gnsf["xdot"][ii]) - # else: - # xdot[ii] is not part of y - # components of z - for ii in range(gnsf["nz1"]): - if which_depends(gnsf["phi_expr"], gnsf["z"][ii])[0]: - y = vertcat(y, gnsf["z"][ii]) - # else: - # z[ii] is not part of y - ## uhat - uhat = SX.sym('uhat', 0, 0) - # components of u - for ii in range(gnsf["nu"]): - if which_depends(gnsf["phi_expr"], gnsf["u"][ii])[0]: - uhat = vertcat(uhat, gnsf["u"][ii]) - # else: - # u[ii] is not part of uhat - ## generate gnsf['phi_expr_fun'] - # linear input matrices - if is_empty(y): - gnsf["L_x"] = [] - gnsf["L_xdot"] = [] - gnsf["L_u"] = [] - gnsf["L_z"] = [] - else: - dummy = SX.sym("dummy_input", 0) - L_x_fun = Function( - "L_x_fun", [dummy], [jacobian(y, gnsf["x"][range(gnsf["nx1"])])] - ) - L_xdot_fun = Function( - "L_xdot_fun", [dummy], [jacobian(y, gnsf["xdot"][range(gnsf["nx1"])])] - ) - L_z_fun = Function( - "L_z_fun", [dummy], [jacobian(y, gnsf["z"][range(gnsf["nz1"])])] - ) - L_u_fun = Function("L_u_fun", [dummy], [jacobian(uhat, gnsf["u"])]) - - gnsf["L_x"] = L_x_fun(0).full() - gnsf["L_xdot"] = L_xdot_fun(0).full() - gnsf["L_u"] = L_u_fun(0).full() - gnsf["L_z"] = L_z_fun(0).full() - gnsf["y"] = y - gnsf["uhat"] = uhat - - gnsf["ny"] = casadi_length(y) - gnsf["nuhat"] = casadi_length(uhat) - gnsf["n_out"] = casadi_length(gnsf["phi_expr"]) - - return gnsf diff --git a/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py b/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py deleted file mode 100644 index 23c2440537cdba..00000000000000 --- a/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py +++ /dev/null @@ -1,155 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -from casadi import * -import numpy as np -from ..utils import casadi_length, idx_perm_to_ipiv -from .determine_input_nonlinearity_function import determine_input_nonlinearity_function -from .check_reformulation import check_reformulation - - -def determine_trivial_gnsf_transcription(acados_ocp, print_info): - ## Description - # this function takes a model of an implicit ODE/ index-1 DAE and sets up - # an equivalent model in the GNSF structure, with empty linear output - # system and trivial model matrices, i.e. A, B, E, c are zeros, and C is - # eye. - no structure is exploited - - model = acados_ocp.model - # initial print - print("*****************************************************************") - print(" ") - print(f"****** Restructuring {model.name} model ***********") - print(" ") - print("*****************************************************************") - - # load model - f_impl_expr = model.f_impl_expr - - model_name_prefix = model.name - - # x - x = model.x - nx = acados_ocp.dims.nx - # check type - if isinstance(x[0], SX): - isSX = True - else: - print("GNSF detection only works for SX CasADi type!!!") - import pdb - - pdb.set_trace() - # xdot - xdot = model.xdot - # u - nu = acados_ocp.dims.nu - if nu == 0: - u = SX.sym("u", 0, 0) - else: - u = model.u - - nz = acados_ocp.dims.nz - if nz == 0: - z = SX.sym("z", 0, 0) - else: - z = model.z - - p = model.p - nparam = acados_ocp.dims.np - - # avoid SX of size 0x1 - if casadi_length(u) == 0: - u = SX.sym("u", 0, 0) - nu = 0 - ## initialize gnsf struct - # dimensions - gnsf = {"nx": nx, "nu": nu, "nz": nz, "np": nparam} - gnsf["nx1"] = nx - gnsf["nx2"] = 0 - gnsf["nz1"] = nz - gnsf["nz2"] = 0 - gnsf["nuhat"] = nu - gnsf["ny"] = 2 * nx + nz - - gnsf["phi_expr"] = f_impl_expr - gnsf["A"] = np.zeros((nx + nz, nx)) - gnsf["B"] = np.zeros((nx + nz, nu)) - gnsf["E"] = np.zeros((nx + nz, nx + nz)) - gnsf["c"] = np.zeros((nx + nz, 1)) - gnsf["C"] = np.eye(nx + nz) - gnsf["name"] = model_name_prefix - - gnsf["x"] = x - gnsf["xdot"] = xdot - gnsf["z"] = z - gnsf["u"] = u - gnsf["p"] = p - - gnsf = determine_input_nonlinearity_function(gnsf) - - gnsf["A_LO"] = [] - gnsf["E_LO"] = [] - gnsf["B_LO"] = [] - gnsf["c_LO"] = [] - gnsf["f_lo_expr"] = [] - - # permutation - gnsf["idx_perm_x"] = range(nx) # matlab-style) - gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) # blasfeo-style - gnsf["idx_perm_z"] = range(nz) - gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"]) - gnsf["idx_perm_f"] = range((nx + nz)) - gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"]) - - gnsf["nontrivial_f_LO"] = 0 - - check_reformulation(model, gnsf, print_info) - if print_info: - print(f"Success: Set up equivalent GNSF model with trivial matrices") - print(" ") - if print_info: - print( - "-----------------------------------------------------------------------------------" - ) - print(" ") - print( - "reduced input ny of phi from ", - str(2 * nx + nz), - " to ", - str(gnsf["ny"]), - ) - print( - "reduced input nuhat of phi from ", str(nu), " to ", str(gnsf["nuhat"]) - ) - print(" ") - print( - "-----------------------------------------------------------------------------------" - ) - return gnsf diff --git a/third_party/acados/acados_template/gnsf/matlab to python.md b/third_party/acados/acados_template/gnsf/matlab to python.md deleted file mode 100644 index 53a0ed971e5b65..00000000000000 --- a/third_party/acados/acados_template/gnsf/matlab to python.md +++ /dev/null @@ -1,43 +0,0 @@ -# matlab to python - -% -> # - -; -> - -from casadi import * --> -from casadi import * - - -print\('(.*)'\) -print('$1') - -print\(\['(.*)'\]\) -print(f'$1') - -keyboard -import pdb; pdb.set_trace() - - -range((([^))]*)) -range($1) - -\s*end --> -nothing - - -if (.*) -if $1: - -else -else: - -num2str -str - -for ([a-z_]*) = -for $1 in - -length\( -len( \ No newline at end of file diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py b/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py deleted file mode 100644 index 297a56556c477b..00000000000000 --- a/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py +++ /dev/null @@ -1,394 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com - -from .determine_input_nonlinearity_function import determine_input_nonlinearity_function -from .check_reformulation import check_reformulation -from casadi import * -from ..utils import casadi_length, idx_perm_to_ipiv, is_empty - - -def reformulate_with_LOS(acados_ocp, gnsf, print_info): - - ## Description: - # This function takes an intitial transcription of the implicit ODE model - # "model" into "gnsf" and reformulates "gnsf" with a linear output system - # (LOS), containing as many states of the model as possible. - # Therefore it might be that the state vector and the implicit function - # vector have to be reordered. This reordered model is part of the output, - # namely reordered_model. - - ## import CasADi and load models - model = acados_ocp.model - - # symbolics - x = gnsf["x"] - xdot = gnsf["xdot"] - u = gnsf["u"] - z = gnsf["z"] - - # dimensions - nx = gnsf["nx"] - nz = gnsf["nz"] - - # get model matrices - A = gnsf["A"] - B = gnsf["B"] - C = gnsf["C"] - E = gnsf["E"] - c = gnsf["c"] - - A_LO = gnsf["A_LO"] - - y = gnsf["y"] - - phi_old = gnsf["phi_expr"] - - if print_info: - print(" ") - print("=================================================================") - print(" ") - print("================ Detect Linear Output System ===============") - print(" ") - print("=================================================================") - print(" ") - ## build initial I_x1 and I_x2_candidates - # I_xrange( all components of x for which either xii or xdot_ii enters y): - # I_LOS_candidates: the remaining components - - I_nsf_components = set() - I_LOS_candidates = set() - - if gnsf["ny"] > 0: - for ii in range(nx): - if which_depends(y, x[ii])[0] or which_depends(y, xdot[ii])[0]: - # i.e. xii or xiidot are part of y, and enter phi_expr - if print_info: - print(f"x_{ii} is part of x1") - I_nsf_components = set.union(I_nsf_components, set([ii])) - else: - # i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr - I_LOS_candidates = set.union(I_LOS_candidates, set([ii])) - if print_info: - print(" ") - for ii in range(nz): - if which_depends(y, z[ii])[0]: - # i.e. xii or xiidot are part of y, and enter phi_expr - if print_info: - print(f"z_{ii} is part of x1") - I_nsf_components = set.union(I_nsf_components, set([ii + nx])) - else: - # i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr - I_LOS_candidates = set.union(I_LOS_candidates, set([ii + nx])) - else: - I_LOS_candidates = set(range((nx + nz))) - if print_info: - print(" ") - print(f"I_LOS_candidates {I_LOS_candidates}") - new_nsf_components = I_nsf_components - I_nsf_eq = set([]) - unsorted_dyn = set(range(nx + nz)) - xdot_z = vertcat(xdot, z) - - ## determine components of Linear Output System - # determine maximal index set I_x2 - # such that the components x(I_x2) can be written as a LOS - Eq_map = [] - while True: - ## find equations corresponding to new_nsf_components - for ii in new_nsf_components: - current_var = xdot_z[ii] - var_name = current_var.name - - # print( unsorted_dyn) - # print("np.nonzero(E[:,ii])[0]",np.nonzero(E[:,ii])[0]) - I_eq = set.intersection(set(np.nonzero(E[:, ii])[0]), unsorted_dyn) - if len(I_eq) == 1: - i_eq = I_eq.pop() - if print_info: - print(f"component {i_eq} is associated with state {ii}") - elif len(I_eq) > 1: # x_ii_dot occurs in more than 1 eq linearly - # find the equation with least linear dependencies on - # I_LOS_cancidates - number_of_eq = 0 - candidate_dependencies = np.zeros(len(I_eq), 1) - I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx))) - for eq in I_eq: - depending_candidates = set.union( - np.nonzero(E[eq, I_LOS_candidates])[0], - np.nonzero(A[eq, I_x2_candidates])[0], - ) - candidate_dependencies[number_of_eq] = +len(depending_candidates) - number_of_eq += 1 - number_of_eq = np.argmin(candidate_dependencies) - i_eq = I_eq[number_of_eq] - else: ## x_ii_dot does not occur linearly in any of the unsorted dynamics - for j in unsorted_dyn: - phi_eq_j = gnsf["phi_expr"][np.nonzero(C[j, :])[0]] - if which_depends(phi_eq_j, xdot_z(ii))[0]: - I_eq = set.union(I_eq, j) - if is_empty(I_eq): - I_eq = unsorted_dyn - # find the equation with least linear dependencies on I_LOS_cancidates - number_of_eq = 0 - candidate_dependencies = np.zeros(len(I_eq), 1) - I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx))) - for eq in I_eq: - depending_candidates = set.union( - np.nonzero(E[eq, I_LOS_candidates])[0], - np.nonzero(A[eq, I_x2_candidates])[0], - ) - candidate_dependencies[number_of_eq] = +len(depending_candidates) - number_of_eq += 1 - number_of_eq = np.argmin(candidate_dependencies) - i_eq = I_eq[number_of_eq] - ## add 1 * [xdot,z](ii) to both sides of i_eq - if print_info: - print( - "adding 1 * ", - var_name, - " to both sides of equation ", - i_eq, - ".", - ) - gnsf["E"][i_eq, ii] = 1 - i_phi = np.nonzero(gnsf["C"][i_eq, :]) - if is_empty(i_phi): - i_phi = len(gnsf["phi_expr"]) + 1 - gnsf["C"][i_eq, i_phi] = 1 # add column to C with 1 entry - gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], 0) - gnsf["phi_expr"][i_phi] = ( - gnsf["phi_expr"](i_phi) - + gnsf["E"][i_eq, ii] / gnsf["C"][i_eq, i_phi] * xdot_z[ii] - ) - if print_info: - print( - "detected equation ", - i_eq, - " to correspond to variable ", - var_name, - ) - I_nsf_eq = set.union(I_nsf_eq, {i_eq}) - # remove i_eq from unsorted_dyn - unsorted_dyn.remove(i_eq) - Eq_map.append([ii, i_eq]) - - ## add components to I_x1 - for eq in I_nsf_eq: - I_linear_dependence = set.union( - set(np.nonzero(A[eq, :])[0]), set(np.nonzero(E[eq, :])[0]) - ) - I_nsf_components = set.union(I_linear_dependence, I_nsf_components) - # I_nsf_components = I_nsf_components[:] - - new_nsf_components = set.intersection(I_LOS_candidates, I_nsf_components) - if is_empty(new_nsf_components): - if print_info: - print("new_nsf_components is empty") - break - # remove new_nsf_components from candidates - I_LOS_candidates = set.difference(I_LOS_candidates, new_nsf_components) - if not is_empty(Eq_map): - # [~, new_eq_order] = sort(Eq_map(1,:)) - # I_nsf_eq = Eq_map(2, new_eq_order ) - for count, m in enumerate(Eq_map): - m.append(count) - sorted(Eq_map, key=lambda x: x[1]) - new_eq_order = [m[2] for m in Eq_map] - Eq_map = [Eq_map[i] for i in new_eq_order] - I_nsf_eq = [m[1] for m in Eq_map] - - else: - I_nsf_eq = [] - - I_LOS_components = I_LOS_candidates - I_LOS_eq = sorted(set.difference(set(range(nx + nz)), I_nsf_eq)) - I_nsf_eq = sorted(I_nsf_eq) - - I_x1 = set.intersection(I_nsf_components, set(range(nx))) - I_z1 = set.intersection(I_nsf_components, set(range(nx, nx + nz))) - I_z1 = set([i - nx for i in I_z1]) - - I_x2 = set.intersection(I_LOS_components, set(range(nx))) - I_z2 = set.intersection(I_LOS_components, set(range(nx, nx + nz))) - I_z2 = set([i - nx for i in I_z2]) - - if print_info: - print(f"I_x1 {I_x1}, I_x2 {I_x2}") - - ## permute x, xdot - if is_empty(I_x1): - x1 = [] - x1dot = [] - else: - x1 = x[list(I_x1)] - x1dot = xdot[list(I_x1)] - if is_empty(I_x2): - x2 = [] - x2dot = [] - else: - x2 = x[list(I_x2)] - x2dot = xdot[list(I_x2)] - if is_empty(I_z1): - z1 = [] - else: - z1 = z(I_z1) - if is_empty(I_z2): - z2 = [] - else: - z2 = z[list(I_z2)] - - I_x1 = sorted(I_x1) - I_x2 = sorted(I_x2) - I_z1 = sorted(I_z1) - I_z2 = sorted(I_z2) - gnsf["xdot"] = vertcat(x1dot, x2dot) - gnsf["x"] = vertcat(x1, x2) - gnsf["z"] = vertcat(z1, z2) - gnsf["nx1"] = len(I_x1) - gnsf["nx2"] = len(I_x2) - gnsf["nz1"] = len(I_z1) - gnsf["nz2"] = len(I_z2) - - # store permutations - gnsf["idx_perm_x"] = I_x1 + I_x2 - gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) - gnsf["idx_perm_z"] = I_z1 + I_z2 - gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"]) - gnsf["idx_perm_f"] = I_nsf_eq + I_LOS_eq - gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"]) - - f_LO = SX.sym("f_LO", 0, 0) - - ## rewrite I_LOS_eq as LOS - if gnsf["n_out"] == 0: - C_phi = np.zeros(gnsf["nx"] + gnsf["nz"], 1) - else: - C_phi = C @ phi_old - if gnsf["nx1"] == 0: - Ax1 = np.zeros(gnsf["nx"] + gnsf["nz"], 1) - else: - Ax1 = A[:, sorted(I_x1)] @ x1 - if gnsf["nx1"] + gnsf["nz1"] == 0: - lhs_nsf = np.zeros(gnsf["nx"] + gnsf["nz"], 1) - else: - lhs_nsf = E[:, sorted(I_nsf_components)] @ vertcat(x1, z1) - n_LO = len(I_LOS_eq) - B_LO = np.zeros((n_LO, gnsf["nu"])) - A_LO = np.zeros((gnsf["nx2"] + gnsf["nz2"], gnsf["nx2"])) - E_LO = np.zeros((n_LO, n_LO)) - c_LO = np.zeros((n_LO, 1)) - - I_LOS_eq = list(I_LOS_eq) - for eq in I_LOS_eq: - i_LO = I_LOS_eq.index(eq) - f_LO = vertcat(f_LO, Ax1[eq] + C_phi[eq] - lhs_nsf[eq]) - print(f"eq {eq} I_LOS_components {I_LOS_components}, i_LO {i_LO}, f_LO {f_LO}") - E_LO[i_LO, :] = E[eq, sorted(I_LOS_components)] - A_LO[i_LO, :] = A[eq, I_x2] - c_LO[i_LO, :] = c[eq] - B_LO[i_LO, :] = B[eq, :] - if casadi_length(f_LO) == 0: - f_LO = SX.zeros((gnsf["nx2"] + gnsf["nz2"], 1)) - f_LO = simplify(f_LO) - gnsf["A_LO"] = A_LO - gnsf["E_LO"] = E_LO - gnsf["B_LO"] = B_LO - gnsf["c_LO"] = c_LO - gnsf["f_lo_expr"] = f_LO - - ## remove I_LOS_eq from NSF type system - gnsf["A"] = gnsf["A"][np.ix_(sorted(I_nsf_eq), sorted(I_x1))] - gnsf["B"] = gnsf["B"][sorted(I_nsf_eq), :] - gnsf["C"] = gnsf["C"][sorted(I_nsf_eq), :] - gnsf["E"] = gnsf["E"][np.ix_(sorted(I_nsf_eq), sorted(I_nsf_components))] - gnsf["c"] = gnsf["c"][sorted(I_nsf_eq), :] - - ## reduce phi, C - I_nonzero = [] - for ii in range(gnsf["C"].shape[1]): # n_colums of C: - print(f"ii {ii}") - if not all(gnsf["C"][:, ii] == 0): # if column ~= 0 - I_nonzero.append(ii) - gnsf["C"] = gnsf["C"][:, I_nonzero] - gnsf["phi_expr"] = gnsf["phi_expr"][I_nonzero] - - gnsf = determine_input_nonlinearity_function(gnsf) - - check_reformulation(model, gnsf, print_info) - - gnsf["nontrivial_f_LO"] = 0 - if not is_empty(gnsf["f_lo_expr"]): - for ii in range(casadi_length(gnsf["f_lo_expr"])): - fii = gnsf["f_lo_expr"][ii] - if not fii.is_zero(): - gnsf["nontrivial_f_LO"] = 1 - if not gnsf["nontrivial_f_LO"] and print_info: - print("f_LO is fully trivial (== 0)") - check_reformulation(model, gnsf, print_info) - - if print_info: - print("") - print( - "---------------------------------------------------------------------------------" - ) - print( - "------------- Success: Linear Output System (LOS) detected ----------------------" - ) - print( - "---------------------------------------------------------------------------------" - ) - print("") - print( - "==>> moved ", - gnsf["nx2"], - "differential states and ", - gnsf["nz2"], - " algebraic variables to the Linear Output System", - ) - print( - "==>> recuced output dimension of phi from ", - casadi_length(phi_old), - " to ", - casadi_length(gnsf["phi_expr"]), - ) - print(" ") - print("Matrices defining the LOS read as") - print(" ") - print("E_LO =") - print(gnsf["E_LO"]) - print("A_LO =") - print(gnsf["A_LO"]) - print("B_LO =") - print(gnsf["B_LO"]) - print("c_LO =") - print(gnsf["c_LO"]) - - return gnsf diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py b/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py deleted file mode 100644 index 21ab8ebfd53b78..00000000000000 --- a/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py +++ /dev/null @@ -1,167 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com - -from casadi import * -from .determine_input_nonlinearity_function import determine_input_nonlinearity_function -from .check_reformulation import check_reformulation - - -def reformulate_with_invertible_E_mat(gnsf, model, print_info): - ## Description - # this function checks that the necessary condition to apply the gnsf - # structure exploiting integrator to a model, namely that the matrices E11, - # E22 are invertible holds. - # if this is not the case, it will make these matrices invertible and add: - # corresponding terms, to the term C * phi, such that the obtained model is - # still equivalent - - # check invertibility of E11, E22 and reformulate if needed: - ind_11 = range(gnsf["nx1"]) - ind_22 = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"]) - - if print_info: - print(" ") - print("----------------------------------------------------") - print("checking rank of E11 and E22") - print("----------------------------------------------------") - ## check if E11, E22 are invertible: - z_check = False - if gnsf["nz1"] > 0: - z_check = ( - np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_22, ind_22)]) != gnsf["nz1"] - ) - - if ( - np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_11, ind_11)]) != gnsf["nx1"] - or z_check - ): - # print warning (always) - print(f"the rank of E11 or E22 is not full after the reformulation") - print("") - print( - f"the script will try to reformulate the model with an invertible matrix instead" - ) - print( - f"NOTE: this feature is based on a heuristic, it should be used with care!!!" - ) - - ## load models - xdot = gnsf["xdot"] - z = gnsf["z"] - - # # GNSF - # get dimensions - nx1 = gnsf["nx1"] - x1dot = xdot[range(nx1)] - - k = vertcat(x1dot, z) - for i in [1, 2]: - if i == 1: - ind = range(gnsf["nx1"]) - else: - ind = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"]) - mat = gnsf["E"][np.ix_(ind, ind)] - import pdb - - pdb.set_trace() - while np.linalg.matrix_rank(mat) < len(ind): - # import pdb; pdb.set_trace() - if print_info: - print(" ") - print(f"the rank of E", str(i), str(i), " is not full") - print( - f"the algorithm will try to reformulate the model with an invertible matrix instead" - ) - print( - f"NOTE: this feature is not super stable and might need more testing!!!!!!" - ) - for sub_max in ind: - sub_ind = range(min(ind), sub_max) - # regard the submatrix mat(sub_ind, sub_ind) - sub_mat = gnsf["E"][sub_ind, sub_ind] - if np.linalg.matrix_rank(sub_mat) < len(sub_ind): - # reformulate the model by adding a 1 to last diagonal - # element and changing rhs respectively. - gnsf["E"][sub_max, sub_max] = gnsf["E"][sub_max, sub_max] + 1 - # this means adding the term 1 * k(sub_max) to the sub_max - # row of the l.h.s - if len(np.nonzero(gnsf["C"][sub_max, :])[0]) == 0: - # if isempty(find(gnsf['C'](sub_max,:), 1)): - # add new nonlinearity entry - gnsf["C"][sub_max, gnsf["n_out"] + 1] = 1 - gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], k[sub_max]) - else: - ind_f = np.nonzero(gnsf["C"][sub_max, :])[0] - if len(ind_f) != 1: - raise Exception("C is assumed to be a selection matrix") - else: - ind_f = ind_f[0] - # add term to corresponding nonlinearity entry - # note: herbey we assume that C is a selection matrix, - # i.e. gnsf['phi_expr'](ind_f) is only entering one equation - - gnsf["phi_expr"][ind_f] = ( - gnsf["phi_expr"][ind_f] - + k[sub_max] / gnsf["C"][sub_max, ind_f] - ) - gnsf = determine_input_nonlinearity_function(gnsf) - check_reformulation(model, gnsf, print_info) - print("successfully reformulated the model with invertible matrices E11, E22") - else: - if print_info: - print(" ") - print( - "the rank of both E11 and E22 is naturally full after the reformulation " - ) - print("==> model reformulation finished") - print(" ") - if (gnsf['nx2'] > 0 or gnsf['nz2'] > 0) and det(gnsf["E_LO"]) == 0: - print( - "_______________________________________________________________________________________________________" - ) - print(" ") - print("TAKE CARE ") - print("E_LO matrix is NOT regular after automatic transcription!") - print("->> this means the model CANNOT be used with the gnsf integrator") - print( - "->> it probably means that one entry (of xdot or z) that was moved to the linear output type system" - ) - print(" does not appear in the model at all (zero column in E_LO)") - print(" OR: the columns of E_LO are linearly dependent ") - print(" ") - print( - " SOLUTIONs: a) go through your model & check equations the method wanted to move to LOS" - ) - print(" b) deactivate the detect_LOS option") - print( - "_______________________________________________________________________________________________________" - ) - return gnsf diff --git a/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py b/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py deleted file mode 100644 index db2d18758e99db..00000000000000 --- a/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py +++ /dev/null @@ -1,174 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# -# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com - -from casadi import n_nodes -import numpy as np - - -def structure_detection_print_summary(gnsf, acados_ocp): - - ## Description - # this function prints the most important info after determining a GNSF - # reformulation of the implicit model "initial_model" into "gnsf", which is - # equivalent to the "reordered_model". - model = acados_ocp.model - # # GNSF - # get dimensions - nx = gnsf["nx"] - nu = gnsf["nu"] - nz = gnsf["nz"] - - nx1 = gnsf["nx1"] - nx2 = gnsf["nx2"] - - nz1 = gnsf["nz1"] - nz2 = gnsf["nz2"] - - # np = gnsf['np'] - n_out = gnsf["n_out"] - ny = gnsf["ny"] - nuhat = gnsf["nuhat"] - - # - f_impl_expr = model.f_impl_expr - n_nodes_initial = n_nodes(model.f_impl_expr) - # x_old = model.x - # f_impl_old = model.f_impl_expr - - x = gnsf["x"] - z = gnsf["z"] - - phi_current = gnsf["phi_expr"] - - ## PRINT SUMMARY -- STRUCHTRE DETECTION - print(" ") - print( - "*********************************************************************************************" - ) - print(" ") - print( - "****************** SUCCESS: GNSF STRUCTURE DETECTION COMPLETE !!! ***************" - ) - print(" ") - print( - "*********************************************************************************************" - ) - print(" ") - print( - f"========================= STRUCTURE DETECTION SUMMARY ====================================" - ) - print(" ") - print("-------- Nonlinear Static Feedback type system --------") - print(" ") - print(" successfully transcribed dynamic system model into GNSF structure ") - print(" ") - print( - "reduced dimension of nonlinearity phi from ", - str(nx + nz), - " to ", - str(gnsf["n_out"]), - ) - print(" ") - print( - "reduced input dimension of nonlinearity phi from ", - 2 * nx + nz + nu, - " to ", - gnsf["ny"] + gnsf["nuhat"], - ) - print(" ") - print(f"reduced number of nodes in CasADi expression of nonlinearity phi from {n_nodes_initial} to {n_nodes(phi_current)}\n") - print("----------- Linear Output System (LOS) ---------------") - if nx2 + nz2 > 0: - print(" ") - print(f"introduced Linear Output System of size ", str(nx2 + nz2)) - print(" ") - if nx2 > 0: - print("consisting of the states:") - print(" ") - print(x[range(nx1, nx)]) - print(" ") - if nz2 > 0: - print("and algebraic variables:") - print(" ") - print(z[range(nz1, nz)]) - print(" ") - if gnsf["purely_linear"] == 1: - print(" ") - print("Model is fully linear!") - print(" ") - if not all(gnsf["idx_perm_x"] == np.array(range(nx))): - print(" ") - print( - "--------------------------------------------------------------------------------------------------" - ) - print( - "NOTE: permuted differential state vector x, such that x_gnsf = x(idx_perm_x) with idx_perm_x =" - ) - print(" ") - print(gnsf["idx_perm_x"]) - if nz != 0 and not all(gnsf["idx_perm_z"] == np.array(range(nz))): - print(" ") - print( - "--------------------------------------------------------------------------------------------------" - ) - print( - "NOTE: permuted algebraic state vector z, such that z_gnsf = z(idx_perm_z) with idx_perm_z =" - ) - print(" ") - print(gnsf["idx_perm_z"]) - if not all(gnsf["idx_perm_f"] == np.array(range(nx + nz))): - print(" ") - print( - "--------------------------------------------------------------------------------------------------" - ) - print( - "NOTE: permuted rhs expression vector f, such that f_gnsf = f(idx_perm_f) with idx_perm_f =" - ) - print(" ") - print(gnsf["idx_perm_f"]) - ## print GNSF dimensions - print( - "--------------------------------------------------------------------------------------------------------" - ) - print(" ") - print("The dimensions of the GNSF reformulated model read as:") - print(" ") - # T_dim = table(nx, nu, nz, np, nx1, nz1, n_out, ny, nuhat) - # print( T_dim ) - print(f"nx ", {nx}) - print(f"nu ", {nu}) - print(f"nz ", {nz}) - # print(f"np ", {np}) - print(f"nx1 ", {nx1}) - print(f"nz1 ", {nz1}) - print(f"n_out ", {n_out}) - print(f"ny ", {ny}) - print(f"nuhat ", {nuhat}) diff --git a/third_party/acados/acados_template/utils.py b/third_party/acados/acados_template/utils.py deleted file mode 100644 index f27617fa309867..00000000000000 --- a/third_party/acados/acados_template/utils.py +++ /dev/null @@ -1,434 +0,0 @@ -# -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os, sys, json -import urllib.request -import shutil -import numpy as np -from casadi import SX, MX, DM, Function, CasadiMeta - -ALLOWED_CASADI_VERSIONS = ('3.5.6', '3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0') - -TERA_VERSION = "0.0.34" - -PLATFORM2TERA = { - "linux": "linux", - "darwin": "osx", - "win32": "windows" -} - - -def get_acados_path(): - ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR') - if not ACADOS_PATH: - acados_template_path = os.path.dirname(os.path.abspath(__file__)) - acados_path = os.path.join(acados_template_path, '..','..','..') - ACADOS_PATH = os.path.realpath(acados_path) - msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, ' - msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH) - msg += 'Please export ACADOS_SOURCE_DIR to avoid this warning.' - print(msg) - return ACADOS_PATH - - -def get_python_interface_path(): - ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH') - if not ACADOS_PYTHON_INTERFACE_PATH: - acados_path = get_acados_path() - ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template') - return ACADOS_PYTHON_INTERFACE_PATH - - -def get_tera_exec_path(): - TERA_PATH = os.environ.get('TERA_PATH') - if not TERA_PATH: - TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') - if os.name == 'nt': - TERA_PATH += '.exe' - return TERA_PATH - - -def check_casadi_version(): - casadi_version = CasadiMeta.version() - if casadi_version in ALLOWED_CASADI_VERSIONS: - return - else: - msg = 'Warning: Please note that the following versions of CasADi are ' - msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) - msg += 'If there is an incompatibility with the CasADi generated code, ' - msg += 'please consider changing your CasADi version.\n' - msg += 'Version {} currently in use.'.format(casadi_version) - print(msg) - - -def is_column(x): - if isinstance(x, np.ndarray): - if x.ndim == 1: - return True - elif x.ndim == 2 and x.shape[1] == 1: - return True - else: - return False - elif isinstance(x, (MX, SX, DM)): - if x.shape[1] == 1: - return True - elif x.shape[0] == 0 and x.shape[1] == 0: - return True - else: - return False - elif x == None or x == []: - return False - else: - raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." - + " Got: " + str(type(x))) - - -def is_empty(x): - if isinstance(x, (MX, SX, DM)): - return x.is_empty() - elif isinstance(x, np.ndarray): - if np.prod(x.shape) == 0: - return True - else: - return False - elif x == None: - return True - elif isinstance(x, (set, list)): - if len(x)==0: - return True - else: - return False - else: - raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, " - + "None, numpy array empty list, set. Got: " + str(type(x))) - - -def casadi_length(x): - if isinstance(x, (MX, SX, DM)): - return int(np.prod(x.shape)) - else: - raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX." - + " Got: " + str(type(x))) - - -def make_model_consistent(model): - x = model.x - xdot = model.xdot - u = model.u - z = model.z - p = model.p - - if isinstance(x, MX): - symbol = MX.sym - elif isinstance(x, SX): - symbol = SX.sym - else: - raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x))) - - if is_empty(p): - model.p = symbol('p', 0, 0) - - if is_empty(z): - model.z = symbol('z', 0, 0) - - return model - -def get_lib_ext(): - lib_ext = '.so' - if sys.platform == 'darwin': - lib_ext = '.dylib' - elif os.name == 'nt': - lib_ext = '' - - return lib_ext - -def get_tera(): - tera_path = get_tera_exec_path() - acados_path = get_acados_path() - - if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): - return tera_path - - repo_url = "https://github.com/acados/tera_renderer/releases" - url = "{}/download/v{}/t_renderer-v{}-{}".format( - repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform]) - - manual_install = 'For manual installation follow these instructions:\n' - manual_install += '1 Download binaries from {}\n'.format(url) - manual_install += '2 Copy them in {}/bin\n'.format(acados_path) - manual_install += '3 Strip the version and platform from the binaries: ' - manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n' - manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' - manual_install += '"chmod +x {}"\n\n'.format(tera_path) - - msg = "\n" - msg += 'Tera template render executable not found, ' - msg += 'while looking in path:\n{}\n'.format(tera_path) - msg += 'In order to be able to render the templates, ' - msg += 'you need to download the tera renderer binaries from:\n' - msg += '{}\n\n'.format(repo_url) - msg += 'Do you wish to set up Tera renderer automatically?\n' - msg += 'y/N? (press y to download tera or any key for manual installation)\n' - - if input(msg) == 'y': - print("Dowloading {}".format(url)) - with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: - shutil.copyfileobj(response, out_file) - print("Successfully downloaded t_renderer.") - os.chmod(tera_path, 0o755) - return tera_path - - msg_cancel = "\nYou cancelled automatic download.\n\n" - msg_cancel += manual_install - msg_cancel += "Once installed re-run your script.\n\n" - print(msg_cancel) - - sys.exit(1) - - -def render_template(in_file, out_file, output_dir, json_path, template_glob=None): - - acados_path = os.path.dirname(os.path.abspath(__file__)) - if template_glob is None: - template_glob = os.path.join(acados_path, 'c_templates_tera', '**', '*') - cwd = os.getcwd() - - if not os.path.exists(output_dir): - os.makedirs(output_dir) - os.chdir(output_dir) - - tera_path = get_tera() - - # call tera as system cmd - os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'" - # Windows cmd.exe can not cope with '...', so use "..." instead: - if os.name == 'nt': - os_cmd = os_cmd.replace('\'', '\"') - - status = os.system(os_cmd) - if (status != 0): - raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n') - - os.chdir(cwd) - - -## Conversion functions -def make_object_json_dumpable(input): - if isinstance(input, (np.ndarray)): - return input.tolist() - elif isinstance(input, (SX)): - return input.serialize() - elif isinstance(input, (MX)): - # NOTE: MX expressions can not be serialized, only Functions. - return input.__str__() - elif isinstance(input, (DM)): - return input.full() - else: - raise TypeError(f"Cannot make input of type {type(input)} dumpable.") - - -def format_class_dict(d): - """ - removes the __ artifact from class to dict conversion - """ - out = {} - for k, v in d.items(): - if isinstance(v, dict): - v = format_class_dict(v) - - out_key = k.split('__', 1)[-1] - out[k.replace(k, out_key)] = v - return out - - -def get_ocp_nlp_layout() -> dict: - python_interface_path = get_python_interface_path() - abs_path = os.path.join(python_interface_path, 'acados_layout.json') - with open(abs_path, 'r') as f: - ocp_nlp_layout = json.load(f) - return ocp_nlp_layout - - -def get_default_simulink_opts() -> dict: - python_interface_path = get_python_interface_path() - abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json') - with open(abs_path, 'r') as f: - simulink_opts = json.load(f) - return simulink_opts - - -def J_to_idx(J): - nrows = J.shape[0] - idx = np.zeros((nrows, )) - for i in range(nrows): - this_idx = np.nonzero(J[i,:])[0] - if len(this_idx) != 1: - raise Exception('Invalid J matrix structure detected, ' \ - 'must contain one nonzero element per row.') - if this_idx.size > 0 and J[i,this_idx[0]] != 1: - raise Exception('J matrices can only contain 1s.') - idx[i] = this_idx[0] - return idx - - -def J_to_idx_slack(J): - nrows = J.shape[0] - ncol = J.shape[1] - idx = np.zeros((ncol, )) - i_idx = 0 - for i in range(nrows): - this_idx = np.nonzero(J[i,:])[0] - if len(this_idx) == 1: - idx[i_idx] = i - i_idx = i_idx + 1 - elif len(this_idx) > 1: - raise Exception('J_to_idx_slack: Invalid J matrix. ' \ - 'Found more than one nonzero in row ' + str(i)) - if this_idx.size > 0 and J[i,this_idx[0]] != 1: - raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \ - 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) - if not i_idx == ncol: - raise Exception('J_to_idx_slack: J must contain a 1 in every column!') - return idx - - -def acados_dae_model_json_dump(model): - - # load model - x = model.x - xdot = model.xdot - u = model.u - z = model.z - p = model.p - - f_impl = model.f_impl_expr - model_name = model.name - - # create struct with impl_dae_fun, casadi_version - fun_name = model_name + '_impl_dae_fun' - impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) - - casadi_version = CasadiMeta.version() - str_impl_dae_fun = impl_dae_fun.serialize() - - dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version} - - # dump - json_file = model_name + '_acados_dae.json' - with open(json_file, 'w') as f: - json.dump(dae_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True) - print("dumped ", model_name, " dae to file:", json_file, "\n") - - -def set_up_imported_gnsf_model(acados_ocp): - - gnsf = acados_ocp.gnsf_model - - # check CasADi version - # dump_casadi_version = gnsf['casadi_version'] - # casadi_version = CasadiMeta.version() - - # if not casadi_version == dump_casadi_version: - # print("WARNING: GNSF model was dumped with another CasADi version.\n" - # + "This might yield errors. Please use the same version for compatibility, serialize version: " - # + dump_casadi_version + " current Python CasADi verison: " + casadi_version) - # input("Press any key to attempt to continue...") - - # load model - phi_fun = Function.deserialize(gnsf['phi_fun']) - phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y']) - phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat']) - get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun']) - - # obtain gnsf dimensions - size_gnsf_A = get_matrices_fun.size_out(0) - acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1] - acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] - acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1)) - acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0)) - acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0)) - - # save gnsf functions in model - acados_ocp.model.phi_fun = phi_fun - acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y - acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat - acados_ocp.model.get_matrices_fun = get_matrices_fun - - # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},... - # {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,... - # nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO}); - get_matrices_out = get_matrices_fun(0) - acados_ocp.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12]) - acados_ocp.model.gnsf['purely_linear'] = int(get_matrices_out[13]) - - if "f_lo_fun_jac_x1k1uz" in gnsf: - f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz']) - acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz - else: - dummy_var_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1) - dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_ocp.dims.gnsf_nx1) - dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1) - dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu) - dummy_var_p = SX.sym('dummy_var_z1', acados_ocp.dims.np) - empty_var = SX.sym('empty_var', 0, 0) - - empty_fun = Function('empty_fun', \ - [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p], - [empty_var]) - acados_ocp.model.f_lo_fun_jac_x1k1uz = empty_fun - - del acados_ocp.gnsf_model - - -def idx_perm_to_ipiv(idx_perm): - n = len(idx_perm) - vec = list(range(n)) - ipiv = np.zeros(n) - - print(n, idx_perm) - # import pdb; pdb.set_trace() - for ii in range(n): - idx0 = idx_perm[ii] - for jj in range(ii,n): - if vec[jj]==idx0: - idx1 = jj - break - tmp = vec[ii] - vec[ii] = vec[idx1] - vec[idx1] = tmp - ipiv[ii] = idx1 - - ipiv = ipiv-1 # C 0-based indexing - return ipiv - - -def print_casadi_expression(f): - for ii in range(casadi_length(f)): - print(f[ii,:]) diff --git a/third_party/acados/acados_template/zoro_description.py b/third_party/acados/acados_template/zoro_description.py deleted file mode 100644 index 4d795c1502e1a5..00000000000000 --- a/third_party/acados/acados_template/zoro_description.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright (c) The acados authors. -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; - -from dataclasses import dataclass, field -import numpy as np - - -@dataclass -class ZoroDescription: - """ - Zero-Order Robust Optimization scheme. - - For advanced users. - """ - backoff_scaling_gamma: float = 1.0 - fdbk_K_mat: np.ndarray = None - unc_jac_G_mat: np.ndarray = None # default: an identity matrix - P0_mat: np.ndarray = None - W_mat: np.ndarray = None - idx_lbx_t: list = field(default_factory=list) - idx_ubx_t: list = field(default_factory=list) - idx_lbx_e_t: list = field(default_factory=list) - idx_ubx_e_t: list = field(default_factory=list) - idx_lbu_t: list = field(default_factory=list) - idx_ubu_t: list = field(default_factory=list) - idx_lg_t: list = field(default_factory=list) - idx_ug_t: list = field(default_factory=list) - idx_lg_e_t: list = field(default_factory=list) - idx_ug_e_t: list = field(default_factory=list) - idx_lh_t: list = field(default_factory=list) - idx_uh_t: list = field(default_factory=list) - idx_lh_e_t: list = field(default_factory=list) - idx_uh_e_t: list = field(default_factory=list) - -def process_zoro_description(zoro_description: ZoroDescription): - zoro_description.nw, _ = zoro_description.W_mat.shape - if zoro_description.unc_jac_G_mat is None: - zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw) - zoro_description.nlbx_t = len(zoro_description.idx_lbx_t) - zoro_description.nubx_t = len(zoro_description.idx_ubx_t) - zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t) - zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t) - zoro_description.nlbu_t = len(zoro_description.idx_lbu_t) - zoro_description.nubu_t = len(zoro_description.idx_ubu_t) - zoro_description.nlg_t = len(zoro_description.idx_lg_t) - zoro_description.nug_t = len(zoro_description.idx_ug_t) - zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t) - zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t) - zoro_description.nlh_t = len(zoro_description.idx_lh_t) - zoro_description.nuh_t = len(zoro_description.idx_uh_t) - zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t) - zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t) - return zoro_description.__dict__ diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 2b803ef6b2458a..0481e8159b7601 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -13,8 +13,13 @@ fi ACADOS_FLAGS="-DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET" if [[ "$OSTYPE" == "darwin"* ]]; then - ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64 -DCMAKE_MACOSX_RPATH=1" - ARCHNAME="Darwin" + if [[ $(uname -m) == "x86_64" ]]; then + ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=x86_64" + ARCHNAME="Darwin_x86_64" + else + ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64" + ARCHNAME="Darwin" + fi fi if [ ! -d acados_repo/ ]; then @@ -23,8 +28,8 @@ if [ ! -d acados_repo/ ]; then fi cd acados_repo git fetch --all -git checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2 -git submodule update --depth=1 --recursive --init +git checkout 8ea8827fafb1b23b4c7da1c4cf650de1cbd73584 +git submodule update --recursive --init # build mkdir -p build @@ -38,25 +43,14 @@ mkdir -p $INSTALL_DIR rm $DIR/acados_repo/lib/*.json -rm -rf $DIR/include $DIR/acados_template +rm -rf $DIR/include cp -r $DIR/acados_repo/include $DIR cp -r $DIR/acados_repo/lib $INSTALL_DIR -cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/ +rm -rf $DIR/../../pyextra/acados_template +cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra #pip3 install -e $DIR/acados/interfaces/acados_template -# skip macOS - sed is different :/ -if [[ "$OSTYPE" != "darwin"* ]]; then - # strip future_fstrings to avoid having to install the compatibility package - find $DIR/acados_template/ -type f -exec sed -i '/future.fstrings/d' {} + -fi - # build tera cd $DIR/acados_repo/interfaces/acados_template/tera_renderer/ -if [[ "$OSTYPE" == "darwin"* ]]; then - cargo build --verbose --release --target aarch64-apple-darwin - cargo build --verbose --release --target x86_64-apple-darwin - lipo -create -output target/release/t_renderer target/x86_64-apple-darwin/release/t_renderer target/aarch64-apple-darwin/release/t_renderer -else - cargo build --verbose --release -fi +cargo build --verbose --release cp target/release/t_renderer $INSTALL_DIR/ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_common.h b/third_party/acados/include/acados/dense_qp/dense_qp_common.h index 2a9a974f99b0d2..f3809c42948fc7 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_common.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h deleted file mode 100644 index b262089b4ffe0e..00000000000000 --- a/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ -#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -// blasfeo -#include "blasfeo/include/blasfeo_common.h" - -// daqp -#include "daqp/include/types.h" - -// acados -#include "acados/dense_qp/dense_qp_common.h" -#include "acados/utils/types.h" - - -typedef struct dense_qp_daqp_opts_ -{ - DAQPSettings* daqp_opts; - int warm_start; -} dense_qp_daqp_opts; - - -typedef struct dense_qp_daqp_memory_ -{ - double* lb_tmp; - double* ub_tmp; - int* idxb; - int* idxv_to_idxb; - int* idxs; - int* idxdaqp_to_idxs; - - double* Zl; - double* Zu; - double* zl; - double* zu; - double* d_ls; - double* d_us; - - double time_qp_solver_call; - int iter; - DAQPWorkspace * daqp_work; - -} dense_qp_daqp_memory; - -// opts -acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims); -// -void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); -// -void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); -// -void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_); -// -// memory -acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); -// -void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory); -// -acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); -// -void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); -// -// functions -int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); -// -void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// -void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// -void dense_qp_daqp_config_initialize_default(void *config_); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h index 136279d666144f..20eedc26a2bf33 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h index d051cb15f70281..646f11f06f6664 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h index 392e472918d613..52606fac5def32 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h index 0e13d3ef64ef6e..9f0bb1af68c320 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h index ba97db97684245..60d835fc58c2ea 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -416,9 +419,6 @@ void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha); // -int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, - ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); -// double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, int check_early_termination); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 9dbf16f6dc2df3..7f7a30faf3a6d3 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -79,6 +82,9 @@ acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config); // void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory); // +void ocp_nlp_constraints_bgh_dims_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx, + int nbu, int ng, int nh, int dummy0, int ns); +// void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, @@ -110,9 +116,6 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_m int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); -// -void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_, - void *model_, const char *field, void *value); /************************************************ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index eb05edf7a6bf2d..beeec784116b82 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -60,7 +63,7 @@ typedef struct int nbu; int nbx; int ng; // number of general linear constraints - int nphi; // dimension of convex outer part + int nphi; // dimension of convex outer part int ns; // nsbu + nsbx + nsg + nsphi int nsbu; // number of softened input bounds int nsbx; // number of softened state bounds @@ -78,6 +81,9 @@ acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config); // void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory); // +void ocp_nlp_constraints_bgp_dims_initialize(void *config, void *dims, int nx, int nu, int nz, + int nbx, int nbu, int ng, int nphi, int nq, int ns); +// void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -103,9 +109,6 @@ void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory) // int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); -// -void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_, - void *model_, const char *field, void *value); /* options */ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h index bb73c468deb377..7cadecab46b6c7 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -57,10 +60,11 @@ typedef struct { acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng, + int nh, int nq, int ns); acados_size_t (*model_calculate_size)(void *config, void *dims); void *(*model_assign)(void *config, void *dims, void *raw_memory); int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value); - void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value); acados_size_t (*opts_calculate_size)(void *config, void *dims); void *(*opts_assign)(void *config, void *dims, void *raw_memory); void (*opts_initialize_default)(void *config, void *dims, void *opts); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h index eb4056403648c6..c9fbbfb4046033 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -30,8 +33,8 @@ /// -/// \defgroup ocp_nlp_cost ocp_nlp_cost -/// +/// \defgroup ocp_nlp_cost ocp_nlp_cost +/// /// \addtogroup ocp_nlp_cost ocp_nlp_cost /// @{ @@ -59,6 +62,7 @@ typedef struct { acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); void (*dims_set)(void *config_, void *dims_, const char *field, int *value); void (*dims_get)(void *config_, void *dims_, const char *field, int *value); acados_size_t (*model_calculate_size)(void *config, void *dims); @@ -87,8 +91,6 @@ typedef struct // computes the cost function value (intended for globalization) void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); void (*config_initialize_default)(void *config); - void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); - } ocp_nlp_cost_config; // @@ -103,5 +105,5 @@ ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory); #endif #endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ -/// @} -/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h deleted file mode 100644 index 2eb3f5d127a7a9..00000000000000 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h +++ /dev/null @@ -1,207 +0,0 @@ -/* - * Copyright (c) The acados authors. - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -/// \addtogroup ocp_nlp -/// @{ -/// \addtogroup ocp_nlp_cost ocp_nlp_cost -/// @{ -/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl -/// \brief This module implements convex-over-nonlinear costs of the form -/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$, - - -#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ -#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -// blasfeo -#include "blasfeo/include/blasfeo_common.h" - -// acados -#include "acados/ocp_nlp/ocp_nlp_cost_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/types.h" - - - -/************************************************ - * dims - ************************************************/ - -typedef struct -{ - int nx; // number of states - int nz; // number of algebraic variables - int nu; // number of inputs - int ny; // number of outputs - int ns; // number of slacks -} ocp_nlp_cost_conl_dims; - -// -acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config); -// -void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory); -// -void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); -// -void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value); -// -void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value); - - - -/************************************************ - * model - ************************************************/ - -typedef struct -{ - // slack penalty has the form z^T * s + .5 * s^T * Z * s - external_function_generic *conl_cost_fun; - external_function_generic *conl_cost_fun_jac_hess; - struct blasfeo_dvec y_ref; - struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector - struct blasfeo_dvec z; // gradient of slacks as vector - double scaling; -} ocp_nlp_cost_conl_model; - -// -acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims); -// -void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory); -// -int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_); - - - -/************************************************ - * options - ************************************************/ - -typedef struct -{ - bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian -} ocp_nlp_cost_conl_opts; - -// -acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims); -// -void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory); -// -void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts); -// -void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts); -// -void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value); - - - -/************************************************ - * memory - ************************************************/ -typedef struct -{ - struct blasfeo_dvec grad; // gradient of cost function - struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out - struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in - struct blasfeo_dvec *Z; // pointer to Z in qp_in - struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out - struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out - double fun; ///< value of the cost function -} ocp_nlp_cost_conl_memory; - -// -acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts); -// -void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory); -// -double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_); -// -struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_); -// -void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); -// -void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); -// -void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); -// -void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// -void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); -// -void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); - -/************************************************ - * workspace - ************************************************/ - -typedef struct -{ - struct blasfeo_dmat W; // hessian of outer loss function - struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function - struct blasfeo_dmat Jt_ux; // jacobian of inner residual function - struct blasfeo_dmat Jt_ux_tilde; // jacobian of inner residual function plus gradient contribution of algebraic variables - struct blasfeo_dmat Jt_z; // jacobian of inner residual function wrt algebraic variables - struct blasfeo_dmat tmp_nv_ny; - struct blasfeo_dvec tmp_ny; - struct blasfeo_dvec tmp_2ns; -} ocp_nlp_cost_conl_workspace; - -// -acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts); - -/************************************************ - * functions - ************************************************/ - -// -void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); -// -void ocp_nlp_cost_conl_config_initialize_default(void *config); -// -void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); -// -void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); -// -void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ -/// @} -/// @} -/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h index 78958270ded651..f2196dbee79820 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -51,7 +54,6 @@ extern "C" { typedef struct { int nx; // number of states - int nz; // number of algebraic variables int nu; // number of inputs int ns; // number of slacks } ocp_nlp_cost_external_dims; @@ -61,6 +63,9 @@ acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config); // void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory); // +void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx, + int nu, int ny, int ns, int nz); +// void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -117,7 +122,7 @@ typedef struct { struct blasfeo_dvec grad; // gradient of cost function struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *Z; // pointer to Z in qp_in struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out @@ -152,10 +157,7 @@ void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_ typedef struct { - struct blasfeo_dmat tmp_nunx_nunx; - struct blasfeo_dmat tmp_nz_nz; - struct blasfeo_dmat tmp_nz_nunx; - struct blasfeo_dvec tmp_nunxnz; + struct blasfeo_dmat tmp_nv_nv; struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns } ocp_nlp_cost_external_workspace; @@ -166,8 +168,6 @@ acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void * functions ************************************************/ -// -void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); // void ocp_nlp_cost_external_config_initialize_default(void *config); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h index 801e9a5b87b2d6..3cf759504aba05 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -79,14 +82,30 @@ typedef struct acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config); -/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct +/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct /// -/// \param[in] config structure containing configuration of ocp_nlp_cost +/// \param[in] config structure containing configuration of ocp_nlp_cost /// module -/// \param[in] raw_memory pointer to memory location +/// \param[in] raw_memory pointer to memory location /// \param[out] [] /// \return dims void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory); + + +/// Initialize the dimensions struct of the +/// ocp_nlp-cost_ls component +/// +/// \param[in] config structure containing configuration ocp_nlp-cost_ls component +/// \param[in] nx number of states +/// \param[in] nu number of inputs +/// \param[in] ny number of residuals +/// \param[in] ns number of slacks +/// \param[in] nz number of algebraic variables +/// \param[out] dims +/// \return size +void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx, + int nu, int ny, int ns, int nz); + // void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value); // @@ -98,7 +117,7 @@ void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int //////////////////////////////////////////////////////////////////////////////// -/// structure containing the data describing the linear least-square cost +/// structure containing the data describing the linear least-square cost typedef struct { // slack penalty has the form z^T * s + .5 * s^T * Z * s @@ -109,8 +128,6 @@ typedef struct struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper) struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper) double scaling; - int W_changed; ///< flag indicating whether W has changed and needs to be refactorized - int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed } ocp_nlp_cost_ls_model; // @@ -153,7 +170,7 @@ void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void -/// structure containing the memory associated with cost_ls component +/// structure containing the memory associated with cost_ls component /// of the ocp_nlp module typedef struct { @@ -220,8 +237,7 @@ acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, //////////////////////////////////////////////////////////////////////////////// -// computations that are done once when solver is created -void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); + // void ocp_nlp_cost_ls_config_initialize_default(void *config); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h index 5ec68cf580a3e8..aafb6b35407cfd 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -35,7 +38,8 @@ /// @{ /// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls /// \brief This module implements nonlinear-least squares costs of the form -/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$, +/// \f$\min_{x,u} \| r(x,u) - y_{\text{ref}} \|_W^2\f$. +/// @{ #ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ #define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ @@ -61,7 +65,6 @@ extern "C" { typedef struct { int nx; // number of states - int nz; // number of algebraic variables int nu; // number of inputs int ny; // number of outputs int ns; // number of slacks @@ -72,6 +75,8 @@ acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config); // void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory); // +void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); +// void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -94,7 +99,6 @@ typedef struct struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector struct blasfeo_dvec z; // gradient of slacks as vector double scaling; - int W_changed; ///< flag indicating whether W has changed and needs to be refactorized } ocp_nlp_cost_nls_model; // @@ -140,11 +144,11 @@ typedef struct struct blasfeo_dvec grad; // gradient of cost function struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out - struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in - struct blasfeo_dvec *Z; // pointer to Z in qp_in struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out - double fun; ///< value of the cost function + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + double fun; ///< value of the cost function } ocp_nlp_cost_nls_memory; // @@ -176,11 +180,8 @@ typedef struct { struct blasfeo_dmat tmp_nv_ny; struct blasfeo_dmat tmp_nv_nv; - struct blasfeo_dmat Vz; - struct blasfeo_dmat Cyt_tilde; struct blasfeo_dvec tmp_ny; - struct blasfeo_dvec tmp_2ns; - struct blasfeo_dvec tmp_nz; + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny } ocp_nlp_cost_nls_workspace; // @@ -190,8 +191,6 @@ acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims * functions ************************************************/ -// -void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); // void ocp_nlp_cost_nls_config_initialize_default(void *config); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h index 43fe71b12fcfbc..f2128a85749cd6 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -65,6 +68,7 @@ typedef struct /* dims */ acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz); void (*dims_set)(void *config_, void *dims_, const char *field, int *value); void (*dims_get)(void *config_, void *dims_, const char *field, int* value); /* model */ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 3afdc9f4ed83f2..59a2df4f47a0ee 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -72,6 +75,10 @@ typedef struct acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config); // void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_cont_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, + int nu1, int nz); + // void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h index 6ea26a70106fc6..8b2a6177bff78a 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -65,6 +68,10 @@ typedef struct acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config); // void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_disc_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, + int nu1, int nz); + // void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h index 9388f3fd24651a..cd26788a5697d8 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h index cb523525e188bd..df313616801e56 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h index 84a023cb69f077..f6bd7dcafd2d77 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h index b571f3bac1508d..c085e00d5d248e 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h index 682ea206dc50ae..104c297207035b 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h index 7e12952c15265a..e0b854bc1a1e7f 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h index fdb96417f94d71..74cfb042e3edea 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 364d0f47178835..af22c06a178ea5 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -142,6 +145,12 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims /************************************************ * functions ************************************************/ + +void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_); +// +void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); // int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h index d1a45635e40547..caf6caf2f01bee 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h index f65f602c15c304..50b80850c6a33c 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h index d23e658b4806c4..14ac97bbf4eace 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h index 261606b8423a6a..91690e458d3e32 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h index 8db53a279dbca6..de6ce501b19a92 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h index a535503f215118..1a3b7b2fa2d420 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h index 51df1b1cd68067..4ae391a9ff993b 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h index 844f6048fe6220..b95a11114eb95d 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h index 3b875caeb50419..ad4d094516af45 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h index a78bc65bb9c621..d4b30e007b3fad 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -81,7 +84,6 @@ typedef struct acados_size_t (*dims_calculate_size)(void *config, int N); ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory); void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); - void (*dims_get)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims); void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory); void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); diff --git a/third_party/acados/include/acados/sim/sim_collocation_utils.h b/third_party/acados/include/acados/sim/sim_collocation_utils.h index 045d165cbccee2..40a0b1c0cc103c 100644 --- a/third_party/acados/include/acados/sim/sim_collocation_utils.h +++ b/third_party/acados/include/acados/sim/sim_collocation_utils.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_common.h b/third_party/acados/include/acados/sim/sim_common.h index c4bbd6ed2b8de1..1838d76f819d81 100644 --- a/third_party/acados/include/acados/sim/sim_common.h +++ b/third_party/acados/include/acados/sim/sim_common.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -146,8 +149,6 @@ typedef struct bool jac_reuse; // Newton_scheme *scheme; - double newton_tol; // optinally used in implicit integrators - // workspace void *work; diff --git a/third_party/acados/include/acados/sim/sim_erk_integrator.h b/third_party/acados/include/acados/sim/sim_erk_integrator.h index fd46cb4d99c035..24a00c70774090 100644 --- a/third_party/acados/include/acados/sim/sim_erk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_erk_integrator.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_gnsf.h b/third_party/acados/include/acados/sim/sim_gnsf.h index 404532a7327622..5524b384e06c9c 100644 --- a/third_party/acados/include/acados/sim/sim_gnsf.h +++ b/third_party/acados/include/acados/sim/sim_gnsf.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -43,12 +46,12 @@ extern "C" { #include "acados/sim/sim_common.h" #include "blasfeo/include/blasfeo_common.h" -// #include "blasfeo/include/blasfeo_d_aux.h" -// #include "blasfeo/include/blasfeo_d_aux_ext_dep.h" -// #include "blasfeo/include/blasfeo_d_blas.h" -// #include "blasfeo/include/blasfeo_d_kernel.h" -// #include "blasfeo/include/blasfeo_i_aux_ext_dep.h" -// #include "blasfeo/include/blasfeo_target.h" +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" +#include "blasfeo/include/blasfeo_d_blas.h" +#include "blasfeo/include/blasfeo_d_kernel.h" +#include "blasfeo/include/blasfeo_i_aux_ext_dep.h" +#include "blasfeo/include/blasfeo_target.h" /* GNSF - Generalized Nonlinear Static Feedback Model diff --git a/third_party/acados/include/acados/sim/sim_irk_integrator.h b/third_party/acados/include/acados/sim/sim_irk_integrator.h index 5090aa0bb52604..6851bacb3a7902 100644 --- a/third_party/acados/include/acados/sim/sim_irk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_irk_integrator.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h index e60bb80ebf8b2f..9ec2d97bed944d 100644 --- a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/external_function_generic.h b/third_party/acados/include/acados/utils/external_function_generic.h index 1e68dc155db82d..021363f26eced3 100644 --- a/third_party/acados/include/acados/utils/external_function_generic.h +++ b/third_party/acados/include/acados/utils/external_function_generic.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -134,8 +137,8 @@ typedef struct int (*casadi_work)(int *, int *, int *, int *); const int *(*casadi_sparsity_in)(int); const int *(*casadi_sparsity_out)(int); - int (*casadi_n_in)(void); - int (*casadi_n_out)(void); + int (*casadi_n_in)(); + int (*casadi_n_out)(); double **args; double **res; double *w; @@ -192,8 +195,8 @@ typedef struct int (*casadi_work)(int *, int *, int *, int *); const int *(*casadi_sparsity_in)(int); const int *(*casadi_sparsity_out)(int); - int (*casadi_n_in)(void); - int (*casadi_n_out)(void); + int (*casadi_n_in)(); + int (*casadi_n_out)(); double **args; double **res; double *w; diff --git a/third_party/acados/include/acados/utils/math.h b/third_party/acados/include/acados/utils/math.h index 7156a82084267f..fe1da875f6e600 100644 --- a/third_party/acados/include/acados/utils/math.h +++ b/third_party/acados/include/acados/utils/math.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -39,7 +42,6 @@ extern "C" { #if defined(__MABX2__) double fmax(double a, double b); -int isnan(double x); #endif #define MIN(a,b) (((a)<(b))?(a):(b)) diff --git a/third_party/acados/include/acados/utils/mem.h b/third_party/acados/include/acados/utils/mem.h index 681a371e360f64..7b9efc5ed8d2a5 100644 --- a/third_party/acados/include/acados/utils/mem.h +++ b/third_party/acados/include/acados/utils/mem.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h index 824d3cee224a99..f8568afb26ff05 100644 --- a/third_party/acados/include/acados/utils/print.h +++ b/third_party/acados/include/acados/utils/print.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/strsep.h b/third_party/acados/include/acados/utils/strsep.h index 62bdfb48913266..02f1835593a244 100644 --- a/third_party/acados/include/acados/utils/strsep.h +++ b/third_party/acados/include/acados/utils/strsep.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/timing.h b/third_party/acados/include/acados/utils/timing.h index b0955932da3a2f..fe561d38914306 100644 --- a/third_party/acados/include/acados/utils/timing.h +++ b/third_party/acados/include/acados/utils/timing.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/types.h b/third_party/acados/include/acados/utils/types.h index a27ef9e552ce62..d3da0a86b7405a 100644 --- a/third_party/acados/include/acados/utils/types.h +++ b/third_party/acados/include/acados/utils/types.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -75,7 +78,7 @@ typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, doub enum return_values { ACADOS_SUCCESS, - ACADOS_NAN_DETECTED, + ACADOS_FAILURE, ACADOS_MAXITER, ACADOS_MINSTEP, ACADOS_QP_FAILURE, diff --git a/third_party/acados/include/acados_c/condensing_interface.h b/third_party/acados/include/acados_c/condensing_interface.h index b4302078d63516..51fe8271279ac5 100644 --- a/third_party/acados/include/acados_c/condensing_interface.h +++ b/third_party/acados/include/acados_c/condensing_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados_c/dense_qp_interface.h b/third_party/acados/include/acados_c/dense_qp_interface.h index b3af4bf68250a2..4ecc77381d669b 100644 --- a/third_party/acados/include/acados_c/dense_qp_interface.h +++ b/third_party/acados/include/acados_c/dense_qp_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -38,7 +41,7 @@ extern "C" { #include "acados/dense_qp/dense_qp_common.h" -typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP, DENSE_QP_DAQP } dense_qp_solver_t; +typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t; typedef struct { diff --git a/third_party/acados/include/acados_c/external_function_interface.h b/third_party/acados/include/acados_c/external_function_interface.h index d4f52db8505981..6838975071476c 100644 --- a/third_party/acados/include/acados_c/external_function_interface.h +++ b/third_party/acados/include/acados_c/external_function_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h index dd3e596f8be763..50bc0cf1af2944 100644 --- a/third_party/acados/include/acados_c/ocp_nlp_interface.h +++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -63,7 +66,6 @@ typedef enum { LINEAR_LS, NONLINEAR_LS, - CONVEX_OVER_NONLINEAR, EXTERNAL, INVALID_COST, } ocp_nlp_cost_t; @@ -244,10 +246,6 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_ ACADOS_SYMBOL_EXPORT int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage, const char *field, void *value); -/// -ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in, int stage, const char *field, void *value); - /* out */ /// Constructs an output struct for the non-linear program. @@ -299,7 +297,7 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config * ACADOS_SYMBOL_EXPORT void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out); -ACADOS_SYMBOL_EXPORT void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out); /* opts */ @@ -388,6 +386,8 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in * /// \param nlp_out The output struct. ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); +// +void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); /// Computes the residuals. /// diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h index 3dc3f1a532e50c..2582f142da1320 100644 --- a/third_party/acados/include/acados_c/ocp_qp_interface.h +++ b/third_party/acados/include/acados_c/ocp_qp_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * @@ -84,11 +87,6 @@ typedef enum { #else FULL_CONDENSING_QPOASES_NOT_AVAILABLE, #endif -#ifdef ACADOS_WITH_DAQP - FULL_CONDENSING_DAQP, -#else - FULL_CONDENSING_DAQP_NOT_AVAILABLE, -#endif #ifdef ACADOS_WITH_QORE FULL_CONDENSING_QORE, #else diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h index 09a05d6995cf3d..5dce6f153a7b00 100644 --- a/third_party/acados/include/acados_c/sim_interface.h +++ b/third_party/acados/include/acados_c/sim_interface.h @@ -1,5 +1,8 @@ /* - * Copyright (c) The acados authors. + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * This file is part of acados. * diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h index 2eab1e40949c22..ff9e9d4c16cba6 100644 --- a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h @@ -50,20 +50,18 @@ #define BLASFEO_CBLAS_ENUM #ifdef FORTRAN_BLAS_API #ifndef CBLAS_H -enum CBLAS_LAYOUT {CblasRowMajor=101, CblasColMajor=102}; +enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102}; enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; enum CBLAS_UPLO {CblasUpper=121, CblasLower=122}; enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; enum CBLAS_SIDE {CblasLeft=141, CblasRight=142}; -#define CBLAS_ORDER CBLAS_LAYOUT /* this for backward compatibility with CBLAS_ORDER */ #endif // CBLAS_H #else // FORTRAN_BLAS_API -enum BLASFEO_CBLAS_LAYOUT {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102}; +enum BLASFEO_CBLAS_ORDER {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102}; enum BLASFEO_CBLAS_TRANSPOSE {BlasfeoCblasNoTrans=111, BlasfeoCblasTrans=112, BlasfeoCblasConjTrans=113}; enum BLASFEO_CBLAS_UPLO {BlasfeoCblasUpper=121, BlasfeoCblasLower=122}; enum BLASFEO_CBLAS_DIAG {BlasfeoCblasNonUnit=131, BlasfeoCblasUnit=132}; enum BLASFEO_CBLAS_SIDE {BlasfeoCblasLeft=141, BlasfeoCblasRight=142}; -#define BLASFEO_CBLAS_ORDER BLASFEO_CBLAS_LAYOUT /* this for backward compatibility with BLASFEO_CBLAS_ORDER */ #endif // FORTRAN_BLAS_API #endif // BLASFEO_CBLAS_ENUM #endif // CBLAS_API @@ -153,19 +151,15 @@ void cblas_dswap(const int N, double *X, const int incX, double *Y, const int in // void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); -// CBLAS 2 -// -void cblas_dgemv(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); - // CBLAS 3 // -void cblas_dgemm(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); // -void cblas_dsyrk(const enum CBLAS_LAYOUT layout, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +void cblas_dsyrk(const enum CBLAS_ORDER Order, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); // -void cblas_dtrmm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void cblas_dtrmm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); // -void cblas_dtrsm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void cblas_dtrsm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); @@ -246,19 +240,15 @@ void blasfeo_cblas_dswap(const int N, double *X, const int incX, double *Y, cons // void blasfeo_cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); -// CBLAS 2 -// -void blasfeo_cblas_dgemv(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); - // CBLAS 3 // -void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); // -void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); // -void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); // -void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); diff --git a/third_party/acados/include/blasfeo/include/blasfeo_target.h b/third_party/acados/include/blasfeo/include/blasfeo_target.h index 51f617a6499273..4ff4f307b397ae 100644 --- a/third_party/acados/include/blasfeo/include/blasfeo_target.h +++ b/third_party/acados/include/blasfeo/include/blasfeo_target.h @@ -1,13 +1,13 @@ -#ifndef TARGET_X64_INTEL_HASWELL -#define TARGET_X64_INTEL_HASWELL +#ifndef TARGET_ARMV8A_ARM_CORTEX_A57 +#define TARGET_ARMV8A_ARM_CORTEX_A57 #endif #ifndef TARGET_NEED_FEATURE_AVX2 -#define TARGET_NEED_FEATURE_AVX2 1 +/* #undef TARGET_NEED_FEATURE_AVX2 */ #endif #ifndef TARGET_NEED_FEATURE_FMA -#define TARGET_NEED_FEATURE_FMA 1 +/* #undef TARGET_NEED_FEATURE_FMA */ #endif #ifndef TARGET_NEED_FEATURE_SSE3 @@ -27,11 +27,11 @@ #endif #ifndef TARGET_NEED_FEATURE_VFPv4 -/* #undef TARGET_NEED_FEATURE_VFPv4 */ +#define TARGET_NEED_FEATURE_VFPv4 1 #endif #ifndef TARGET_NEED_FEATURE_NEONv2 -/* #undef TARGET_NEED_FEATURE_NEONv2 */ +#define TARGET_NEED_FEATURE_NEONv2 1 #endif #ifndef LA_HIGH_PERFORMANCE diff --git a/third_party/acados/include/qpOASES_e/Constants.h b/third_party/acados/include/qpOASES_e/Constants.h index 13c777d75b83a3..0e3dcd19f4452f 100644 --- a/third_party/acados/include/qpOASES_e/Constants.h +++ b/third_party/acados/include/qpOASES_e/Constants.h @@ -1,134 +1,134 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/Constants.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - * - * Definition of all global constants. - */ - - -#ifndef QPOASES_CONSTANTS_H -#define QPOASES_CONSTANTS_H - - -#include - -#ifdef __CODE_GENERATION__ - - #define CONVERTTOSTRINGAUX(x) #x - #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x) - - #ifndef QPOASES_CUSTOM_INTERFACE - #include "acado_qpoases3_interface.h" - #else - #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE) - #endif - -#endif - - -BEGIN_NAMESPACE_QPOASES - - -#ifndef __EXTERNAL_DIMENSIONS__ - - /*#define QPOASES_NVMAX 50 - #define QPOASES_NCMAX 100*/ - #define QPOASES_NVMAX 287 - #define QPOASES_NCMAX 709 - -#endif /* __EXTERNAL_DIMENSIONS__ */ - - -/** Maximum number of variables within a QP formulation. - * Note: this value has to be positive! */ -#define NVMAX QPOASES_NVMAX - -/** Maximum number of constraints within a QP formulation. - * Note: this value has to be positive! */ -#define NCMAX QPOASES_NCMAX - -#if ( QPOASES_NVMAX > QPOASES_NCMAX ) -#define NVCMAX QPOASES_NVMAX -#else -#define NVCMAX QPOASES_NCMAX -#endif - -#if ( QPOASES_NVMAX > QPOASES_NCMAX ) -#define NVCMIN QPOASES_NCMAX -#else -#define NVCMIN QPOASES_NVMAX -#endif - - -/** Maximum number of QPs in a sequence solved by means of the OQP interface. - * Note: this value has to be positive! */ -#define NQPMAX 1000 - - -/** Numerical value of machine precision (min eps, s.t. 1+eps > 1). - * Note: this value has to be positive! */ -#ifndef __CODE_GENERATION__ - - #ifdef __USE_SINGLE_PRECISION__ - static const real_t QPOASES_EPS = 1.193e-07; - #else - static const real_t QPOASES_EPS = 2.221e-16; - #endif /* __USE_SINGLE_PRECISION__ */ - -#endif /* __CODE_GENERATION__ */ - - -/** Numerical value of zero (for situations in which it would be - * unreasonable to compare with 0.0). - * Note: this value has to be positive! */ -static const real_t QPOASES_ZERO = 1.0e-25; - -/** Numerical value of infinity (e.g. for non-existing bounds). - * Note: this value has to be positive! */ -static const real_t QPOASES_INFTY = 1.0e20; - -/** Tolerance to used for isEqual, isZero etc. - * Note: this value has to be positive! */ -static const real_t QPOASES_TOL = 1.0e-25; - - -/** Maximum number of characters within a string. - * Note: this value should be at least 41! */ -#define QPOASES_MAX_STRING_LENGTH 160 - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_CONSTANTS_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Constants.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Definition of all global constants. + */ + + +#ifndef QPOASES_CONSTANTS_H +#define QPOASES_CONSTANTS_H + + +#include + +#ifdef __CODE_GENERATION__ + + #define CONVERTTOSTRINGAUX(x) #x + #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x) + + #ifndef QPOASES_CUSTOM_INTERFACE + #include "acado_qpoases3_interface.h" + #else + #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE) + #endif + +#endif + + +BEGIN_NAMESPACE_QPOASES + + +#ifndef __EXTERNAL_DIMENSIONS__ + + /*#define QPOASES_NVMAX 50 + #define QPOASES_NCMAX 100*/ + #define QPOASES_NVMAX 287 + #define QPOASES_NCMAX 709 + +#endif /* __EXTERNAL_DIMENSIONS__ */ + + +/** Maximum number of variables within a QP formulation. + * Note: this value has to be positive! */ +#define NVMAX QPOASES_NVMAX + +/** Maximum number of constraints within a QP formulation. + * Note: this value has to be positive! */ +#define NCMAX QPOASES_NCMAX + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMAX QPOASES_NVMAX +#else +#define NVCMAX QPOASES_NCMAX +#endif + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMIN QPOASES_NCMAX +#else +#define NVCMIN QPOASES_NVMAX +#endif + + +/** Maximum number of QPs in a sequence solved by means of the OQP interface. + * Note: this value has to be positive! */ +#define NQPMAX 1000 + + +/** Numerical value of machine precision (min eps, s.t. 1+eps > 1). + * Note: this value has to be positive! */ +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + static const real_t QPOASES_EPS = 1.193e-07; + #else + static const real_t QPOASES_EPS = 2.221e-16; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ + + +/** Numerical value of zero (for situations in which it would be + * unreasonable to compare with 0.0). + * Note: this value has to be positive! */ +static const real_t QPOASES_ZERO = 1.0e-25; + +/** Numerical value of infinity (e.g. for non-existing bounds). + * Note: this value has to be positive! */ +static const real_t QPOASES_INFTY = 1.0e20; + +/** Tolerance to used for isEqual, isZero etc. + * Note: this value has to be positive! */ +static const real_t QPOASES_TOL = 1.0e-25; + + +/** Maximum number of characters within a string. + * Note: this value should be at least 41! */ +#define QPOASES_MAX_STRING_LENGTH 160 + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_CONSTANTS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/ConstraintProduct.h b/third_party/acados/include/qpOASES_e/ConstraintProduct.h index eb5400c6cb91fa..ddfcfbe5dcd96a 100644 --- a/third_party/acados/include/qpOASES_e/ConstraintProduct.h +++ b/third_party/acados/include/qpOASES_e/ConstraintProduct.h @@ -1,62 +1,62 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/ConstraintProduct.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor) - * \version 3.1embedded - * \date 2009-2015 - * - * Declaration of the ConstraintProduct interface which allows to specify a - * user-defined function for evaluating the constraint product at the - * current iterate to speed-up QP solution in case of a specially structured - * constraint matrix. - */ - - - -#ifndef QPOASES_CONSTRAINT_PRODUCT_H -#define QPOASES_CONSTRAINT_PRODUCT_H - - -BEGIN_NAMESPACE_QPOASES - - -/** - * \brief Interface for specifying user-defined evaluations of constraint products. - * - * An interface which allows to specify a user-defined function for evaluating the - * constraint product at the current iterate to speed-up QP solution in case - * of a specially structured constraint matrix. - * - * \author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor) - * \version 3.1embedded - * \date 2009-2015 - */ -typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const ); - - -END_NAMESPACE_QPOASES - -#endif /* QPOASES_CONSTRAINT_PRODUCT_H */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/ConstraintProduct.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + * + * Declaration of the ConstraintProduct interface which allows to specify a + * user-defined function for evaluating the constraint product at the + * current iterate to speed-up QP solution in case of a specially structured + * constraint matrix. + */ + + + +#ifndef QPOASES_CONSTRAINT_PRODUCT_H +#define QPOASES_CONSTRAINT_PRODUCT_H + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interface for specifying user-defined evaluations of constraint products. + * + * An interface which allows to specify a user-defined function for evaluating the + * constraint product at the current iterate to speed-up QP solution in case + * of a specially structured constraint matrix. + * + * \author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + */ +typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const ); + + +END_NAMESPACE_QPOASES + +#endif /* QPOASES_CONSTRAINT_PRODUCT_H */ diff --git a/third_party/acados/include/qpOASES_e/Indexlist.h b/third_party/acados/include/qpOASES_e/Indexlist.h index 02d259d63d03db..c3026a7ffc29a3 100644 --- a/third_party/acados/include/qpOASES_e/Indexlist.h +++ b/third_party/acados/include/qpOASES_e/Indexlist.h @@ -1,221 +1,221 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/Indexlist.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - * - * Declaration of the Indexlist class designed to manage index lists of - * constraints and bounds within a SubjectTo object. - */ - - -#ifndef QPOASES_INDEXLIST_H -#define QPOASES_INDEXLIST_H - - -#include - - -BEGIN_NAMESPACE_QPOASES - - -/** - * \brief Stores and manages index lists. - * - * This class manages index lists of active/inactive bounds/constraints. - * - * \author Hans Joachim Ferreau - * \version 3.1embedded - * \date 2007-2015 - */ -typedef struct -{ - int *number; /**< Array to store numbers of constraints or bounds. */ - int *iSort; /**< Index list to sort vector \a number */ - - int length; /**< Length of index list. */ - int first; /**< Physical index of first element. */ - int last; /**< Physical index of last element. */ - int lastusedindex; /**< Physical index of last entry in index list. */ - int physicallength; /**< Physical length of index list. */ -} Indexlist; - -int Indexlist_calculateMemorySize( int n); - -char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory); - -Indexlist *Indexlist_createMemory( int n ); - -/** Constructor which takes the desired physical length of the index list. */ -void IndexlistCON( Indexlist* _THIS, - int n /**< Physical length of index list. */ - ); - -/** Copies all members from given rhs object. - * \return SUCCESSFUL_RETURN */ -void IndexlistCPY( Indexlist* FROM, - Indexlist* TO - ); - -/** Initialises index list of desired physical length. - * \return SUCCESSFUL_RETURN \n - RET_INVALID_ARGUMENTS */ -returnValue Indexlist_init( Indexlist* _THIS, - int n /**< Physical length of index list. */ - ); - -/** Creates an array of all numbers within the index set in correct order. - * \return SUCCESSFUL_RETURN \n - RET_INDEXLIST_CORRUPTED */ -returnValue Indexlist_getNumberArray( Indexlist* _THIS, - int** const numberarray /**< Output: Array of numbers (NULL on error). */ - ); - -/** Creates an array of all numbers within the index set in correct order. - * \return SUCCESSFUL_RETURN \n - RET_INDEXLIST_CORRUPTED */ -returnValue Indexlist_getISortArray( Indexlist* _THIS, - int** const iSortArray /**< Output: iSort Array. */ - ); - - -/** Determines the index within the index list at which a given number is stored. - * \return >= 0: Index of given number. \n - -1: Number not found. */ -int Indexlist_getIndex( Indexlist* _THIS, - int givennumber /**< Number whose index shall be determined. */ - ); - -/** Returns the number stored at a given physical index. - * \return >= 0: Number stored at given physical index. \n - -RET_INDEXLIST_OUTOFBOUNDS */ -static inline int Indexlist_getNumber( Indexlist* _THIS, - int physicalindex /**< Physical index of the number to be returned. */ - ); - - -/** Returns the current length of the index list. - * \return Current length of the index list. */ -static inline int Indexlist_getLength( Indexlist* _THIS - ); - -/** Returns last number within the index list. - * \return Last number within the index list. */ -static inline int Indexlist_getLastNumber( Indexlist* _THIS - ); - - -/** Adds number to index list. - * \return SUCCESSFUL_RETURN \n - RET_INDEXLIST_MUST_BE_REORDERD \n - RET_INDEXLIST_EXCEEDS_MAX_LENGTH */ -returnValue Indexlist_addNumber( Indexlist* _THIS, - int addnumber /**< Number to be added. */ - ); - -/** Removes number from index list. - * \return SUCCESSFUL_RETURN */ -returnValue Indexlist_removeNumber( Indexlist* _THIS, - int removenumber /**< Number to be removed. */ - ); - -/** Swaps two numbers within index list. - * \return SUCCESSFUL_RETURN */ -returnValue Indexlist_swapNumbers( Indexlist* _THIS, - int number1, /**< First number for swapping. */ - int number2 /**< Second number for swapping. */ - ); - -/** Determines if a given number is contained in the index set. - * \return BT_TRUE iff number is contain in the index set */ -static inline BooleanType Indexlist_isMember( Indexlist* _THIS, - int _number /**< Number to be tested for membership. */ - ); - - -/** Find first index j between -1 and length in sorted list of indices - * iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses - * bisection. - * \return j. */ -int Indexlist_findInsert( Indexlist* _THIS, - int i - ); - - - -/* - * g e t N u m b e r - */ -static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex ) -{ - /* consistency check */ - if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) ) - return -RET_INDEXLIST_OUTOFBOUNDS; - - return _THIS->number[physicalindex]; -} - - -/* - * g e t L e n g t h - */ -static inline int Indexlist_getLength( Indexlist* _THIS ) -{ - return _THIS->length; -} - - -/* - * g e t L a s t N u m b e r - */ -static inline int Indexlist_getLastNumber( Indexlist* _THIS ) -{ - return _THIS->number[_THIS->length-1]; -} - - -/* - * g e t L a s t N u m b e r - */ -static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number ) -{ - if ( Indexlist_getIndex( _THIS,_number ) >= 0 ) - return BT_TRUE; - else - return BT_FALSE; -} - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_INDEXLIST_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Indexlist.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Indexlist class designed to manage index lists of + * constraints and bounds within a SubjectTo object. + */ + + +#ifndef QPOASES_INDEXLIST_H +#define QPOASES_INDEXLIST_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Stores and manages index lists. + * + * This class manages index lists of active/inactive bounds/constraints. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + int *number; /**< Array to store numbers of constraints or bounds. */ + int *iSort; /**< Index list to sort vector \a number */ + + int length; /**< Length of index list. */ + int first; /**< Physical index of first element. */ + int last; /**< Physical index of last element. */ + int lastusedindex; /**< Physical index of last entry in index list. */ + int physicallength; /**< Physical length of index list. */ +} Indexlist; + +int Indexlist_calculateMemorySize( int n); + +char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory); + +Indexlist *Indexlist_createMemory( int n ); + +/** Constructor which takes the desired physical length of the index list. */ +void IndexlistCON( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void IndexlistCPY( Indexlist* FROM, + Indexlist* TO + ); + +/** Initialises index list of desired physical length. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Indexlist_init( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getNumberArray( Indexlist* _THIS, + int** const numberarray /**< Output: Array of numbers (NULL on error). */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getISortArray( Indexlist* _THIS, + int** const iSortArray /**< Output: iSort Array. */ + ); + + +/** Determines the index within the index list at which a given number is stored. + * \return >= 0: Index of given number. \n + -1: Number not found. */ +int Indexlist_getIndex( Indexlist* _THIS, + int givennumber /**< Number whose index shall be determined. */ + ); + +/** Returns the number stored at a given physical index. + * \return >= 0: Number stored at given physical index. \n + -RET_INDEXLIST_OUTOFBOUNDS */ +static inline int Indexlist_getNumber( Indexlist* _THIS, + int physicalindex /**< Physical index of the number to be returned. */ + ); + + +/** Returns the current length of the index list. + * \return Current length of the index list. */ +static inline int Indexlist_getLength( Indexlist* _THIS + ); + +/** Returns last number within the index list. + * \return Last number within the index list. */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS + ); + + +/** Adds number to index list. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_MUST_BE_REORDERD \n + RET_INDEXLIST_EXCEEDS_MAX_LENGTH */ +returnValue Indexlist_addNumber( Indexlist* _THIS, + int addnumber /**< Number to be added. */ + ); + +/** Removes number from index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_removeNumber( Indexlist* _THIS, + int removenumber /**< Number to be removed. */ + ); + +/** Swaps two numbers within index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_swapNumbers( Indexlist* _THIS, + int number1, /**< First number for swapping. */ + int number2 /**< Second number for swapping. */ + ); + +/** Determines if a given number is contained in the index set. + * \return BT_TRUE iff number is contain in the index set */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, + int _number /**< Number to be tested for membership. */ + ); + + +/** Find first index j between -1 and length in sorted list of indices + * iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses + * bisection. + * \return j. */ +int Indexlist_findInsert( Indexlist* _THIS, + int i + ); + + + +/* + * g e t N u m b e r + */ +static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex ) +{ + /* consistency check */ + if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) ) + return -RET_INDEXLIST_OUTOFBOUNDS; + + return _THIS->number[physicalindex]; +} + + +/* + * g e t L e n g t h + */ +static inline int Indexlist_getLength( Indexlist* _THIS ) +{ + return _THIS->length; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS ) +{ + return _THIS->number[_THIS->length-1]; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number ) +{ + if ( Indexlist_getIndex( _THIS,_number ) >= 0 ) + return BT_TRUE; + else + return BT_FALSE; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_INDEXLIST_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Matrices.h b/third_party/acados/include/qpOASES_e/Matrices.h index e2a46b3a9daeda..1d6da8c3c17822 100644 --- a/third_party/acados/include/qpOASES_e/Matrices.h +++ b/third_party/acados/include/qpOASES_e/Matrices.h @@ -1,287 +1,287 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/Matrices.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2009-2015 - * - * Various matrix classes: Abstract base matrix class, dense and sparse matrices, - * including symmetry exploiting specializations. - */ - - - -#ifndef QPOASES_MATRICES_H -#define QPOASES_MATRICES_H - -#ifdef __USE_SINGLE_PRECISION__ - - // single precision - #define GEMM sgemm_ - #define GEMV sgemv_ -// #define SYR ssyr_ -// #define SYR2 ssyr2_ - #define POTRF spotrf_ - -#else - - // double precision - #define GEMM dgemm_ - #define GEMV dgemv_ -// #define SYR dsyr_ -// #define SYR2 dsyr2_ - #define POTRF dpotrf_ - -#endif /* __USE_SINGLE_PRECISION__ */ - - -#ifdef EXTERNAL_BLAS - // double precision - void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc); - void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); - void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); - // single precision - void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc); - void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); - void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); -#else - /** Performs one of the matrix-matrix operation in double precision. */ - void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, - const double*, const double*, const unsigned long*, const double*, const unsigned long*, - const double*, double*, const unsigned long* ); - /** Performs one of the matrix-matrix operation in single precision. */ - void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, - const float*, const float*, const unsigned long*, const float*, const unsigned long*, - const float*, float*, const unsigned long* ); - - /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */ - void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); - /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */ - void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); - -#endif - - /** Performs a symmetric rank 1 operation in double precision. */ -// void dsyr_ ( const char *, const unsigned long *, const double *, const double *, -// const unsigned long *, double *, const unsigned long *); - /** Performs a symmetric rank 1 operation in single precision. */ -// void ssyr_ ( const char *, const unsigned long *, const float *, const float *, -// const unsigned long *, float *, const unsigned long *); - - /** Performs a symmetric rank 2 operation in double precision. */ -// void dsyr2_ ( const char *, const unsigned long *, const double *, const double *, -// const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *); - /** Performs a symmetric rank 2 operation in single precision. */ -// void ssyr2_ ( const char *, const unsigned long *, const float *, const float *, -// const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *); - - -#include - - -BEGIN_NAMESPACE_QPOASES - - -/** - * \brief Interfaces matrix-vector operations tailored to general dense matrices. - * - * Dense matrix class (row major format). - * - * \author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau - * \version 3.1embedded - * \date 2011-2015 - */ -typedef struct -{ - real_t *val; /**< Vector of entries. */ - int nRows; /**< Number of rows. */ - int nCols; /**< Number of columns. */ - int leaDim; /**< Leading dimension. */ -} DenseMatrix; - -int DenseMatrix_calculateMemorySize( int m, int n ); - -char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory ); - -DenseMatrix *DenseMatrix_createMemory( int m, int n ); - -/** Constructor from vector of values. - * Caution: Data pointer must be valid throughout lifetime - */ -void DenseMatrixCON( DenseMatrix* _THIS, - int m, /**< Number of rows. */ - int n, /**< Number of columns. */ - int lD, /**< Leading dimension. */ - real_t *v /**< Values. */ - ); - -void DenseMatrixCPY( DenseMatrix* FROM, - DenseMatrix* TO - ); - - -/** Frees all internal memory. */ -void DenseMatrix_free( DenseMatrix* _THIS ); - -/** Constructor from vector of values. - * Caution: Data pointer must be valid throughout lifetime - */ -returnValue DenseMatrix_init( DenseMatrix* _THIS, - int m, /**< Number of rows. */ - int n, /**< Number of columns. */ - int lD, /**< Leading dimension. */ - real_t *v /**< Values. */ - ); - - -/** Returns i-th diagonal entry. - * \return i-th diagonal entry */ -real_t DenseMatrix_diag( DenseMatrix* _THIS, - int i /**< Index. */ - ); - -/** Checks whether matrix is square and diagonal. - * \return BT_TRUE iff matrix is square and diagonal; \n - * BT_FALSE otherwise. */ -BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS ); - -/** Get the N-norm of the matrix - * \return N-norm of the matrix - */ -real_t DenseMatrix_getNorm( DenseMatrix* _THIS, - int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ - ); - -/** Get the N-norm of a row - * \return N-norm of row \a rNum - */ -real_t DenseMatrix_getRowNorm( DenseMatrix* _THIS, - int rNum, /**< Row number. */ - int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ - ); - -/** Retrieve indexed entries of matrix row multiplied by alpha. - * \return SUCCESSFUL_RETURN */ -returnValue DenseMatrix_getRow( DenseMatrix* _THIS, - int rNum, /**< Row number. */ - const Indexlist* const icols, /**< Index list specifying columns. */ - real_t alpha, /**< Scalar factor. */ - real_t *row /**< Output row vector. */ - ); - -/** Retrieve indexed entries of matrix column multiplied by alpha. - * \return SUCCESSFUL_RETURN */ - returnValue DenseMatrix_getCol( DenseMatrix* _THIS, - int cNum, /**< Column number. */ - const Indexlist* const irows, /**< Index list specifying rows. */ - real_t alpha, /**< Scalar factor. */ - real_t *col /**< Output column vector. */ - ); - -/** Evaluate Y=alpha*A*X + beta*Y. - * \return SUCCESSFUL_RETURN. */ -returnValue DenseMatrix_times( DenseMatrix* _THIS, - int xN, /**< Number of vectors to multiply. */ - real_t alpha, /**< Scalar factor for matrix vector product. */ - const real_t *x, /**< Input vector to be multiplied. */ - int xLD, /**< Leading dimension of input x. */ - real_t beta, /**< Scalar factor for y. */ - real_t *y, /**< Output vector of results. */ - int yLD /**< Leading dimension of output y. */ - ); - -/** Evaluate Y=alpha*A'*X + beta*Y. - * \return SUCCESSFUL_RETURN. */ -returnValue DenseMatrix_transTimes( DenseMatrix* _THIS, - int xN, /**< Number of vectors to multiply. */ - real_t alpha, /**< Scalar factor for matrix vector product. */ - const real_t *x, /**< Input vector to be multiplied. */ - int xLD, /**< Leading dimension of input x. */ - real_t beta, /**< Scalar factor for y. */ - real_t *y, /**< Output vector of results. */ - int yLD /**< Leading dimension of output y. */ - ); - -/** Evaluate matrix vector product with submatrix given by Indexlist. - * \return SUCCESSFUL_RETURN */ - returnValue DenseMatrix_subTimes( DenseMatrix* _THIS, - const Indexlist* const irows, /**< Index list specifying rows. */ - const Indexlist* const icols, /**< Index list specifying columns. */ - int xN, /**< Number of vectors to multiply. */ - real_t alpha, /**< Scalar factor for matrix vector product. */ - const real_t *x, /**< Input vector to be multiplied. */ - int xLD, /**< Leading dimension of input x. */ - real_t beta, /**< Scalar factor for y. */ - real_t *y, /**< Output vector of results. */ - int yLD, /**< Leading dimension of output y. */ - BooleanType yCompr /**< Compressed storage for y. */ - ); - -/** Evaluate matrix transpose vector product. - * \return SUCCESSFUL_RETURN */ -returnValue DenseMatrix_subTransTimes( DenseMatrix* _THIS, - const Indexlist* const irows, /**< Index list specifying rows. */ - const Indexlist* const icols, /**< Index list specifying columns. */ - int xN, /**< Number of vectors to multiply. */ - real_t alpha, /**< Scalar factor for matrix vector product. */ - const real_t *x, /**< Input vector to be multiplied. */ - int xLD, /**< Leading dimension of input x. */ - real_t beta, /**< Scalar factor for y. */ - real_t *y, /**< Output vector of results. */ - int yLD /**< Leading dimension of output y. */ - ); - -/** Adds given offset to diagonal of matrix. - * \return SUCCESSFUL_RETURN \n - RET_NO_DIAGONAL_AVAILABLE */ -returnValue DenseMatrix_addToDiag( DenseMatrix* _THIS, - real_t alpha /**< Diagonal offset. */ - ); - -/** Prints matrix to screen. - * \return SUCCESSFUL_RETURN */ -returnValue DenseMatrix_print( DenseMatrix* _THIS - ); - -static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; } - -/** Compute bilinear form y = x'*H*x using submatrix given by index list. - * \return SUCCESSFUL_RETURN */ -returnValue DenseMatrix_bilinear( DenseMatrix* _THIS, - const Indexlist* const icols, /**< Index list specifying columns of x. */ - int xN, /**< Number of vectors to multiply. */ - const real_t *x, /**< Input vector to be multiplied (uncompressed). */ - int xLD, /**< Leading dimension of input x. */ - real_t *y, /**< Output vector of results (compressed). */ - int yLD /**< Leading dimension of output y. */ - ); - - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_MATRICES_H */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Matrices.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2009-2015 + * + * Various matrix classes: Abstract base matrix class, dense and sparse matrices, + * including symmetry exploiting specializations. + */ + + + +#ifndef QPOASES_MATRICES_H +#define QPOASES_MATRICES_H + +#ifdef __USE_SINGLE_PRECISION__ + + // single precision + #define GEMM sgemm_ + #define GEMV sgemv_ +// #define SYR ssyr_ +// #define SYR2 ssyr2_ + #define POTRF spotrf_ + +#else + + // double precision + #define GEMM dgemm_ + #define GEMV dgemv_ +// #define SYR dsyr_ +// #define SYR2 dsyr2_ + #define POTRF dpotrf_ + +#endif /* __USE_SINGLE_PRECISION__ */ + + +#ifdef EXTERNAL_BLAS + // double precision + void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc); + void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); + void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); + // single precision + void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc); + void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); + void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); +#else + /** Performs one of the matrix-matrix operation in double precision. */ + void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const double*, const double*, const unsigned long*, const double*, const unsigned long*, + const double*, double*, const unsigned long* ); + /** Performs one of the matrix-matrix operation in single precision. */ + void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const float*, const float*, const unsigned long*, const float*, const unsigned long*, + const float*, float*, const unsigned long* ); + + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */ + void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */ + void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); + +#endif + + /** Performs a symmetric rank 1 operation in double precision. */ +// void dsyr_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 1 operation in single precision. */ +// void ssyr_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, float *, const unsigned long *); + + /** Performs a symmetric rank 2 operation in double precision. */ +// void dsyr2_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 2 operation in single precision. */ +// void ssyr2_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *); + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interfaces matrix-vector operations tailored to general dense matrices. + * + * Dense matrix class (row major format). + * + * \author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau + * \version 3.1embedded + * \date 2011-2015 + */ +typedef struct +{ + real_t *val; /**< Vector of entries. */ + int nRows; /**< Number of rows. */ + int nCols; /**< Number of columns. */ + int leaDim; /**< Leading dimension. */ +} DenseMatrix; + +int DenseMatrix_calculateMemorySize( int m, int n ); + +char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory ); + +DenseMatrix *DenseMatrix_createMemory( int m, int n ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +void DenseMatrixCON( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + +void DenseMatrixCPY( DenseMatrix* FROM, + DenseMatrix* TO + ); + + +/** Frees all internal memory. */ +void DenseMatrix_free( DenseMatrix* _THIS ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +returnValue DenseMatrix_init( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + + +/** Returns i-th diagonal entry. + * \return i-th diagonal entry */ +real_t DenseMatrix_diag( DenseMatrix* _THIS, + int i /**< Index. */ + ); + +/** Checks whether matrix is square and diagonal. + * \return BT_TRUE iff matrix is square and diagonal; \n + * BT_FALSE otherwise. */ +BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS ); + +/** Get the N-norm of the matrix + * \return N-norm of the matrix + */ +real_t DenseMatrix_getNorm( DenseMatrix* _THIS, + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Get the N-norm of a row + * \return N-norm of row \a rNum + */ +real_t DenseMatrix_getRowNorm( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Retrieve indexed entries of matrix row multiplied by alpha. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_getRow( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + real_t alpha, /**< Scalar factor. */ + real_t *row /**< Output row vector. */ + ); + +/** Retrieve indexed entries of matrix column multiplied by alpha. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_getCol( DenseMatrix* _THIS, + int cNum, /**< Column number. */ + const Indexlist* const irows, /**< Index list specifying rows. */ + real_t alpha, /**< Scalar factor. */ + real_t *col /**< Output column vector. */ + ); + +/** Evaluate Y=alpha*A*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_times( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate Y=alpha*A'*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_transTimes( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate matrix vector product with submatrix given by Indexlist. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_subTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD, /**< Leading dimension of output y. */ + BooleanType yCompr /**< Compressed storage for y. */ + ); + +/** Evaluate matrix transpose vector product. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_subTransTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Adds given offset to diagonal of matrix. + * \return SUCCESSFUL_RETURN \n + RET_NO_DIAGONAL_AVAILABLE */ +returnValue DenseMatrix_addToDiag( DenseMatrix* _THIS, + real_t alpha /**< Diagonal offset. */ + ); + +/** Prints matrix to screen. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_print( DenseMatrix* _THIS + ); + +static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; } + +/** Compute bilinear form y = x'*H*x using submatrix given by index list. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_bilinear( DenseMatrix* _THIS, + const Indexlist* const icols, /**< Index list specifying columns of x. */ + int xN, /**< Number of vectors to multiply. */ + const real_t *x, /**< Input vector to be multiplied (uncompressed). */ + int xLD, /**< Leading dimension of input x. */ + real_t *y, /**< Output vector of results (compressed). */ + int yLD /**< Leading dimension of output y. */ + ); + + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_MATRICES_H */ diff --git a/third_party/acados/include/qpOASES_e/Options.h b/third_party/acados/include/qpOASES_e/Options.h index ca8086d2cc64ed..b471ee0668fd76 100644 --- a/third_party/acados/include/qpOASES_e/Options.h +++ b/third_party/acados/include/qpOASES_e/Options.h @@ -1,153 +1,153 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/Options.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - * - * Declaration of the Options class designed to manage user-specified - * options for solving a QProblem. - */ - - -#ifndef QPOASES_OPTIONS_H -#define QPOASES_OPTIONS_H - - -#include - - -BEGIN_NAMESPACE_QPOASES - - -/** - * \brief Manages all user-specified options for solving QPs. - * - * This class manages all user-specified options used for solving - * quadratic programs. - * - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - */ -typedef struct -{ - PrintLevel printLevel; /**< Print level. */ - - BooleanType enableRamping; /**< Specifies whether ramping shall be enabled or not. */ - BooleanType enableFarBounds; /**< Specifies whether far bounds shall be used or not. */ - BooleanType enableFlippingBounds; /**< Specifies whether flipping bounds shall be used or not. */ - BooleanType enableRegularisation; /**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */ - BooleanType enableFullLITests; /**< Specifies whether condition-hardened LI test shall be used or not. */ - BooleanType enableNZCTests; /**< Specifies whether nonzero curvature tests shall be used. */ - int enableDriftCorrection; /**< Specifies the frequency of drift corrections (0 = off). */ - int enableCholeskyRefactorisation; /**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */ - BooleanType enableEqualities; /**< Specifies whether equalities shall be always treated as active constraints. */ - - real_t terminationTolerance; /**< Termination tolerance. */ - real_t boundTolerance; /**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */ - real_t boundRelaxation; /**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */ - real_t epsNum; /**< Numerator tolerance for ratio tests. */ - real_t epsDen; /**< Denominator tolerance for ratio tests. */ - real_t maxPrimalJump; /**< Maximum allowed jump in primal variables in nonzero curvature tests. */ - real_t maxDualJump; /**< Maximum allowed jump in dual variables in linear independence tests. */ - - real_t initialRamping; /**< Start value for Ramping Strategy. */ - real_t finalRamping; /**< Final value for Ramping Strategy. */ - real_t initialFarBounds; /**< Initial size of Far Bounds. */ - real_t growFarBounds; /**< Factor to grow Far Bounds. */ - SubjectToStatus initialStatusBounds; /**< Initial status of bounds at first iteration. */ - real_t epsFlipping; /**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */ - int numRegularisationSteps; /**< Maximum number of successive regularisation steps. */ - real_t epsRegularisation; /**< Scaling factor of identity matrix used for Hessian regularisation. */ - int numRefinementSteps; /**< Maximum number of iterative refinement steps. */ - real_t epsIterRef; /**< Early termination tolerance for iterative refinement. */ - real_t epsLITests; /**< Tolerance for linear independence tests. */ - real_t epsNZCTests; /**< Tolerance for nonzero curvature tests. */ - - BooleanType enableDropInfeasibles; /**< ... */ - int dropBoundPriority; /**< ... */ - int dropEqConPriority; /**< ... */ - int dropIneqConPriority; /**< ... */ -} Options; - - -void OptionsCON( Options* _THIS - ); - -/** Copies all members from given rhs object. - * \return SUCCESSFUL_RETURN */ -void OptionsCPY( Options* FROM, - Options* TO - ); - - -/** Sets all options to default values. - * \return SUCCESSFUL_RETURN */ -returnValue Options_setToDefault( Options* _THIS - ); - -/** Sets all options to values resulting in maximum reliabilty. - * \return SUCCESSFUL_RETURN */ -returnValue Options_setToReliable( Options* _THIS - ); - -/** Sets all options to values resulting in minimum solution time. - * \return SUCCESSFUL_RETURN */ -returnValue Options_setToMPC( Options* _THIS - ); - -/** Same as setToMPC( ), for ensuring backwards compatibility. - * \return SUCCESSFUL_RETURN */ -returnValue Options_setToFast( Options* _THIS - ); - - -/** Ensures that all options have consistent values by automatically - * adjusting inconsistent ones. - * Note: This routine cannot (and does not try to) ensure that values - * are set to reasonable values that make the QP solution work! - * \return SUCCESSFUL_RETURN \n - * RET_OPTIONS_ADJUSTED */ -returnValue Options_ensureConsistency( Options* _THIS - ); - - -/** Prints values of all options. - * \return SUCCESSFUL_RETURN */ -returnValue Options_print( Options* _THIS - ); - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_OPTIONS_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Options.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Options class designed to manage user-specified + * options for solving a QProblem. + */ + + +#ifndef QPOASES_OPTIONS_H +#define QPOASES_OPTIONS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages all user-specified options for solving QPs. + * + * This class manages all user-specified options used for solving + * quadratic programs. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + PrintLevel printLevel; /**< Print level. */ + + BooleanType enableRamping; /**< Specifies whether ramping shall be enabled or not. */ + BooleanType enableFarBounds; /**< Specifies whether far bounds shall be used or not. */ + BooleanType enableFlippingBounds; /**< Specifies whether flipping bounds shall be used or not. */ + BooleanType enableRegularisation; /**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */ + BooleanType enableFullLITests; /**< Specifies whether condition-hardened LI test shall be used or not. */ + BooleanType enableNZCTests; /**< Specifies whether nonzero curvature tests shall be used. */ + int enableDriftCorrection; /**< Specifies the frequency of drift corrections (0 = off). */ + int enableCholeskyRefactorisation; /**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */ + BooleanType enableEqualities; /**< Specifies whether equalities shall be always treated as active constraints. */ + + real_t terminationTolerance; /**< Termination tolerance. */ + real_t boundTolerance; /**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */ + real_t boundRelaxation; /**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */ + real_t epsNum; /**< Numerator tolerance for ratio tests. */ + real_t epsDen; /**< Denominator tolerance for ratio tests. */ + real_t maxPrimalJump; /**< Maximum allowed jump in primal variables in nonzero curvature tests. */ + real_t maxDualJump; /**< Maximum allowed jump in dual variables in linear independence tests. */ + + real_t initialRamping; /**< Start value for Ramping Strategy. */ + real_t finalRamping; /**< Final value for Ramping Strategy. */ + real_t initialFarBounds; /**< Initial size of Far Bounds. */ + real_t growFarBounds; /**< Factor to grow Far Bounds. */ + SubjectToStatus initialStatusBounds; /**< Initial status of bounds at first iteration. */ + real_t epsFlipping; /**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */ + int numRegularisationSteps; /**< Maximum number of successive regularisation steps. */ + real_t epsRegularisation; /**< Scaling factor of identity matrix used for Hessian regularisation. */ + int numRefinementSteps; /**< Maximum number of iterative refinement steps. */ + real_t epsIterRef; /**< Early termination tolerance for iterative refinement. */ + real_t epsLITests; /**< Tolerance for linear independence tests. */ + real_t epsNZCTests; /**< Tolerance for nonzero curvature tests. */ + + BooleanType enableDropInfeasibles; /**< ... */ + int dropBoundPriority; /**< ... */ + int dropEqConPriority; /**< ... */ + int dropIneqConPriority; /**< ... */ +} Options; + + +void OptionsCON( Options* _THIS + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void OptionsCPY( Options* FROM, + Options* TO + ); + + +/** Sets all options to default values. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToDefault( Options* _THIS + ); + +/** Sets all options to values resulting in maximum reliabilty. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToReliable( Options* _THIS + ); + +/** Sets all options to values resulting in minimum solution time. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToMPC( Options* _THIS + ); + +/** Same as setToMPC( ), for ensuring backwards compatibility. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToFast( Options* _THIS + ); + + +/** Ensures that all options have consistent values by automatically + * adjusting inconsistent ones. + * Note: This routine cannot (and does not try to) ensure that values + * are set to reasonable values that make the QP solution work! + * \return SUCCESSFUL_RETURN \n + * RET_OPTIONS_ADJUSTED */ +returnValue Options_ensureConsistency( Options* _THIS + ); + + +/** Prints values of all options. + * \return SUCCESSFUL_RETURN */ +returnValue Options_print( Options* _THIS + ); + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_OPTIONS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/QProblem.h b/third_party/acados/include/qpOASES_e/QProblem.h index 91a4a6f396720a..3c61a4d59681d6 100644 --- a/third_party/acados/include/qpOASES_e/QProblem.h +++ b/third_party/acados/include/qpOASES_e/QProblem.h @@ -1,2369 +1,2369 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/QProblem.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - * - * Declaration of the QProblem class which is able to use the newly - * developed online active set strategy for parametric quadratic programming. - */ - - - -#ifndef QPOASES_QPROBLEM_H -#define QPOASES_QPROBLEM_H - - -#include -#include -#include -#include -#include -#include - - -BEGIN_NAMESPACE_QPOASES - -typedef struct { - Bounds *auxiliaryBounds; - Constraints *auxiliaryConstraints; - - real_t *ub_new_far; - real_t *lb_new_far; - real_t *ubA_new_far; - real_t *lbA_new_far; - - real_t *g_new; - real_t *lb_new; - real_t *ub_new; - real_t *lbA_new; - real_t *ubA_new; - - real_t *g_new2; - real_t *lb_new2; - real_t *ub_new2; - real_t *lbA_new2; - real_t *ubA_new2; - - real_t *delta_xFX5; - real_t *delta_xFR5; - real_t *delta_yAC5; - real_t *delta_yFX5; - - real_t *Hx; - - real_t *_H; - - real_t *g_original; - real_t *lb_original; - real_t *ub_original; - real_t *lbA_original; - real_t *ubA_original; - - real_t *delta_xFR; - real_t *delta_xFX; - real_t *delta_yAC; - real_t *delta_yFX; - real_t *delta_g; - real_t *delta_lb; - real_t *delta_ub; - real_t *delta_lbA; - real_t *delta_ubA; - - real_t *gMod; - - real_t *aFR; - real_t *wZ; - - real_t *delta_g2; - real_t *delta_xFX2; - real_t *delta_xFR2; - real_t *delta_yAC2; - real_t *delta_yFX2; - real_t *nul; - real_t *Arow; - - real_t *xiC; - real_t *xiC_TMP; - real_t *xiB; - real_t *Arow2; - real_t *num; - - real_t *w; - real_t *tmp; - - real_t *delta_g3; - real_t *delta_xFX3; - real_t *delta_xFR3; - real_t *delta_yAC3; - real_t *delta_yFX3; - real_t *nul2; - - real_t *xiC2; - real_t *xiC_TMP2; - real_t *xiB2; - real_t *num2; - - real_t *Hz; - real_t *z; - real_t *ZHz; - real_t *r; - - real_t *tmp2; - real_t *Hz2; - real_t *z2; - real_t *r2; - real_t *rhs; - - real_t *delta_xFX4; - real_t *delta_xFR4; - real_t *delta_yAC4; - real_t *delta_yFX4; - real_t *nul3; - real_t *ek; - real_t *x_W; - real_t *As; - real_t *Ax_W; - - real_t *num3; - real_t *den; - real_t *delta_Ax_l; - real_t *delta_Ax_u; - real_t *delta_Ax; - real_t *delta_x; - - real_t *_A; - - real_t *grad; - real_t *AX; -} QProblem_ws; - -int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); - -char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory ); - -QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC ); - -/** - * \brief Implements the online active set strategy for QPs with general constraints. - * - * A class for setting up and solving quadratic programs. The main feature is - * the possibily to use the newly developed online active set strategy for - * parametric quadratic programming. - * - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - */ -typedef struct -{ - QProblem_ws *ws; /**< Workspace */ - Bounds *bounds; /**< Data structure for problem's bounds. */ - Constraints *constraints; /**< Data structure for problem's constraints. */ - Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ - - DenseMatrix* H; /**< Hessian matrix pointer. */ - DenseMatrix* A; /**< Constraint matrix pointer. */ - - Options options; /**< Struct containing all user-defined options for solving QPs. */ - TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ - - real_t *g; /**< Gradient. */ - - real_t *lb; /**< Lower bound vector (on variables). */ - real_t *ub; /**< Upper bound vector (on variables). */ - real_t *lbA; /**< Lower constraints' bound vector. */ - real_t *ubA; /**< Upper constraints' bound vector. */ - - real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ - - real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ - real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ - - real_t *Ax; /**< Stores the current A*x \n - * (for increased efficiency only). */ - real_t *Ax_l; /**< Stores the current distance to lower constraints' bounds A*x-lbA \n - * (for increased efficiency only). */ - real_t *Ax_u; /**< Stores the current distance to lower constraints' bounds ubA-A*x \n - * (for increased efficiency only). */ - - real_t *x; /**< Primal solution vector. */ - real_t *y; /**< Dual solution vector. */ - - real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ - real_t *tempA; /**< Temporary for determineStepDirection. */ - real_t *tempB; /**< Temporary for determineStepDirection. */ - real_t *ZFR_delta_xFRz; /**< Temporary for determineStepDirection. */ - real_t *delta_xFRy; /**< Temporary for determineStepDirection. */ - real_t *delta_xFRz; /**< Temporary for determineStepDirection. */ - real_t *delta_yAC_TMP; /**< Temporary for determineStepDirection. */ - - ConstraintProduct constraintProduct; /**< Pointer to user-defined constraint product function. */ - - real_t tau; /**< Last homotopy step length. */ - real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ - - real_t ramp0; /**< Start value for Ramping Strategy. */ - real_t ramp1; /**< Final value for Ramping Strategy. */ - - QProblemStatus status; /**< Current status of the solution process. */ - HessianType hessianType; /**< Type of Hessian matrix. */ - - BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ - BooleanType infeasible; /**< QP infeasible? */ - BooleanType unbounded; /**< QP unbounded? */ - - int rampOffset; /**< Offset index for Ramping. */ - unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ - - int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */ -} QProblem; - -int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC ); - -char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory ); - -QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC ); - - -/** Constructor which takes the QP dimension and Hessian type - * information. If the Hessian is the zero (i.e. HST_ZERO) or the - * identity matrix (i.e. HST_IDENTITY), respectively, no memory - * is allocated for it and a NULL pointer can be passed for it - * to the init() functions. */ -void QProblemCON( QProblem* _THIS, - int _nV, /**< Number of variables. */ - int _nC, /**< Number of constraints. */ - HessianType _hessianType /**< Type of Hessian matrix. */ - ); - -/** Copies all members from given rhs object. - * \return SUCCESSFUL_RETURN */ -void QProblemCPY( QProblem* FROM, - QProblem* TO - ); - - -/** Clears all data structures of QProblem except for QP data. - * \return SUCCESSFUL_RETURN \n - RET_RESET_FAILED */ -returnValue QProblem_reset( QProblem* _THIS ); - - -/** Initialises a QP problem with given QP data and tries to solve it - * using at most nWSR iterations. - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_initM( QProblem* _THIS, - DenseMatrix *_H, /**< Hessian matrix. */ - const real_t* const _g, /**< Gradient vector. */ - DenseMatrix *_A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA, /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation (if pointer passed). */ - ); - - -/** Initialises a QP problem with given QP data and tries to solve it - * using at most nWSR iterations. - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_init( QProblem* _THIS, - real_t* const _H, /**< Hessian matrix. \n - If Hessian matrix is trivial, a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - real_t* const _A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA, /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation (if pointer passed). */ - ); - -/** Initialises a QP problem with given QP data to be read from files and tries to solve it - * using at most nWSR iterations. - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_UNABLE_TO_READ_FILE */ -returnValue QProblem_initF( QProblem* _THIS, - const char* const H_file, /**< Name of file where Hessian matrix is stored. \n - If Hessian matrix is trivial, a NULL pointer can be passed. */ - const char* const g_file, /**< Name of file where gradient vector is stored. */ - const char* const A_file, /**< Name of file where constraint matrix is stored. */ - const char* const lb_file, /**< Name of file where lower bound vector. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bound vector. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation (if pointer passed). */ - ); - -/** Initialises a QP problem with given QP data and tries to solve it - * using at most nWSR iterations. Depending on the parameter constellation it: \n - * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n - * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n - * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n - * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n - * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n - * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n - * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_initMW( QProblem* _THIS, - DenseMatrix *_H, /**< Hessian matrix. \n - If Hessian matrix is trivial, a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - DenseMatrix *_A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA, /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - * Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation. */ - const real_t* const xOpt, /**< Optimal primal solution vector. \n - (If a null pointer is passed, the old primal solution is kept!) */ - const real_t* const yOpt, /**< Optimal dual solution vector. \n - (If a null pointer is passed, the old dual solution is kept!) */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ - Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ - const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. - The Cholesky factor must be stored in a real_t array of size nV*nV - in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n - (If a null pointer is passed, Cholesky decomposition is computed internally!) */ - ); - -/** Initialises a QP problem with given QP data and tries to solve it - * using at most nWSR iterations. Depending on the parameter constellation it: \n - * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n - * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n - * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n - * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n - * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n - * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n - * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_initW( QProblem* _THIS, - real_t* const _H, /**< Hessian matrix. \n - If Hessian matrix is trivial, a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - real_t* const _A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA, /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - * Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation. */ - const real_t* const xOpt, /**< Optimal primal solution vector. \n - (If a null pointer is passed, the old primal solution is kept!) */ - const real_t* const yOpt, /**< Optimal dual solution vector. \n - (If a null pointer is passed, the old dual solution is kept!) */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ - Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ - const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. - The Cholesky factor must be stored in a real_t array of size nV*nV - in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n - (If a null pointer is passed, Cholesky decomposition is computed internally!) */ - ); - -/** Initialises a QP problem with given QP data to be ream from files and tries to solve it - * using at most nWSR iterations. Depending on the parameter constellation it: \n - * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n - * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n - * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n - * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n - * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n - * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n - * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) - * - * Note: This function internally calls solveInitialQP for initialisation! - * - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED \n - RET_UNABLE_TO_READ_FILE */ -returnValue QProblem_initFW( QProblem* _THIS, - const char* const H_file, /**< Name of file where Hessian matrix is stored. \n - If Hessian matrix is trivial, a NULL pointer can be passed. */ - const char* const g_file, /**< Name of file where gradient vector is stored. */ - const char* const A_file, /**< Name of file where constraint matrix is stored. */ - const char* const lb_file, /**< Name of file where lower bound vector. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bound vector. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. - Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n - Output: CPU time spent for QP initialisation. */ - const real_t* const xOpt, /**< Optimal primal solution vector. \n - (If a null pointer is passed, the old primal solution is kept!) */ - const real_t* const yOpt, /**< Optimal dual solution vector. \n - (If a null pointer is passed, the old dual solution is kept!) */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ - Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ - const char* const R_file /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. - The Cholesky factor must be stored in a real_t array of size nV*nV - in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n - (If a null pointer is passed, Cholesky decomposition is computed internally!) */ - ); - -/** Solves an initialised QP sequence using the online active set strategy. - * QP solution is started from previous solution. - * - * Note: This function internally calls solveQP/solveRegularisedQP - * for solving an initialised QP! - * - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ -returnValue QProblem_hotstart( QProblem* _THIS, - const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ - const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - ); - -/** Solves an initialised QP sequence using the online active set strategy, - * where QP data is read from files. QP solution is started from previous solution. - * - * Note: This function internally calls solveQP/solveRegularisedQP - * for solving an initialised QP! - * - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_hotstartF( QProblem* _THIS, - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - ); - -/** Solves an initialised QP sequence using the online active set strategy. - * By default, QP solution is started from previous solution. If a guess - * for the working set is provided, an initialised homotopy is performed. - * - * Note: This function internally calls solveQP/solveRegularisedQP - * for solving an initialised QP! - * - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n - RET_SETUP_AUXILIARYQP_FAILED */ -returnValue QProblem_hotstartW( QProblem* _THIS, - const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ - const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n - (If a null pointer is passed, the previous working set of bounds is kept!) */ - Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n - (If a null pointer is passed, the previous working set of constraints is kept!) */ - ); - -/** Solves an initialised QP sequence using the online active set strategy, - * where QP data is read from files. - * By default, QP solution is started from previous solution. If a guess - * for the working set is provided, an initialised homotopy is performed. - * - * Note: This function internally calls solveQP/solveRegularisedQP - * for solving an initialised QP! - * - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n - RET_SETUP_AUXILIARYQP_FAILED \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_hotstartFW( QProblem* _THIS, - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n - (If a null pointer is passed, the previous working set of bounds is kept!) */ - Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n - (If a null pointer is passed, the previous working set of constraints is kept!) */ - ); - - -/** Solves using the current working set - * \return SUCCESSFUL_RETURN \n - * RET_STEPDIRECTION_FAILED_TQ \n - * RET_STEPDIRECTION_FAILED_CHOLESKY \n - * RET_INVALID_ARGUMENTS */ -returnValue QProblem_solveCurrentEQP ( QProblem* _THIS, - const int n_rhs, /**< Number of consecutive right hand sides */ - const real_t* g_in, /**< Gradient of neighbouring QP to be solved. */ - const real_t* lb_in, /**< Lower bounds of neighbouring QP to be solved. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* ub_in, /**< Upper bounds of neighbouring QP to be solved. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* lbA_in, /**< Lower constraints' bounds of neighbouring QP to be solved. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* ubA_in, /**< Upper constraints' bounds of neighbouring QP to be solved. \n */ - real_t* x_out, /**< Output: Primal solution */ - real_t* y_out /**< Output: Dual solution */ - ); - - - -/** Returns current constraints object of the QP (deep copy). - * \return SUCCESSFUL_RETURN \n - RET_QPOBJECT_NOT_SETUP */ -static inline returnValue QProblem_getConstraints( QProblem* _THIS, - Constraints* _constraints /** Output: Constraints object. */ - ); - - -/** Returns the number of constraints. - * \return Number of constraints. */ -static inline int QProblem_getNC( QProblem* _THIS ); - -/** Returns the number of (implicitly defined) equality constraints. - * \return Number of (implicitly defined) equality constraints. */ -static inline int QProblem_getNEC( QProblem* _THIS ); - -/** Returns the number of active constraints. - * \return Number of active constraints. */ -static inline int QProblem_getNAC( QProblem* _THIS ); - -/** Returns the number of inactive constraints. - * \return Number of inactive constraints. */ -static inline int QProblem_getNIAC( QProblem* _THIS ); - -/** Returns the dimension of null space. - * \return Dimension of null space. */ -int QProblem_getNZ( QProblem* _THIS ); - - -/** Returns the dual solution vector (deep copy). - * \return SUCCESSFUL_RETURN \n - RET_QP_NOT_SOLVED */ -returnValue QProblem_getDualSolution( QProblem* _THIS, - real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ - ); - - -/** Defines user-defined routine for calculating the constraint product A*x - * \return SUCCESSFUL_RETURN \n */ -returnValue QProblem_setConstraintProduct( QProblem* _THIS, - ConstraintProduct _constraintProduct - ); - - -/** Prints concise list of properties of the current QP. - * \return SUCCESSFUL_RETURN \n */ -returnValue QProblem_printProperties( QProblem* _THIS ); - - - -/** Writes a vector with the state of the working set -* \return SUCCESSFUL_RETURN */ -returnValue QProblem_getWorkingSet( QProblem* _THIS, - real_t* workingSet /** Output: array containing state of the working set. */ - ); - -/** Writes a vector with the state of the working set of bounds - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -returnValue QProblem_getWorkingSetBounds( QProblem* _THIS, - real_t* workingSetB /** Output: array containing state of the working set of bounds. */ - ); - -/** Writes a vector with the state of the working set of constraints - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -returnValue QProblem_getWorkingSetConstraints( QProblem* _THIS, - real_t* workingSetC /** Output: array containing state of the working set of constraints. */ - ); - - -/** Returns current bounds object of the QP (deep copy). - * \return SUCCESSFUL_RETURN \n - RET_QPOBJECT_NOT_SETUP */ -static inline returnValue QProblem_getBounds( QProblem* _THIS, - Bounds* _bounds /** Output: Bounds object. */ - ); - - -/** Returns the number of variables. - * \return Number of variables. */ -static inline int QProblem_getNV( QProblem* _THIS ); - -/** Returns the number of free variables. - * \return Number of free variables. */ -static inline int QProblem_getNFR( QProblem* _THIS ); - -/** Returns the number of fixed variables. - * \return Number of fixed variables. */ -static inline int QProblem_getNFX( QProblem* _THIS ); - -/** Returns the number of implicitly fixed variables. - * \return Number of implicitly fixed variables. */ -static inline int QProblem_getNFV( QProblem* _THIS ); - - -/** Returns the optimal objective function value. - * \return finite value: Optimal objective function value (QP was solved) \n - +infinity: QP was not yet solved */ -real_t QProblem_getObjVal( QProblem* _THIS ); - -/** Returns the objective function value at an arbitrary point x. - * \return Objective function value at point x */ -real_t QProblem_getObjValX( QProblem* _THIS, - const real_t* const _x /**< Point at which the objective function shall be evaluated. */ - ); - -/** Returns the primal solution vector. - * \return SUCCESSFUL_RETURN \n - RET_QP_NOT_SOLVED */ -returnValue QProblem_getPrimalSolution( QProblem* _THIS, - real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ - ); - - -/** Returns status of the solution process. - * \return Status of solution process. */ -static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ); - - -/** Returns if the QProblem object is initialised. - * \return BT_TRUE: QProblem initialised \n - BT_FALSE: QProblem not initialised */ -static inline BooleanType QProblem_isInitialised( QProblem* _THIS ); - -/** Returns if the QP has been solved. - * \return BT_TRUE: QProblem solved \n - BT_FALSE: QProblem not solved */ -static inline BooleanType QProblem_isSolved( QProblem* _THIS ); - -/** Returns if the QP is infeasible. - * \return BT_TRUE: QP infeasible \n - BT_FALSE: QP feasible (or not known to be infeasible!) */ -static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ); - -/** Returns if the QP is unbounded. - * \return BT_TRUE: QP unbounded \n - BT_FALSE: QP unbounded (or not known to be unbounded!) */ -static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ); - - -/** Returns Hessian type flag (type is not determined due to _THIS call!). - * \return Hessian type. */ -static inline HessianType QProblem_getHessianType( QProblem* _THIS ); - -/** Changes the print level. - * \return SUCCESSFUL_RETURN */ -static inline returnValue QProblem_setHessianType( QProblem* _THIS, - HessianType _hessianType /**< New Hessian type. */ - ); - -/** Returns if the QP has been internally regularised. - * \return BT_TRUE: Hessian is internally regularised for QP solution \n - BT_FALSE: No internal Hessian regularisation is used for QP solution */ -static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ); - -/** Returns current options struct. - * \return Current options struct. */ -static inline Options QProblem_getOptions( QProblem* _THIS ); - -/** Overrides current options with given ones. - * \return SUCCESSFUL_RETURN */ -static inline returnValue QProblem_setOptions( QProblem* _THIS, - Options _options /**< New options. */ - ); - -/** Returns the print level. - * \return Print level. */ -static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ); - -/** Changes the print level. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_setPrintLevel( QProblem* _THIS, - PrintLevel _printlevel /**< New print level. */ - ); - - -/** Returns the current number of QP problems solved. - * \return Number of QP problems solved. */ -static inline unsigned int QProblem_getCount( QProblem* _THIS ); - -/** Resets QP problem counter (to zero). - * \return SUCCESSFUL_RETURN. */ -static inline returnValue QProblem_resetCounter( QProblem* _THIS ); - - -/** Prints a list of all options and their current values. - * \return SUCCESSFUL_RETURN \n */ -returnValue QProblem_printOptions( QProblem* _THIS ); - - -/** Solves a QProblem whose QP data is assumed to be stored in the member variables. - * A guess for its primal/dual optimal solution vectors and the corresponding - * working sets of bounds and constraints can be provided. - * Note: This function is internally called by all init functions! - * \return SUCCESSFUL_RETURN \n - RET_INIT_FAILED \n - RET_INIT_FAILED_CHOLESKY \n - RET_INIT_FAILED_TQ \n - RET_INIT_FAILED_HOTSTART \n - RET_INIT_FAILED_INFEASIBILITY \n - RET_INIT_FAILED_UNBOUNDEDNESS \n - RET_MAX_NWSR_REACHED */ -returnValue QProblem_solveInitialQP( QProblem* _THIS, - const real_t* const xOpt, /**< Optimal primal solution vector.*/ - const real_t* const yOpt, /**< Optimal dual solution vector. */ - Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ - Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ - const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - * Output: Number of performed working set recalculations. */ - real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n - * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - ); - -/** Solves QProblem using online active set strategy. - * Note: This function is internally called by all hotstart functions! - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ -returnValue QProblem_solveQP( QProblem* _THIS, - const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ - const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - int nWSRperformed, /**< Number of working set recalculations already performed to solve - this QP within previous solveQP() calls. This number is - always zero, except for successive calls from solveRegularisedQP() - or when using the far bound strategy. */ - BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ - ); - - -/** Solves QProblem using online active set strategy. - * Note: This function is internally called by all hotstart functions! - * \return SUCCESSFUL_RETURN \n - RET_MAX_NWSR_REACHED \n - RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n - RET_HOTSTART_FAILED \n - RET_SHIFT_DETERMINATION_FAILED \n - RET_STEPDIRECTION_DETERMINATION_FAILED \n - RET_STEPLENGTH_DETERMINATION_FAILED \n - RET_HOMOTOPY_STEP_FAILED \n - RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ -returnValue QProblem_solveRegularisedQP( QProblem* _THIS, - const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ - const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - int* nWSR, /**< Input: Maximum number of working set recalculations; \n - Output: Number of performed working set recalculations. */ - real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n - Output: CPU time spent for QP solution (or to perform nWSR iterations). */ - int nWSRperformed, /**< Number of working set recalculations already performed to solve - this QP within previous solveRegularisedQP() calls. This number is - always zero, except for successive calls when using the far bound strategy. */ - BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ - ); - - -/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). - * \return SUCCESSFUL_RETURN \n - RET_SETUPSUBJECTTOTYPE_FAILED */ -returnValue QProblem_setupSubjectToType( QProblem* _THIS ); - -/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). - * \return SUCCESSFUL_RETURN \n - RET_SETUPSUBJECTTOTYPE_FAILED */ -returnValue QProblem_setupSubjectToTypeNew( QProblem* _THIS, - const real_t* const lb_new, /**< New lower bounds. */ - const real_t* const ub_new, /**< New upper bounds. */ - const real_t* const lbA_new, /**< New lower constraints' bounds. */ - const real_t* const ubA_new /**< New upper constraints' bounds. */ - ); - -/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z). - * Note: If Hessian turns out not to be positive definite, the Hessian type - * is set to HST_SEMIDEF accordingly. - * \return SUCCESSFUL_RETURN \n - * RET_HESSIAN_NOT_SPD \n - * RET_INDEXLIST_CORRUPTED */ -returnValue QProblem_computeProjectedCholesky( QProblem* _THIS ); - -/** Computes initial Cholesky decomposition of the projected Hessian making - * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). - * \return SUCCESSFUL_RETURN \n - * RET_HESSIAN_NOT_SPD \n - * RET_INDEXLIST_CORRUPTED */ -returnValue QProblem_setupInitialCholesky( QProblem* _THIS ); - -/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active. - * \return SUCCESSFUL_RETURN \n - RET_INDEXLIST_CORRUPTED */ -returnValue QProblem_setupTQfactorisation( QProblem* _THIS ); - - -/** Obtains the desired working set for the auxiliary initial QP in - * accordance with the user specifications - * (assumes that member AX has already been initialised!) - * \return SUCCESSFUL_RETURN \n - RET_OBTAINING_WORKINGSET_FAILED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_obtainAuxiliaryWorkingSet( QProblem* _THIS, - const real_t* const xOpt, /**< Optimal primal solution vector. - * If a NULL pointer is passed, all entries are assumed to be zero. */ - const real_t* const yOpt, /**< Optimal dual solution vector. - * If a NULL pointer is passed, all entries are assumed to be zero. */ - Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */ - Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */ - Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n - * Ouput: Working set of constraints for auxiliary QP. */ - Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n - * Ouput: Working set for auxiliary QP. */ - ); - -/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints. - * (If the working set shall be setup afresh, make sure that - * bounds and constraints data structure have been resetted - * and the TQ factorisation has been initialised!) - * \return SUCCESSFUL_RETURN \n - RET_SETUP_WORKINGSET_FAILED \n - RET_INVALID_ARGUMENTS \n - RET_UNKNOWN_BUG */ -returnValue QProblem_setupAuxiliaryWorkingSet( QProblem* _THIS, - Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ - Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ - BooleanType setupAfresh /**< Flag indicating if given working set shall be - * setup afresh or by updating the current one. */ - ); - -/** Sets up the optimal primal/dual solution of the auxiliary initial QP. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_setupAuxiliaryQPsolution( QProblem* _THIS, - const real_t* const xOpt, /**< Optimal primal solution vector. - * If a NULL pointer is passed, all entries are set to zero. */ - const real_t* const yOpt /**< Optimal dual solution vector. - * If a NULL pointer is passed, all entries are set to zero. */ - ); - -/** Sets up gradient of the auxiliary initial QP for given - * optimal primal/dual solution and given initial working set - * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS ); - -/** Sets up (constraints') bounds of the auxiliary initial QP for given - * optimal primal/dual solution and given initial working set - * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). - * \return SUCCESSFUL_RETURN \n - RET_UNKNOWN_BUG */ -returnValue QProblem_setupAuxiliaryQPbounds( QProblem* _THIS, - Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ - Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ - BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */ - ); - - -/** Adds a constraint to active set. - * \return SUCCESSFUL_RETURN \n - RET_ADDCONSTRAINT_FAILED \n - RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n - RET_ENSURELI_FAILED */ -returnValue QProblem_addConstraint( QProblem* _THIS, - int number, /**< Number of constraint to be added to active set. */ - SubjectToStatus C_status, /**< Status of new active constraint. */ - BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ - BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ - ); - -/** Checks if new active constraint to be added is linearly dependent from - * from row of the active constraints matrix. - * \return RET_LINEARLY_DEPENDENT \n - RET_LINEARLY_INDEPENDENT \n - RET_INDEXLIST_CORRUPTED */ -returnValue QProblem_addConstraint_checkLI( QProblem* _THIS, - int number /**< Number of constraint to be added to active set. */ - ); - -/** Ensures linear independence of constraint matrix when a new constraint is added. - * To _THIS end a bound or constraint is removed simultaneously if necessary. - * \return SUCCESSFUL_RETURN \n - RET_LI_RESOLVED \n - RET_ENSURELI_FAILED \n - RET_ENSURELI_FAILED_TQ \n - RET_ENSURELI_FAILED_NOINDEX \n - RET_REMOVE_FROM_ACTIVESET */ -returnValue QProblem_addConstraint_ensureLI( QProblem* _THIS, - int number, /**< Number of constraint to be added to active set. */ - SubjectToStatus C_status /**< Status of new active bound. */ - ); - -/** Adds a bound to active set. - * \return SUCCESSFUL_RETURN \n - RET_ADDBOUND_FAILED \n - RET_ADDBOUND_FAILED_INFEASIBILITY \n - RET_ENSURELI_FAILED */ -returnValue QProblem_addBound( QProblem* _THIS, - int number, /**< Number of bound to be added to active set. */ - SubjectToStatus B_status, /**< Status of new active bound. */ - BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ - BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ - ); - -/** Checks if new active bound to be added is linearly dependent from - * from row of the active constraints matrix. - * \return RET_LINEARLY_DEPENDENT \n - RET_LINEARLY_INDEPENDENT */ -returnValue QProblem_addBound_checkLI( QProblem* _THIS, - int number /**< Number of bound to be added to active set. */ - ); - -/** Ensures linear independence of constraint matrix when a new bound is added. - * To _THIS end a bound or constraint is removed simultaneously if necessary. - * \return SUCCESSFUL_RETURN \n - RET_LI_RESOLVED \n - RET_ENSURELI_FAILED \n - RET_ENSURELI_FAILED_TQ \n - RET_ENSURELI_FAILED_NOINDEX \n - RET_REMOVE_FROM_ACTIVESET */ -returnValue QProblem_addBound_ensureLI( QProblem* _THIS, - int number, /**< Number of bound to be added to active set. */ - SubjectToStatus B_status /**< Status of new active bound. */ - ); - -/** Removes a constraint from active set. - * \return SUCCESSFUL_RETURN \n - RET_CONSTRAINT_NOT_ACTIVE \n - RET_REMOVECONSTRAINT_FAILED \n - RET_HESSIAN_NOT_SPD */ -returnValue QProblem_removeConstraint( QProblem* _THIS, - int number, /**< Number of constraint to be removed from active set. */ - BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ - BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ - BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ - ); - -/** Removes a bounds from active set. - * \return SUCCESSFUL_RETURN \n - RET_BOUND_NOT_ACTIVE \n - RET_HESSIAN_NOT_SPD \n - RET_REMOVEBOUND_FAILED */ -returnValue QProblem_removeBound( QProblem* _THIS, - int number, /**< Number of bound to be removed from active set. */ - BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ - BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ - BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ - ); - - -/** Performs robustified ratio test yield the maximum possible step length - * along the homotopy path. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_performPlainRatioTest( QProblem* _THIS, - int nIdx, /**< Number of ratios to be checked. */ - const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ - const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ - const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ - real_t epsNum, /**< Numerator tolerance. */ - real_t epsDen, /**< Denominator tolerance. */ - real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ - int* BC_idx /**< Output: Index of blocking constraint. */ - ); - - -/** Ensure non-zero curvature by primal jump. - * \return SUCCESSFUL_RETURN \n - * RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ -returnValue QProblem_ensureNonzeroCurvature( QProblem* _THIS, - BooleanType removeBoundNotConstraint, /**< SubjectTo to be removed is a bound. */ - int remIdx, /**< Index of bound/constraint to be removed. */ - BooleanType* exchangeHappened, /**< Output: Exchange was necessary to ensure. */ - BooleanType* addBoundNotConstraint, /**< SubjectTo to be added is a bound. */ - int* addIdx, /**< Index of bound/constraint to be added. */ - SubjectToStatus* addStatus /**< Status of bound/constraint to be added. */ - ); - - -/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix. - * \return SUCCESSFUL_RETURN \n - RET_DIV_BY_ZERO */ -returnValue QProblem_backsolveT( QProblem* _THIS, - const real_t* const b, /**< Right hand side vector. */ - BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ - real_t* const a /**< Output: Solution vector */ - ); - - -/** Determines step direction of the shift of the QP data. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_determineDataShift( QProblem* _THIS, - const real_t* const g_new, /**< New gradient vector. */ - const real_t* const lbA_new, /**< New lower constraints' bounds. */ - const real_t* const ubA_new, /**< New upper constraints' bounds. */ - const real_t* const lb_new, /**< New lower bounds. */ - const real_t* const ub_new, /**< New upper bounds. */ - real_t* const delta_g, /**< Output: Step direction of gradient vector. */ - real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */ - real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */ - real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ - real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ - BooleanType* Delta_bC_isZero, /**< Output: Indicates if active constraints' bounds are to be shifted. */ - BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ - ); - -/** Determines step direction of the homotopy path. - * \return SUCCESSFUL_RETURN \n - RET_STEPDIRECTION_FAILED_TQ \n - RET_STEPDIRECTION_FAILED_CHOLESKY */ -returnValue QProblem_determineStepDirection( QProblem* _THIS, - const real_t* const delta_g, /**< Step direction of gradient vector. */ - const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ - const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ - const real_t* const delta_lb, /**< Step direction of lower bounds. */ - const real_t* const delta_ub, /**< Step direction of upper bounds. */ - BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */ - BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ - real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ - real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ - real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */ - real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ - ); - -/** Determines the maximum possible step length along the homotopy path - * and performs _THIS step (without changing working set). - * \return SUCCESSFUL_RETURN \n - * RET_ERROR_IN_CONSTRAINTPRODUCT \n - * RET_QP_INFEASIBLE */ -returnValue QProblem_performStep( QProblem* _THIS, - const real_t* const delta_g, /**< Step direction of gradient. */ - const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ - const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ - const real_t* const delta_lb, /**< Step direction of lower bounds. */ - const real_t* const delta_ub, /**< Step direction of upper bounds. */ - const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ - const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ - const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ - const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ - int* BC_idx, /**< Output: Index of blocking constraint. */ - SubjectToStatus* BC_status, /**< Output: Status of blocking constraint. */ - BooleanType* BC_isBound /**< Output: Indicates if blocking constraint is a bound. */ - ); - -/** Updates the active set. - * \return SUCCESSFUL_RETURN \n - RET_REMOVE_FROM_ACTIVESET_FAILED \n - RET_ADD_TO_ACTIVESET_FAILED */ -returnValue QProblem_changeActiveSet( QProblem* _THIS, - int BC_idx, /**< Index of blocking constraint. */ - SubjectToStatus BC_status, /**< Status of blocking constraint. */ - BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ - ); - - -/** Compute relative length of homotopy in data space for termination - * criterion. - * \return Relative length in data space. */ -real_t QProblem_getRelativeHomotopyLength( QProblem* _THIS, - const real_t* const g_new, /**< Final gradient. */ - const real_t* const lb_new, /**< Final lower variable bounds. */ - const real_t* const ub_new, /**< Final upper variable bounds. */ - const real_t* const lbA_new, /**< Final lower constraint bounds. */ - const real_t* const ubA_new /**< Final upper constraint bounds. */ - ); - - -/** Ramping Strategy to avoid ties. Modifies homotopy start without - * changing current active set. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_performRamping( QProblem* _THIS ); - - -/** ... */ -returnValue QProblem_updateFarBounds( QProblem* _THIS, - real_t curFarBound, /**< ... */ - int nRamp, /**< ... */ - const real_t* const lb_new, /**< ... */ - real_t* const lb_new_far, /**< ... */ - const real_t* const ub_new, /**< ... */ - real_t* const ub_new_far, /**< ... */ - const real_t* const lbA_new, /**< ... */ - real_t* const lbA_new_far, /**< ... */ - const real_t* const ubA_new, /**< ... */ - real_t* const ubA_new_far /**< ... */ - ); - -/** ... */ -returnValue QProblemBCPY_updateFarBounds( QProblem* _THIS, - real_t curFarBound, /**< ... */ - int nRamp, /**< ... */ - const real_t* const lb_new, /**< ... */ - real_t* const lb_new_far, /**< ... */ - const real_t* const ub_new, /**< ... */ - real_t* const ub_new_far /**< ... */ - ); - - - -/** Performs robustified ratio test yield the maximum possible step length - * along the homotopy path. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_performRatioTestC( QProblem* _THIS, - int nIdx, /**< Number of ratios to be checked. */ - const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ - Constraints* const subjectTo, /**< Constraint object corresponding to ratios to be checked. */ - const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ - const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ - real_t epsNum, /**< Numerator tolerance. */ - real_t epsDen, /**< Denominator tolerance. */ - real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ - int* BC_idx /**< Output: Index of blocking constraint. */ - ); - - -/** Drift correction at end of each active set iteration - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_performDriftCorrection( QProblem* _THIS ); - - -/** Updates QP vectors, working sets and internal data structures in order to - start from an optimal solution corresponding to initial guesses of the working - set for bounds and constraints. - * \return SUCCESSFUL_RETURN \n - * RET_SETUP_AUXILIARYQP_FAILED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_setupAuxiliaryQP( QProblem* _THIS, - Bounds* const guessedBounds, /**< Initial guess for working set of bounds. */ - Constraints* const guessedConstraints /**< Initial guess for working set of constraints. */ - ); - -/** Determines if it is more efficient to refactorise the matrices when - * hotstarting or not (i.e. better to update the existing factorisations). - * \return BT_TRUE iff matrices shall be refactorised afresh - */ -BooleanType QProblem_shallRefactorise( QProblem* _THIS, - Bounds* const guessedBounds, /**< Guessed new working set of bounds. */ - Constraints* const guessedConstraints /**< Guessed new working set of constraints. */ - ); - -/** Setups internal QP data. - * \return SUCCESSFUL_RETURN \n - RET_INVALID_ARGUMENTS \n - RET_UNKNONW_BUG */ -returnValue QProblem_setupQPdataM( QProblem* _THIS, - DenseMatrix *_H, /**< Hessian matrix. \n - If Hessian matrix is trivial,a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - DenseMatrix *_A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - ); - - -/** Sets up dense internal QP data. If the current Hessian is trivial - * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, - * memory for Hessian is allocated and it is set to the given one. - * \return SUCCESSFUL_RETURN \n - RET_INVALID_ARGUMENTS \n - RET_UNKNONW_BUG */ -returnValue QProblem_setupQPdata( QProblem* _THIS, - real_t* const _H, /**< Hessian matrix. \n - If Hessian matrix is trivial,a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - real_t* const _A, /**< Constraint matrix. */ - const real_t* const _lb, /**< Lower bound vector (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub, /**< Upper bound vector (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - const real_t* const _lbA, /**< Lower constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const real_t* const _ubA /**< Upper constraints' bound vector. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - ); - -/** Sets up internal QP data by loading it from files. If the current Hessian - * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, - * memory for Hessian is allocated and it is set to the given one. - * \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS \n - RET_UNKNONW_BUG */ -returnValue QProblem_setupQPdataFromFile( QProblem* _THIS, - const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n - If Hessian matrix is trivial,a NULL pointer can be passed. */ - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const A_file, /**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - ); - -/** Loads new QP vectors from files (internal members are not affected!). - * \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS */ -returnValue QProblem_loadQPvectorsFromFile( QProblem* _THIS, - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no lower constraints' bounds exist, a NULL pointer can be passed. */ - const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n - If no upper constraints' bounds exist, a NULL pointer can be passed. */ - real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ - real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ - real_t* const ub_new, /**< Output: Upper bounds of neighbouring QP to be solved */ - real_t* const lbA_new, /**< Output: Lower constraints' bounds of neighbouring QP to be solved */ - real_t* const ubA_new /**< Output: Upper constraints' bounds of neighbouring QP to be solved */ - ); - - -/** Prints concise information on the current iteration. - * \return SUCCESSFUL_RETURN \n */ -returnValue QProblem_printIteration( QProblem* _THIS, - int iter, /**< Number of current iteration. */ - int BC_idx, /**< Index of blocking constraint. */ - SubjectToStatus BC_status, /**< Status of blocking constraint. */ - BooleanType BC_isBound, /**< Indicates if blocking constraint is a bound. */ - real_t homotopyLength, /**< Current homotopy distance. */ - BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ - ); - - -/** Sets constraint matrix of the QP. \n - Note: Also internal vector Ax is recomputed! - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -static inline returnValue QProblem_setAM( QProblem* _THIS, - DenseMatrix *A_new /**< New constraint matrix. */ - ); - -/** Sets dense constraint matrix of the QP. \n - Note: Also internal vector Ax is recomputed! - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -static inline returnValue QProblem_setA( QProblem* _THIS, - real_t* const A_new /**< New dense constraint matrix (with correct dimension!). */ - ); - - -/** Sets constraints' lower bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_QPOBJECT_NOT_SETUP */ -static inline returnValue QProblem_setLBA( QProblem* _THIS, - const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */ - ); - -/** Changes single entry of lower constraints' bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_QPOBJECT_NOT_SETUP \n - * RET_INDEX_OUT_OF_BOUNDS */ -static inline returnValue QProblem_setLBAn( QProblem* _THIS, - int number, /**< Number of entry to be changed. */ - real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */ - ); - -/** Sets constraints' upper bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_QPOBJECT_NOT_SETUP */ -static inline returnValue QProblem_setUBA( QProblem* _THIS, - const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */ - ); - -/** Changes single entry of upper constraints' bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_QPOBJECT_NOT_SETUP \n - * RET_INDEX_OUT_OF_BOUNDS */ -static inline returnValue QProblem_setUBAn( QProblem* _THIS, - int number, /**< Number of entry to be changed. */ - real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */ - ); - - -/** Decides if lower bounds are smaller than upper bounds - * - * \return SUCCESSFUL_RETURN \n - * RET_QP_INFEASIBLE */ -returnValue QProblem_areBoundsConsistent( QProblem* _THIS, - const real_t* const lb, /**< Vector of lower bounds*/ - const real_t* const ub, /**< Vector of upper bounds*/ - const real_t* const lbA, /**< Vector of lower constraints*/ - const real_t* const ubA /**< Vector of upper constraints*/ - ); - - -/** Drops the blocking bound/constraint that led to infeasibility, or finds another - * bound/constraint to drop according to drop priorities. - * \return SUCCESSFUL_RETURN \n - */ -returnValue QProblem_dropInfeasibles ( QProblem* _THIS, - int BC_number, /**< Number of the bound or constraint to be added */ - SubjectToStatus BC_status, /**< New status of the bound or constraint to be added */ - BooleanType BC_isBound, /**< Whether a bound or a constraint is to be added */ - real_t *xiB, - real_t *xiC - ); - - -/** If Hessian type has been set by the user, nothing is done. - * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or - * HST_POSDEF (default), respectively. - * \return SUCCESSFUL_RETURN \n - RET_HESSIAN_INDEFINITE */ -returnValue QProblem_determineHessianType( QProblem* _THIS ); - -/** Computes the Cholesky decomposition of the (simply projected) Hessian - * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple - * projection matrix! - * Note: If Hessian turns out not to be positive definite, the Hessian type - * is set to HST_SEMIDEF accordingly. - * \return SUCCESSFUL_RETURN \n - * RET_HESSIAN_NOT_SPD \n - * RET_INDEXLIST_CORRUPTED */ -returnValue QProblemBCPY_computeCholesky( QProblem* _THIS ); - -/** Obtains the desired working set for the auxiliary initial QP in - * accordance with the user specifications - * \return SUCCESSFUL_RETURN \n - RET_OBTAINING_WORKINGSET_FAILED \n - RET_INVALID_ARGUMENTS */ -returnValue QProblemBCPY_obtainAuxiliaryWorkingSet( QProblem* _THIS, - const real_t* const xOpt, /**< Optimal primal solution vector. - * If a NULL pointer is passed, all entries are assumed to be zero. */ - const real_t* const yOpt, /**< Optimal dual solution vector. - * If a NULL pointer is passed, all entries are assumed to be zero. */ - Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ - Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n - * Output: Working set for auxiliary QP. */ - ); - - -/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. - * \return SUCCESSFUL_RETURN \n - RET_DIV_BY_ZERO */ -returnValue QProblem_backsolveR( QProblem* _THIS, - const real_t* const b, /**< Right hand side vector. */ - BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ - real_t* const a /**< Output: Solution vector */ - ); - -/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n - * Special variant for the case that _THIS function is called from within "removeBound()". - * \return SUCCESSFUL_RETURN \n - RET_DIV_BY_ZERO */ -returnValue QProblem_backsolveRrem( QProblem* _THIS, - const real_t* const b, /**< Right hand side vector. */ - BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ - BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ - real_t* const a /**< Output: Solution vector */ - ); - - -/** Determines step direction of the shift of the QP data. - * \return SUCCESSFUL_RETURN */ -returnValue QProblemBCPY_determineDataShift( QProblem* _THIS, - const real_t* const g_new, /**< New gradient vector. */ - const real_t* const lb_new, /**< New lower bounds. */ - const real_t* const ub_new, /**< New upper bounds. */ - real_t* const delta_g, /**< Output: Step direction of gradient vector. */ - real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ - real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ - BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ - ); - - -/** Sets up internal QP data. - * \return SUCCESSFUL_RETURN \n - RET_INVALID_ARGUMENTS */ -returnValue QProblemBCPY_setupQPdataM( QProblem* _THIS, - DenseMatrix *_H, /**< Hessian matrix.*/ - const real_t* const _g, /**< Gradient vector. */ - const real_t* const _lb, /**< Lower bounds (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub /**< Upper bounds (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - ); - -/** Sets up internal QP data. If the current Hessian is trivial - * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, - * memory for Hessian is allocated and it is set to the given one. - * \return SUCCESSFUL_RETURN \n - RET_INVALID_ARGUMENTS \n - RET_NO_HESSIAN_SPECIFIED */ -returnValue QProblemBCPY_setupQPdata( QProblem* _THIS, - real_t* const _H, /**< Hessian matrix. \n - If Hessian matrix is trivial,a NULL pointer can be passed. */ - const real_t* const _g, /**< Gradient vector. */ - const real_t* const _lb, /**< Lower bounds (on variables). \n - If no lower bounds exist, a NULL pointer can be passed. */ - const real_t* const _ub /**< Upper bounds (on variables). \n - If no upper bounds exist, a NULL pointer can be passed. */ - ); - -/** Sets up internal QP data by loading it from files. If the current Hessian - * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, - * memory for Hessian is allocated and it is set to the given one. - * \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS \n - RET_NO_HESSIAN_SPECIFIED */ -returnValue QProblemBCPY_setupQPdataFromFile( QProblem* _THIS, - const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n - If Hessian matrix is trivial,a NULL pointer can be passed. */ - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - ); - -/** Loads new QP vectors from files (internal members are not affected!). - * \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE \n - RET_UNABLE_TO_READ_FILE \n - RET_INVALID_ARGUMENTS */ -returnValue QProblemBCPY_loadQPvectorsFromFile( QProblem* _THIS, - const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ - const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n - If no lower bounds exist, a NULL pointer can be passed. */ - const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n - If no upper bounds exist, a NULL pointer can be passed. */ - real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ - real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ - real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ - ); - - -/** Sets internal infeasibility flag and throws given error in case the far bound - * strategy is not enabled (as QP might actually not be infeasible in _THIS case). - * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n - RET_ENSURELI_FAILED_CYCLING \n - RET_ENSURELI_FAILED_NOINDEX */ -returnValue QProblem_setInfeasibilityFlag( QProblem* _THIS, - returnValue returnvalue, /**< Returnvalue to be tunneled. */ - BooleanType doThrowError /**< Flag forcing to throw an error. */ - ); - - -/** Determines if next QP iteration can be performed within given CPU time limit. - * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n - BT_FALSE: Sufficient CPU time for next QP iteration. */ -BooleanType QProblem_isCPUtimeLimitExceeded( QProblem* _THIS, - const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ - real_t starttime, /**< Start time of current QP solution. */ - int nWSR /**< Number of working set recalculations performed so far. */ - ); - - -/** Regularise Hessian matrix by adding a scaled identity matrix to it. - * \return SUCCESSFUL_RETURN \n - RET_HESSIAN_ALREADY_REGULARISED */ -returnValue QProblem_regulariseHessian( QProblem* _THIS ); - - -/** Sets Hessian matrix of the QP. - * \return SUCCESSFUL_RETURN */ -static inline returnValue QProblem_setHM( QProblem* _THIS, - DenseMatrix* H_new /**< New Hessian matrix. */ - ); - -/** Sets dense Hessian matrix of the QP. - * If a null pointer is passed and - * a) hessianType is HST_IDENTITY, nothing is done, - * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. - * \return SUCCESSFUL_RETURN */ -static inline returnValue QProblem_setH( QProblem* _THIS, - real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ - ); - -/** Changes gradient vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -static inline returnValue QProblem_setG( QProblem* _THIS, - const real_t* const g_new /**< New gradient vector (with correct dimension!). */ - ); - -/** Changes lower bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -static inline returnValue QProblem_setLB( QProblem* _THIS, - const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ - ); - -/** Changes single entry of lower bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - RET_INDEX_OUT_OF_BOUNDS */ -static inline returnValue QProblem_setLBn( QProblem* _THIS, - int number, /**< Number of entry to be changed. */ - real_t value /**< New value for entry of lower bound vector. */ - ); - -/** Changes upper bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - * RET_INVALID_ARGUMENTS */ -static inline returnValue QProblem_setUB( QProblem* _THIS, - const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ - ); - -/** Changes single entry of upper bound vector of the QP. - * \return SUCCESSFUL_RETURN \n - RET_INDEX_OUT_OF_BOUNDS */ -static inline returnValue QProblem_setUBn( QProblem* _THIS, - int number, /**< Number of entry to be changed. */ - real_t value /**< New value for entry of upper bound vector. */ - ); - - - -/** Compute relative length of homotopy in data space for termination - * criterion. - * \return Relative length in data space. */ -real_t QProblemBCPY_getRelativeHomotopyLength( QProblem* _THIS, - const real_t* const g_new, /**< Final gradient. */ - const real_t* const lb_new, /**< Final lower variable bounds. */ - const real_t* const ub_new /**< Final upper variable bounds. */ - ); - - - -/** Performs robustified ratio test yield the maximum possible step length - * along the homotopy path. - * \return SUCCESSFUL_RETURN */ -returnValue QProblem_performRatioTestB( QProblem* _THIS, - int nIdx, /**< Number of ratios to be checked. */ - const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ - Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ - const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ - const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ - real_t epsNum, /**< Numerator tolerance. */ - real_t epsDen, /**< Denominator tolerance. */ - real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ - int* BC_idx /**< Output: Index of blocking constraint. */ - ); - -/** Checks whether given ratio is blocking, i.e. limits the maximum step length - * along the homotopy path to a value lower than given one. - * \return SUCCESSFUL_RETURN */ -static inline BooleanType QProblem_isBlocking( QProblem* _THIS, - real_t num, /**< Numerator for performing the ratio test. */ - real_t den, /**< Denominator for performing the ratio test. */ - real_t epsNum, /**< Numerator tolerance. */ - real_t epsDen, /**< Denominator tolerance. */ - real_t* t /**< Input: Current maximum step length along the homotopy path, - * Output: Updated maximum possible step length along the homotopy path. */ - ); - - -/** ... - * \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE */ -returnValue QProblem_writeQpDataIntoMatFile( QProblem* _THIS, - const char* const filename /**< Mat file name. */ - ); - -/** ... -* \return SUCCESSFUL_RETURN \n - RET_UNABLE_TO_OPEN_FILE */ -returnValue QProblem_writeQpWorkspaceIntoMatFile( QProblem* _THIS, - const char* const filename /**< Mat file name. */ - ); - - -/* - * g e t B o u n d s - */ -static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds ) -{ - int nV = QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - _bounds = _THIS->bounds; - - return SUCCESSFUL_RETURN; -} - - -/* - * g e t N V - */ -static inline int QProblem_getNV( QProblem* _THIS ) -{ - return Bounds_getNV( _THIS->bounds ); -} - - -/* - * g e t N F R - */ -static inline int QProblem_getNFR( QProblem* _THIS ) -{ - return Bounds_getNFR( _THIS->bounds ); -} - - -/* - * g e t N F X - */ -static inline int QProblem_getNFX( QProblem* _THIS ) -{ - return Bounds_getNFX( _THIS->bounds ); -} - - -/* - * g e t N F V - */ -static inline int QProblem_getNFV( QProblem* _THIS ) -{ - return Bounds_getNFV( _THIS->bounds ); -} - - -/* - * g e t S t a t u s - */ -static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ) -{ - return _THIS->status; -} - - -/* - * i s I n i t i a l i s e d - */ -static inline BooleanType QProblem_isInitialised( QProblem* _THIS ) -{ - if ( _THIS->status == QPS_NOTINITIALISED ) - return BT_FALSE; - else - return BT_TRUE; -} - - -/* - * i s S o l v e d - */ -static inline BooleanType QProblem_isSolved( QProblem* _THIS ) -{ - if ( _THIS->status == QPS_SOLVED ) - return BT_TRUE; - else - return BT_FALSE; -} - - -/* - * i s I n f e a s i b l e - */ -static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ) -{ - return _THIS->infeasible; -} - - -/* - * i s U n b o u n d e d - */ -static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ) -{ - return _THIS->unbounded; -} - - -/* - * g e t H e s s i a n T y p e - */ -static inline HessianType QProblem_getHessianType( QProblem* _THIS ) -{ - return _THIS->hessianType; -} - - -/* - * s e t H e s s i a n T y p e - */ -static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType ) -{ - _THIS->hessianType = _hessianType; - return SUCCESSFUL_RETURN; -} - - -/* - * u s i n g R e g u l a r i s a t i o n - */ -static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ) -{ - if ( _THIS->regVal > QPOASES_ZERO ) - return BT_TRUE; - else - return BT_FALSE; -} - - -/* - * g e t O p t i o n s - */ -static inline Options QProblem_getOptions( QProblem* _THIS ) -{ - return _THIS->options; -} - - -/* - * s e t O p t i o n s - */ -static inline returnValue QProblem_setOptions( QProblem* _THIS, - Options _options - ) -{ - OptionsCPY( &_options,&(_THIS->options) ); - Options_ensureConsistency( &(_THIS->options) ); - - QProblem_setPrintLevel( _THIS,_THIS->options.printLevel ); - - return SUCCESSFUL_RETURN; -} - - -/* - * g e t P r i n t L e v e l - */ -static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ) -{ - return _THIS->options.printLevel; -} - - -/* - * g e t C o u n t - */ -static inline unsigned int QProblem_getCount( QProblem* _THIS ) -{ - return _THIS->count; -} - - -/* - * r e s e t C o u n t e r - */ -static inline returnValue QProblem_resetCounter( QProblem* _THIS ) -{ - _THIS->count = 0; - return SUCCESSFUL_RETURN; -} - - - -/***************************************************************************** - * P R O T E C T E D * - *****************************************************************************/ - - -/* - * s e t H - */ -static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new ) -{ - if ( H_new == 0 ) - return QProblem_setH( _THIS,(real_t*)0 ); - else - return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) ); -} - - -/* - * s e t H - */ -static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new ) -{ - /* if null pointer is passed, Hessian is set to zero matrix - * (or stays identity matrix) */ - if ( H_new == 0 ) - { - if ( _THIS->hessianType == HST_IDENTITY ) - return SUCCESSFUL_RETURN; - - _THIS->hessianType = HST_ZERO; - - _THIS->H = 0; - } - else - { - DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new ); - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t G - */ -static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new ) -{ - unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( g_new == 0 ) - return THROWERROR( RET_INVALID_ARGUMENTS ); - - memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t L B - */ -static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new ) -{ - unsigned int i; - unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( lb_new != 0 ) - { - memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); - } - else - { - /* if no lower bounds are specified, set them to -infinity */ - for( i=0; ilb[i] = -QPOASES_INFTY; - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t L B - */ -static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value ) -{ - int nV = QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ( number >= 0 ) && ( number < nV ) ) - { - _THIS->lb[number] = value; - return SUCCESSFUL_RETURN; - } - else - { - return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); - } -} - - -/* - * s e t U B - */ -static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new ) -{ - unsigned int i; - unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ub_new != 0 ) - { - memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); - } - else - { - /* if no upper bounds are specified, set them to infinity */ - for( i=0; iub[i] = QPOASES_INFTY; - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t U B - */ -static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value ) -{ - int nV = QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ( number >= 0 ) && ( number < nV ) ) - { - _THIS->ub[number] = value; - - return SUCCESSFUL_RETURN; - } - else - { - return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); - } -} - - - -/* - * i s B l o c k i n g - */ -static inline BooleanType QProblem_isBlocking( QProblem* _THIS, - real_t num, - real_t den, - real_t epsNum, - real_t epsDen, - real_t* t - ) -{ - if ( ( den >= epsDen ) && ( num >= epsNum ) ) - { - if ( num < (*t)*den ) - return BT_TRUE; - } - - return BT_FALSE; -} - - - -/* - * g e t C o n s t r a i n t s - */ -static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints ) -{ - int nV = QProblem_getNV( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - ConstraintsCPY( _THIS->constraints,_constraints ); - - return SUCCESSFUL_RETURN; -} - - - -/* - * g e t N C - */ -static inline int QProblem_getNC( QProblem* _THIS ) -{ - return Constraints_getNC( _THIS->constraints ); -} - - -/* - * g e t N E C - */ -static inline int QProblem_getNEC( QProblem* _THIS ) -{ - return Constraints_getNEC( _THIS->constraints ); -} - - -/* - * g e t N A C - */ -static inline int QProblem_getNAC( QProblem* _THIS ) -{ - return Constraints_getNAC( _THIS->constraints ); -} - - -/* - * g e t N I A C - */ -static inline int QProblem_getNIAC( QProblem* _THIS ) -{ - return Constraints_getNIAC( _THIS->constraints ); -} - - - -/***************************************************************************** - * P R O T E C T E D * - *****************************************************************************/ - - -/* - * s e t A - */ -static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new ) -{ - if ( A_new == 0 ) - return QProblem_setA( _THIS,(real_t*)0 ); - else - return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) ); -} - - -/* - * s e t A - */ -static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new ) -{ - int j; - int nV = QProblem_getNV( _THIS ); - int nC = QProblem_getNC( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( A_new == 0 ) - return THROWERROR( RET_INVALID_ARGUMENTS ); - - DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new ); - - DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC); - - for( j=0; jAx_u[j] = _THIS->ubA[j] - _THIS->Ax[j]; - _THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j]; - - /* (ckirches) disable constraints with empty rows */ - if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE ) - Constraints_setType( _THIS->constraints,j,ST_DISABLED ); - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t L B A - */ -static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new ) -{ - unsigned int i; - unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); - unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( lbA_new != 0 ) - { - memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) ); - } - else - { - /* if no lower constraints' bounds are specified, set them to -infinity */ - for( i=0; ilbA[i] = -QPOASES_INFTY; - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t L B A - */ -static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value ) -{ - int nV = QProblem_getNV( _THIS ); - int nC = QProblem_getNC( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ( number >= 0 ) && ( number < nC ) ) - { - _THIS->lbA[number] = value; - return SUCCESSFUL_RETURN; - } - else - return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); -} - - -/* - * s e t U B A - */ -static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new ) -{ - unsigned int i; - unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); - unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ubA_new != 0 ) - { - memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) ); - } - else - { - /* if no upper constraints' bounds are specified, set them to infinity */ - for( i=0; iubA[i] = QPOASES_INFTY; - } - - return SUCCESSFUL_RETURN; -} - - -/* - * s e t U B A - */ -static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value ) -{ - int nV = QProblem_getNV( _THIS ); - int nC = QProblem_getNC( _THIS ); - - if ( nV == 0 ) - return THROWERROR( RET_QPOBJECT_NOT_SETUP ); - - if ( ( number >= 0 ) && ( number < nC ) ) - { - _THIS->ubA[number] = value; - return SUCCESSFUL_RETURN; - } - else - return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); -} - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_QPROBLEM_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/QProblem.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the QProblem class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + + +#ifndef QPOASES_QPROBLEM_H +#define QPOASES_QPROBLEM_H + + +#include +#include +#include +#include +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + Bounds *auxiliaryBounds; + Constraints *auxiliaryConstraints; + + real_t *ub_new_far; + real_t *lb_new_far; + real_t *ubA_new_far; + real_t *lbA_new_far; + + real_t *g_new; + real_t *lb_new; + real_t *ub_new; + real_t *lbA_new; + real_t *ubA_new; + + real_t *g_new2; + real_t *lb_new2; + real_t *ub_new2; + real_t *lbA_new2; + real_t *ubA_new2; + + real_t *delta_xFX5; + real_t *delta_xFR5; + real_t *delta_yAC5; + real_t *delta_yFX5; + + real_t *Hx; + + real_t *_H; + + real_t *g_original; + real_t *lb_original; + real_t *ub_original; + real_t *lbA_original; + real_t *ubA_original; + + real_t *delta_xFR; + real_t *delta_xFX; + real_t *delta_yAC; + real_t *delta_yFX; + real_t *delta_g; + real_t *delta_lb; + real_t *delta_ub; + real_t *delta_lbA; + real_t *delta_ubA; + + real_t *gMod; + + real_t *aFR; + real_t *wZ; + + real_t *delta_g2; + real_t *delta_xFX2; + real_t *delta_xFR2; + real_t *delta_yAC2; + real_t *delta_yFX2; + real_t *nul; + real_t *Arow; + + real_t *xiC; + real_t *xiC_TMP; + real_t *xiB; + real_t *Arow2; + real_t *num; + + real_t *w; + real_t *tmp; + + real_t *delta_g3; + real_t *delta_xFX3; + real_t *delta_xFR3; + real_t *delta_yAC3; + real_t *delta_yFX3; + real_t *nul2; + + real_t *xiC2; + real_t *xiC_TMP2; + real_t *xiB2; + real_t *num2; + + real_t *Hz; + real_t *z; + real_t *ZHz; + real_t *r; + + real_t *tmp2; + real_t *Hz2; + real_t *z2; + real_t *r2; + real_t *rhs; + + real_t *delta_xFX4; + real_t *delta_xFR4; + real_t *delta_yAC4; + real_t *delta_yFX4; + real_t *nul3; + real_t *ek; + real_t *x_W; + real_t *As; + real_t *Ax_W; + + real_t *num3; + real_t *den; + real_t *delta_Ax_l; + real_t *delta_Ax_u; + real_t *delta_Ax; + real_t *delta_x; + + real_t *_A; + + real_t *grad; + real_t *AX; +} QProblem_ws; + +int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory ); + +QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC ); + +/** + * \brief Implements the online active set strategy for QPs with general constraints. + * + * A class for setting up and solving quadratic programs. The main feature is + * the possibily to use the newly developed online active set strategy for + * parametric quadratic programming. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + QProblem_ws *ws; /**< Workspace */ + Bounds *bounds; /**< Data structure for problem's bounds. */ + Constraints *constraints; /**< Data structure for problem's constraints. */ + Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ + + DenseMatrix* H; /**< Hessian matrix pointer. */ + DenseMatrix* A; /**< Constraint matrix pointer. */ + + Options options; /**< Struct containing all user-defined options for solving QPs. */ + TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ + + real_t *g; /**< Gradient. */ + + real_t *lb; /**< Lower bound vector (on variables). */ + real_t *ub; /**< Upper bound vector (on variables). */ + real_t *lbA; /**< Lower constraints' bound vector. */ + real_t *ubA; /**< Upper constraints' bound vector. */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + + real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + + real_t *Ax; /**< Stores the current A*x \n + * (for increased efficiency only). */ + real_t *Ax_l; /**< Stores the current distance to lower constraints' bounds A*x-lbA \n + * (for increased efficiency only). */ + real_t *Ax_u; /**< Stores the current distance to lower constraints' bounds ubA-A*x \n + * (for increased efficiency only). */ + + real_t *x; /**< Primal solution vector. */ + real_t *y; /**< Dual solution vector. */ + + real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ + real_t *tempA; /**< Temporary for determineStepDirection. */ + real_t *tempB; /**< Temporary for determineStepDirection. */ + real_t *ZFR_delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRy; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_yAC_TMP; /**< Temporary for determineStepDirection. */ + + ConstraintProduct constraintProduct; /**< Pointer to user-defined constraint product function. */ + + real_t tau; /**< Last homotopy step length. */ + real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ + + real_t ramp0; /**< Start value for Ramping Strategy. */ + real_t ramp1; /**< Final value for Ramping Strategy. */ + + QProblemStatus status; /**< Current status of the solution process. */ + HessianType hessianType; /**< Type of Hessian matrix. */ + + BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + int rampOffset; /**< Offset index for Ramping. */ + unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ + + int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */ +} QProblem; + +int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory ); + +QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC ); + + +/** Constructor which takes the QP dimension and Hessian type + * information. If the Hessian is the zero (i.e. HST_ZERO) or the + * identity matrix (i.e. HST_IDENTITY), respectively, no memory + * is allocated for it and a NULL pointer can be passed for it + * to the init() functions. */ +void QProblemCON( QProblem* _THIS, + int _nV, /**< Number of variables. */ + int _nC, /**< Number of constraints. */ + HessianType _hessianType /**< Type of Hessian matrix. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void QProblemCPY( QProblem* FROM, + QProblem* TO + ); + + +/** Clears all data structures of QProblem except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ +returnValue QProblem_reset( QProblem* _THIS ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_init( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data to be read from files and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initF( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initMW( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initW( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data to be ream from files and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initFW( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const char* const R_file /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_hotstart( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartF( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblem_hotstartW( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartFW( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + + +/** Solves using the current working set + * \return SUCCESSFUL_RETURN \n + * RET_STEPDIRECTION_FAILED_TQ \n + * RET_STEPDIRECTION_FAILED_CHOLESKY \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_solveCurrentEQP ( QProblem* _THIS, + const int n_rhs, /**< Number of consecutive right hand sides */ + const real_t* g_in, /**< Gradient of neighbouring QP to be solved. */ + const real_t* lb_in, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* ub_in, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* lbA_in, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* ubA_in, /**< Upper constraints' bounds of neighbouring QP to be solved. \n */ + real_t* x_out, /**< Output: Primal solution */ + real_t* y_out /**< Output: Dual solution */ + ); + + + +/** Returns current constraints object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, + Constraints* _constraints /** Output: Constraints object. */ + ); + + +/** Returns the number of constraints. + * \return Number of constraints. */ +static inline int QProblem_getNC( QProblem* _THIS ); + +/** Returns the number of (implicitly defined) equality constraints. + * \return Number of (implicitly defined) equality constraints. */ +static inline int QProblem_getNEC( QProblem* _THIS ); + +/** Returns the number of active constraints. + * \return Number of active constraints. */ +static inline int QProblem_getNAC( QProblem* _THIS ); + +/** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ +static inline int QProblem_getNIAC( QProblem* _THIS ); + +/** Returns the dimension of null space. + * \return Dimension of null space. */ +int QProblem_getNZ( QProblem* _THIS ); + + +/** Returns the dual solution vector (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getDualSolution( QProblem* _THIS, + real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ); + + +/** Defines user-defined routine for calculating the constraint product A*x + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_setConstraintProduct( QProblem* _THIS, + ConstraintProduct _constraintProduct + ); + + +/** Prints concise list of properties of the current QP. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printProperties( QProblem* _THIS ); + + + +/** Writes a vector with the state of the working set +* \return SUCCESSFUL_RETURN */ +returnValue QProblem_getWorkingSet( QProblem* _THIS, + real_t* workingSet /** Output: array containing state of the working set. */ + ); + +/** Writes a vector with the state of the working set of bounds + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetBounds( QProblem* _THIS, + real_t* workingSetB /** Output: array containing state of the working set of bounds. */ + ); + +/** Writes a vector with the state of the working set of constraints + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetConstraints( QProblem* _THIS, + real_t* workingSetC /** Output: array containing state of the working set of constraints. */ + ); + + +/** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, + Bounds* _bounds /** Output: Bounds object. */ + ); + + +/** Returns the number of variables. + * \return Number of variables. */ +static inline int QProblem_getNV( QProblem* _THIS ); + +/** Returns the number of free variables. + * \return Number of free variables. */ +static inline int QProblem_getNFR( QProblem* _THIS ); + +/** Returns the number of fixed variables. + * \return Number of fixed variables. */ +static inline int QProblem_getNFX( QProblem* _THIS ); + +/** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int QProblem_getNFV( QProblem* _THIS ); + + +/** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ +real_t QProblem_getObjVal( QProblem* _THIS ); + +/** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ +real_t QProblem_getObjValX( QProblem* _THIS, + const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ); + +/** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getPrimalSolution( QProblem* _THIS, + real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ); + + +/** Returns status of the solution process. + * \return Status of solution process. */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ); + + +/** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblem initialised \n + BT_FALSE: QProblem not initialised */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ); + +/** Returns if the QP has been solved. + * \return BT_TRUE: QProblem solved \n + BT_FALSE: QProblem not solved */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ); + +/** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ); + +/** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ); + + +/** Returns Hessian type flag (type is not determined due to _THIS call!). + * \return Hessian type. */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, + HessianType _hessianType /**< New Hessian type. */ + ); + +/** Returns if the QP has been internally regularised. + * \return BT_TRUE: Hessian is internally regularised for QP solution \n + BT_FALSE: No internal Hessian regularisation is used for QP solution */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ); + +/** Returns current options struct. + * \return Current options struct. */ +static inline Options QProblem_getOptions( QProblem* _THIS ); + +/** Overrides current options with given ones. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options /**< New options. */ + ); + +/** Returns the print level. + * \return Print level. */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setPrintLevel( QProblem* _THIS, + PrintLevel _printlevel /**< New print level. */ + ); + + +/** Returns the current number of QP problems solved. + * \return Number of QP problems solved. */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ); + +/** Resets QP problem counter (to zero). + * \return SUCCESSFUL_RETURN. */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ); + + +/** Prints a list of all options and their current values. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printOptions( QProblem* _THIS ); + + +/** Solves a QProblem whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * working sets of bounds and constraints can be provided. + * Note: This function is internally called by all init functions! + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ +returnValue QProblem_solveInitialQP( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector.*/ + const real_t* const yOpt, /**< Optimal dual solution vector. */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveQP() calls. This number is + always zero, except for successive calls from solveRegularisedQP() + or when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveRegularisedQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveRegularisedQP() calls. This number is + always zero, except for successive calls when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToType( QProblem* _THIS ); + +/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToTypeNew( QProblem* _THIS, + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new /**< New upper constraints' bounds. */ + ); + +/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z). + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_computeProjectedCholesky( QProblem* _THIS ); + +/** Computes initial Cholesky decomposition of the projected Hessian making + * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupInitialCholesky( QProblem* _THIS ); + +/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupTQfactorisation( QProblem* _THIS ); + + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * (assumes that member AX has already been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n + * Ouput: Working set of constraints for auxiliary QP. */ + Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ); + +/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints. + * (If the working set shall be setup afresh, make sure that + * bounds and constraints data structure have been resetted + * and the TQ factorisation has been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryWorkingSet( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + +/** Sets up the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPsolution( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + +/** Sets up gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS ); + +/** Sets up (constraints') bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryQPbounds( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */ + ); + + +/** Adds a constraint to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDCONSTRAINT_FAILED \n + RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status, /**< Status of new active constraint. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active constraint to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_addConstraint_checkLI( QProblem* _THIS, + int number /**< Number of constraint to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new constraint is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addConstraint_ensureLI( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status /**< Status of new active bound. */ + ); + +/** Adds a bound to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED \n + RET_ADDBOUND_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addBound( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active bound to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT */ +returnValue QProblem_addBound_checkLI( QProblem* _THIS, + int number /**< Number of bound to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new bound is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addBound_ensureLI( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status /**< Status of new active bound. */ + ); + +/** Removes a constraint from active set. + * \return SUCCESSFUL_RETURN \n + RET_CONSTRAINT_NOT_ACTIVE \n + RET_REMOVECONSTRAINT_FAILED \n + RET_HESSIAN_NOT_SPD */ +returnValue QProblem_removeConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + +/** Removes a bounds from active set. + * \return SUCCESSFUL_RETURN \n + RET_BOUND_NOT_ACTIVE \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ +returnValue QProblem_removeBound( QProblem* _THIS, + int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performPlainRatioTest( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Ensure non-zero curvature by primal jump. + * \return SUCCESSFUL_RETURN \n + * RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_ensureNonzeroCurvature( QProblem* _THIS, + BooleanType removeBoundNotConstraint, /**< SubjectTo to be removed is a bound. */ + int remIdx, /**< Index of bound/constraint to be removed. */ + BooleanType* exchangeHappened, /**< Output: Exchange was necessary to ensure. */ + BooleanType* addBoundNotConstraint, /**< SubjectTo to be added is a bound. */ + int* addIdx, /**< Index of bound/constraint to be added. */ + SubjectToStatus* addStatus /**< Status of bound/constraint to be added. */ + ); + + +/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveT( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new, /**< New upper constraints' bounds. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */ + real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bC_isZero, /**< Output: Indicates if active constraints' bounds are to be shifted. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + +/** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_TQ \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ +returnValue QProblem_determineStepDirection( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + +/** Determines the maximum possible step length along the homotopy path + * and performs _THIS step (without changing working set). + * \return SUCCESSFUL_RETURN \n + * RET_ERROR_IN_CONSTRAINTPRODUCT \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_performStep( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int* BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus* BC_status, /**< Output: Status of blocking constraint. */ + BooleanType* BC_isBound /**< Output: Indicates if blocking constraint is a bound. */ + ); + +/** Updates the active set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED */ +returnValue QProblem_changeActiveSet( QProblem* _THIS, + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ + ); + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblem_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new, /**< Final upper variable bounds. */ + const real_t* const lbA_new, /**< Final lower constraint bounds. */ + const real_t* const ubA_new /**< Final upper constraint bounds. */ + ); + + +/** Ramping Strategy to avoid ties. Modifies homotopy start without + * changing current active set. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRamping( QProblem* _THIS ); + + +/** ... */ +returnValue QProblem_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far, /**< ... */ + const real_t* const lbA_new, /**< ... */ + real_t* const lbA_new_far, /**< ... */ + const real_t* const ubA_new, /**< ... */ + real_t* const ubA_new_far /**< ... */ + ); + +/** ... */ +returnValue QProblemBCPY_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far /**< ... */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestC( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Constraints* const subjectTo, /**< Constraint object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Drift correction at end of each active set iteration + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performDriftCorrection( QProblem* _THIS ); + + +/** Updates QP vectors, working sets and internal data structures in order to + start from an optimal solution corresponding to initial guesses of the working + set for bounds and constraints. + * \return SUCCESSFUL_RETURN \n + * RET_SETUP_AUXILIARYQP_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_setupAuxiliaryQP( QProblem* _THIS, + Bounds* const guessedBounds, /**< Initial guess for working set of bounds. */ + Constraints* const guessedConstraints /**< Initial guess for working set of constraints. */ + ); + +/** Determines if it is more efficient to refactorise the matrices when + * hotstarting or not (i.e. better to update the existing factorisations). + * \return BT_TRUE iff matrices shall be refactorised afresh + */ +BooleanType QProblem_shallRefactorise( QProblem* _THIS, + Bounds* const guessedBounds, /**< Guessed new working set of bounds. */ + Constraints* const guessedConstraints /**< Guessed new working set of constraints. */ + ); + +/** Setups internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + + +/** Sets up dense internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const A_file, /**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new, /**< Output: Upper bounds of neighbouring QP to be solved */ + real_t* const lbA_new, /**< Output: Lower constraints' bounds of neighbouring QP to be solved */ + real_t* const ubA_new /**< Output: Upper constraints' bounds of neighbouring QP to be solved */ + ); + + +/** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printIteration( QProblem* _THIS, + int iter, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound, /**< Indicates if blocking constraint is a bound. */ + real_t homotopyLength, /**< Current homotopy distance. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Sets constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setAM( QProblem* _THIS, + DenseMatrix *A_new /**< New constraint matrix. */ + ); + +/** Sets dense constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setA( QProblem* _THIS, + real_t* const A_new /**< New dense constraint matrix (with correct dimension!). */ + ); + + +/** Sets constraints' lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, + const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */ + ); + +/** Sets constraints' upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, + const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */ + ); + + +/** Decides if lower bounds are smaller than upper bounds + * + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_areBoundsConsistent( QProblem* _THIS, + const real_t* const lb, /**< Vector of lower bounds*/ + const real_t* const ub, /**< Vector of upper bounds*/ + const real_t* const lbA, /**< Vector of lower constraints*/ + const real_t* const ubA /**< Vector of upper constraints*/ + ); + + +/** Drops the blocking bound/constraint that led to infeasibility, or finds another + * bound/constraint to drop according to drop priorities. + * \return SUCCESSFUL_RETURN \n + */ +returnValue QProblem_dropInfeasibles ( QProblem* _THIS, + int BC_number, /**< Number of the bound or constraint to be added */ + SubjectToStatus BC_status, /**< New status of the bound or constraint to be added */ + BooleanType BC_isBound, /**< Whether a bound or a constraint is to be added */ + real_t *xiB, + real_t *xiC + ); + + +/** If Hessian type has been set by the user, nothing is done. + * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or + * HST_POSDEF (default), respectively. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_INDEFINITE */ +returnValue QProblem_determineHessianType( QProblem* _THIS ); + +/** Computes the Cholesky decomposition of the (simply projected) Hessian + * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple + * projection matrix! + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemBCPY_computeCholesky( QProblem* _THIS ); + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Output: Working set for auxiliary QP. */ + ); + + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveR( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that _THIS function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveRrem( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemBCPY_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + + +/** Sets up internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix.*/ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ + ); + + +/** Sets internal infeasibility flag and throws given error in case the far bound + * strategy is not enabled (as QP might actually not be infeasible in _THIS case). + * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_ENSURELI_FAILED_CYCLING \n + RET_ENSURELI_FAILED_NOINDEX */ +returnValue QProblem_setInfeasibilityFlag( QProblem* _THIS, + returnValue returnvalue, /**< Returnvalue to be tunneled. */ + BooleanType doThrowError /**< Flag forcing to throw an error. */ + ); + + +/** Determines if next QP iteration can be performed within given CPU time limit. + * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n + BT_FALSE: Sufficient CPU time for next QP iteration. */ +BooleanType QProblem_isCPUtimeLimitExceeded( QProblem* _THIS, + const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ + real_t starttime, /**< Start time of current QP solution. */ + int nWSR /**< Number of working set recalculations performed so far. */ + ); + + +/** Regularise Hessian matrix by adding a scaled identity matrix to it. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_ALREADY_REGULARISED */ +returnValue QProblem_regulariseHessian( QProblem* _THIS ); + + +/** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHM( QProblem* _THIS, + DenseMatrix* H_new /**< New Hessian matrix. */ + ); + +/** Sets dense Hessian matrix of the QP. + * If a null pointer is passed and + * a) hessianType is HST_IDENTITY, nothing is done, + * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setH( QProblem* _THIS, + real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ + ); + +/** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setG( QProblem* _THIS, + const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + +/** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setLB( QProblem* _THIS, + const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + +/** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setUB( QProblem* _THIS, + const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblemBCPY_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new /**< Final upper variable bounds. */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestB( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + +/** Checks whether given ratio is blocking, i.e. limits the maximum step length + * along the homotopy path to a value lower than given one. + * \return SUCCESSFUL_RETURN */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, /**< Numerator for performing the ratio test. */ + real_t den, /**< Denominator for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t /**< Input: Current maximum step length along the homotopy path, + * Output: Updated maximum possible step length along the homotopy path. */ + ); + + +/** ... + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpDataIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + +/** ... +* \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpWorkspaceIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + + +/* + * g e t B o u n d s + */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + _bounds = _THIS->bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +static inline int QProblem_getNV( QProblem* _THIS ) +{ + return Bounds_getNV( _THIS->bounds ); +} + + +/* + * g e t N F R + */ +static inline int QProblem_getNFR( QProblem* _THIS ) +{ + return Bounds_getNFR( _THIS->bounds ); +} + + +/* + * g e t N F X + */ +static inline int QProblem_getNFX( QProblem* _THIS ) +{ + return Bounds_getNFX( _THIS->bounds ); +} + + +/* + * g e t N F V + */ +static inline int QProblem_getNFV( QProblem* _THIS ) +{ + return Bounds_getNFV( _THIS->bounds ); +} + + +/* + * g e t S t a t u s + */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ) +{ + return _THIS->status; +} + + +/* + * i s I n i t i a l i s e d + */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ) +{ + return _THIS->infeasible; +} + + +/* + * i s U n b o u n d e d + */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ) +{ + return _THIS->unbounded; +} + + +/* + * g e t H e s s i a n T y p e + */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ) +{ + return _THIS->hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType ) +{ + _THIS->hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + +/* + * u s i n g R e g u l a r i s a t i o n + */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ) +{ + if ( _THIS->regVal > QPOASES_ZERO ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t O p t i o n s + */ +static inline Options QProblem_getOptions( QProblem* _THIS ) +{ + return _THIS->options; +} + + +/* + * s e t O p t i o n s + */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options + ) +{ + OptionsCPY( &_options,&(_THIS->options) ); + Options_ensureConsistency( &(_THIS->options) ); + + QProblem_setPrintLevel( _THIS,_THIS->options.printLevel ); + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t P r i n t L e v e l + */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ) +{ + return _THIS->options.printLevel; +} + + +/* + * g e t C o u n t + */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ) +{ + return _THIS->count; +} + + +/* + * r e s e t C o u n t e r + */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ) +{ + _THIS->count = 0; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t H + */ +static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new ) +{ + if ( H_new == 0 ) + return QProblem_setH( _THIS,(real_t*)0 ); + else + return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) ); +} + + +/* + * s e t H + */ +static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new ) +{ + /* if null pointer is passed, Hessian is set to zero matrix + * (or stays identity matrix) */ + if ( H_new == 0 ) + { + if ( _THIS->hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + _THIS->hessianType = HST_ZERO; + + _THIS->H = 0; + } + else + { + DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t G + */ +static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new ) +{ + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( g_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lb_new != 0 ) + { + memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); + } + else + { + /* if no lower bounds are specified, set them to -infinity */ + for( i=0; ilb[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ub_new != 0 ) + { + memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); + } + else + { + /* if no upper bounds are specified, set them to infinity */ + for( i=0; iub[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + + +/* + * i s B l o c k i n g + */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, + real_t den, + real_t epsNum, + real_t epsDen, + real_t* t + ) +{ + if ( ( den >= epsDen ) && ( num >= epsNum ) ) + { + if ( num < (*t)*den ) + return BT_TRUE; + } + + return BT_FALSE; +} + + + +/* + * g e t C o n s t r a i n t s + */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + ConstraintsCPY( _THIS->constraints,_constraints ); + + return SUCCESSFUL_RETURN; +} + + + +/* + * g e t N C + */ +static inline int QProblem_getNC( QProblem* _THIS ) +{ + return Constraints_getNC( _THIS->constraints ); +} + + +/* + * g e t N E C + */ +static inline int QProblem_getNEC( QProblem* _THIS ) +{ + return Constraints_getNEC( _THIS->constraints ); +} + + +/* + * g e t N A C + */ +static inline int QProblem_getNAC( QProblem* _THIS ) +{ + return Constraints_getNAC( _THIS->constraints ); +} + + +/* + * g e t N I A C + */ +static inline int QProblem_getNIAC( QProblem* _THIS ) +{ + return Constraints_getNIAC( _THIS->constraints ); +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t A + */ +static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new ) +{ + if ( A_new == 0 ) + return QProblem_setA( _THIS,(real_t*)0 ); + else + return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) ); +} + + +/* + * s e t A + */ +static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new ) +{ + int j; + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( A_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new ); + + DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC); + + for( j=0; jAx_u[j] = _THIS->ubA[j] - _THIS->Ax[j]; + _THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j]; + + /* (ckirches) disable constraints with empty rows */ + if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE ) + Constraints_setType( _THIS->constraints,j,ST_DISABLED ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lbA_new != 0 ) + { + memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) ); + } + else + { + /* if no lower constraints' bounds are specified, set them to -infinity */ + for( i=0; ilbA[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->lbA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ubA_new != 0 ) + { + memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) ); + } + else + { + /* if no upper constraints' bounds are specified, set them to infinity */ + for( i=0; iubA[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->ubA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_QPROBLEM_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Types.h b/third_party/acados/include/qpOASES_e/Types.h index fc042aed826872..1e452097f8536d 100644 --- a/third_party/acados/include/qpOASES_e/Types.h +++ b/third_party/acados/include/qpOASES_e/Types.h @@ -1,310 +1,310 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/Types.h - * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches - * \version 3.1embedded - * \date 2007-2015 - * - * Declaration of all non-built-in types (except for classes). - */ - - -#ifndef QPOASES_TYPES_H -#define QPOASES_TYPES_H - -#ifdef USE_ACADOS_TYPES -#include "acados/utils/types.h" -#endif - -/* If your compiler does not support the snprintf() function, - * uncomment the following line and try to compile again. */ -/* #define __NO_SNPRINTF__ */ - - -/* Uncomment the following line for setting the __DSPACE__ flag. */ -/* #define __DSPACE__ */ - -/* Uncomment the following line for setting the __XPCTARGET__ flag. */ -/* #define __XPCTARGET__ */ - - -/* Uncomment the following line for setting the __NO_FMATH__ flag. */ -/* #define __NO_FMATH__ */ - -/* Uncomment the following line to enable debug information. */ -/* #define __DEBUG__ */ - -/* Uncomment the following line to enable suppress any kind of console output. */ -/* #define __SUPPRESSANYOUTPUT__ */ - - -/** Forces to always include all implicitly fixed bounds and all equality constraints - * into the initial working set when setting up an auxiliary QP. */ -#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__ - -/* Uncomment the following line to activate the use of an alternative Givens - * plane rotation requiring only three multiplications. */ -/* #define __USE_THREE_MULTS_GIVENS__ */ - -/* Uncomment the following line to activate the use of single precision arithmetic. */ -/* #define __USE_SINGLE_PRECISION__ */ - -/* The inline keyword is skipped by default as it is not part of the C90 standard. - * However, by uncommenting the following line, use of the inline keyword can be enforced. */ -/* #define __USE_INLINE__ */ - - -/* Work-around for Borland BCC 5.5 compiler. */ -#ifdef __BORLANDC__ -#if __BORLANDC__ < 0x0561 - #define __STDC__ 1 -#endif -#endif - - -/* Work-around for Microsoft compilers. */ -#ifdef _MSC_VER - #define __NO_SNPRINTF__ - #pragma warning( disable : 4061 4100 4250 4514 4996 ) -#endif - - -/* Apply pre-processor settings when using qpOASES within auto-generated code. */ -#ifdef __CODE_GENERATION__ - #define __NO_COPYRIGHT__ - #define __EXTERNAL_DIMENSIONS__ -#endif /* __CODE_GENERATION__ */ - - -/* Avoid using static variables declaration within functions. */ -#ifdef __NO_STATIC__ - #define myStatic -#else - #define myStatic static -#endif /* __NO_STATIC__ */ - - -/* Skip inline keyword if not specified otherwise. */ -#ifndef __USE_INLINE__ - #define inline -#endif - - -/* Avoid any printing on embedded platforms. */ -#if defined(__DSPACE__) || defined(__XPCTARGET__) - #define __SUPPRESSANYOUTPUT__ - #define __NO_SNPRINTF__ -#endif - - -#ifdef __NO_SNPRINTF__ - #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__) - /* If snprintf is not available, provide an empty implementation... */ - int snprintf( char* s, size_t n, const char* format, ... ); - #else - /* ... or substitute snprintf by _snprintf for Microsoft compilers. */ - #define snprintf _snprintf - #endif -#endif /* __NO_SNPRINTF__ */ - - -/** Macro for switching on/off the beginning of the qpOASES namespace definition. */ -#define BEGIN_NAMESPACE_QPOASES - -/** Macro for switching on/off the end of the qpOASES namespace definition. */ -#define END_NAMESPACE_QPOASES - -/** Macro for switching on/off the use of the qpOASES namespace. */ -#define USING_NAMESPACE_QPOASES - -/** Macro for switching on/off references to the qpOASES namespace. */ -#define REFER_NAMESPACE_QPOASES /*::*/ - - -/** Macro for accessing the Cholesky factor R. */ -#define RR( I,J ) _THIS->R[(I)+nV*(J)] - -/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */ -#define QQ( I,J ) _THIS->Q[(I)+nV*(J)] - -/** Macro for accessing the triangular matrix T of the QT factorisation. */ -#define TT( I,J ) _THIS->T[(I)*nVC_min+(J)] - - - -BEGIN_NAMESPACE_QPOASES - - -/** Defines real_t for facilitating switching between double and float. */ - -#ifndef USE_ACADOS_TYPES -#ifndef __CODE_GENERATION__ - - #ifdef __USE_SINGLE_PRECISION__ - typedef float real_t; - #else - typedef double real_t; - #endif /* __USE_SINGLE_PRECISION__ */ - -#endif /* __CODE_GENERATION__ */ -#endif /* USE_ACADOS_TYPES */ - -/** Summarises all possible logical values. */ -typedef enum -{ - BT_FALSE = 0, /**< Logical value for "false". */ - BT_TRUE /**< Logical value for "true". */ -} BooleanType; - - -/** Summarises all possible print levels. Print levels are used to describe - * the desired amount of output during runtime of qpOASES. */ -typedef enum -{ - PL_DEBUG_ITER = -2, /**< Full tabular debugging output. */ - PL_TABULAR, /**< Tabular output. */ - PL_NONE, /**< No output. */ - PL_LOW, /**< Print error messages only. */ - PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */ - PL_HIGH /**< Print all messages with full details. */ -} PrintLevel; - - -/** Defines visibility status of a message. */ -typedef enum -{ - VS_HIDDEN, /**< Message not visible. */ - VS_VISIBLE /**< Message visible. */ -} VisibilityStatus; - - -/** Summarises all possible states of the (S)QProblem(B) object during the -solution process of a QP sequence. */ -typedef enum -{ - QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */ - QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning - * via an initial homotopy or after changing the QP matrices. */ - QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning - * via an initial homotopy or after changing the QP matrices. */ - QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active - * set strategy is performed. */ - QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */ - QPS_SOLVED /**< The solution of the actual QP was found. */ -} QProblemStatus; - - -/** Summarises all possible types of the QP's Hessian matrix. */ -typedef enum -{ - HST_ZERO, /**< Hessian is zero matrix (i.e. LP formulation). */ - HST_IDENTITY, /**< Hessian is identity matrix. */ - HST_POSDEF, /**< Hessian is (strictly) positive definite. */ - HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */ - HST_SEMIDEF, /**< Hessian is positive semi-definite. */ - HST_INDEF, /**< Hessian is indefinite. */ - HST_UNKNOWN /**< Hessian type is unknown. */ -} HessianType; - - -/** Summarises all possible types of bounds and constraints. */ -typedef enum -{ - ST_UNBOUNDED, /**< Bound/constraint is unbounded. */ - ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */ - ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */ - ST_DISABLED, /**< Bound/constraint is disabled (i.e. ignored when solving QP). */ - ST_UNKNOWN /**< Type of bound/constraint unknown. */ -} SubjectToType; - - -/** Summarises all possible states of bounds and constraints. */ -typedef enum -{ - ST_LOWER = -1, /**< Bound/constraint is at its lower bound. */ - ST_INACTIVE, /**< Bound/constraint is inactive. */ - ST_UPPER, /**< Bound/constraint is at its upper bound. */ - ST_INFEASIBLE_LOWER, /**< (to be documented) */ - ST_INFEASIBLE_UPPER, /**< (to be documented) */ - ST_UNDEFINED /**< Status of bound/constraint undefined. */ -} SubjectToStatus; - - -/** - * \brief Stores internal information for tabular (debugging) output. - * - * Struct storing internal information for tabular (debugging) output - * when using the (S)QProblem(B) objects. - * - * \author Hans Joachim Ferreau - * \version 3.1embedded - * \date 2013-2015 - */ -typedef struct -{ - int idxAddB; /**< Index of bound that has been added to working set. */ - int idxRemB; /**< Index of bound that has been removed from working set. */ - int idxAddC; /**< Index of constraint that has been added to working set. */ - int idxRemC; /**< Index of constraint that has been removed from working set. */ - int excAddB; /**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */ - int excRemB; /**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */ - int excAddC; /**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */ - int excRemC; /**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */ -} TabularOutput; - -/** - * \brief Struct containing the variable header for mat file. - * - * Struct storing the header of a variable to be stored in - * Matlab's binary format (using the outdated Level 4 variant - * for simplictiy). - * - * Note, this code snippet has been inspired from the document - * "Matlab(R) MAT-file Format, R2013b" by MathWorks - * - * \author Hans Joachim Ferreau - * \version 3.1embedded - * \date 2013-2015 - */ -typedef struct -{ - long numericFormat; /**< Flag indicating numerical format. */ - long nRows; /**< Number of rows. */ - long nCols; /**< Number of rows. */ - long imaginaryPart; /**< (to be documented) */ - long nCharName; /**< Number of character in name. */ -} MatMatrixHeader; - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_TYPES_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Types.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of all non-built-in types (except for classes). + */ + + +#ifndef QPOASES_TYPES_H +#define QPOASES_TYPES_H + +#ifdef USE_ACADOS_TYPES +#include "acados/utils/types.h" +#endif + +/* If your compiler does not support the snprintf() function, + * uncomment the following line and try to compile again. */ +/* #define __NO_SNPRINTF__ */ + + +/* Uncomment the following line for setting the __DSPACE__ flag. */ +/* #define __DSPACE__ */ + +/* Uncomment the following line for setting the __XPCTARGET__ flag. */ +/* #define __XPCTARGET__ */ + + +/* Uncomment the following line for setting the __NO_FMATH__ flag. */ +/* #define __NO_FMATH__ */ + +/* Uncomment the following line to enable debug information. */ +/* #define __DEBUG__ */ + +/* Uncomment the following line to enable suppress any kind of console output. */ +/* #define __SUPPRESSANYOUTPUT__ */ + + +/** Forces to always include all implicitly fixed bounds and all equality constraints + * into the initial working set when setting up an auxiliary QP. */ +#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__ + +/* Uncomment the following line to activate the use of an alternative Givens + * plane rotation requiring only three multiplications. */ +/* #define __USE_THREE_MULTS_GIVENS__ */ + +/* Uncomment the following line to activate the use of single precision arithmetic. */ +/* #define __USE_SINGLE_PRECISION__ */ + +/* The inline keyword is skipped by default as it is not part of the C90 standard. + * However, by uncommenting the following line, use of the inline keyword can be enforced. */ +/* #define __USE_INLINE__ */ + + +/* Work-around for Borland BCC 5.5 compiler. */ +#ifdef __BORLANDC__ +#if __BORLANDC__ < 0x0561 + #define __STDC__ 1 +#endif +#endif + + +/* Work-around for Microsoft compilers. */ +#ifdef _MSC_VER + #define __NO_SNPRINTF__ + #pragma warning( disable : 4061 4100 4250 4514 4996 ) +#endif + + +/* Apply pre-processor settings when using qpOASES within auto-generated code. */ +#ifdef __CODE_GENERATION__ + #define __NO_COPYRIGHT__ + #define __EXTERNAL_DIMENSIONS__ +#endif /* __CODE_GENERATION__ */ + + +/* Avoid using static variables declaration within functions. */ +#ifdef __NO_STATIC__ + #define myStatic +#else + #define myStatic static +#endif /* __NO_STATIC__ */ + + +/* Skip inline keyword if not specified otherwise. */ +#ifndef __USE_INLINE__ + #define inline +#endif + + +/* Avoid any printing on embedded platforms. */ +#if defined(__DSPACE__) || defined(__XPCTARGET__) + #define __SUPPRESSANYOUTPUT__ + #define __NO_SNPRINTF__ +#endif + + +#ifdef __NO_SNPRINTF__ + #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__) + /* If snprintf is not available, provide an empty implementation... */ + int snprintf( char* s, size_t n, const char* format, ... ); + #else + /* ... or substitute snprintf by _snprintf for Microsoft compilers. */ + #define snprintf _snprintf + #endif +#endif /* __NO_SNPRINTF__ */ + + +/** Macro for switching on/off the beginning of the qpOASES namespace definition. */ +#define BEGIN_NAMESPACE_QPOASES + +/** Macro for switching on/off the end of the qpOASES namespace definition. */ +#define END_NAMESPACE_QPOASES + +/** Macro for switching on/off the use of the qpOASES namespace. */ +#define USING_NAMESPACE_QPOASES + +/** Macro for switching on/off references to the qpOASES namespace. */ +#define REFER_NAMESPACE_QPOASES /*::*/ + + +/** Macro for accessing the Cholesky factor R. */ +#define RR( I,J ) _THIS->R[(I)+nV*(J)] + +/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */ +#define QQ( I,J ) _THIS->Q[(I)+nV*(J)] + +/** Macro for accessing the triangular matrix T of the QT factorisation. */ +#define TT( I,J ) _THIS->T[(I)*nVC_min+(J)] + + + +BEGIN_NAMESPACE_QPOASES + + +/** Defines real_t for facilitating switching between double and float. */ + +#ifndef USE_ACADOS_TYPES +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + typedef float real_t; + #else + typedef double real_t; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ +#endif /* USE_ACADOS_TYPES */ + +/** Summarises all possible logical values. */ +typedef enum +{ + BT_FALSE = 0, /**< Logical value for "false". */ + BT_TRUE /**< Logical value for "true". */ +} BooleanType; + + +/** Summarises all possible print levels. Print levels are used to describe + * the desired amount of output during runtime of qpOASES. */ +typedef enum +{ + PL_DEBUG_ITER = -2, /**< Full tabular debugging output. */ + PL_TABULAR, /**< Tabular output. */ + PL_NONE, /**< No output. */ + PL_LOW, /**< Print error messages only. */ + PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */ + PL_HIGH /**< Print all messages with full details. */ +} PrintLevel; + + +/** Defines visibility status of a message. */ +typedef enum +{ + VS_HIDDEN, /**< Message not visible. */ + VS_VISIBLE /**< Message visible. */ +} VisibilityStatus; + + +/** Summarises all possible states of the (S)QProblem(B) object during the +solution process of a QP sequence. */ +typedef enum +{ + QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */ + QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active + * set strategy is performed. */ + QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */ + QPS_SOLVED /**< The solution of the actual QP was found. */ +} QProblemStatus; + + +/** Summarises all possible types of the QP's Hessian matrix. */ +typedef enum +{ + HST_ZERO, /**< Hessian is zero matrix (i.e. LP formulation). */ + HST_IDENTITY, /**< Hessian is identity matrix. */ + HST_POSDEF, /**< Hessian is (strictly) positive definite. */ + HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */ + HST_SEMIDEF, /**< Hessian is positive semi-definite. */ + HST_INDEF, /**< Hessian is indefinite. */ + HST_UNKNOWN /**< Hessian type is unknown. */ +} HessianType; + + +/** Summarises all possible types of bounds and constraints. */ +typedef enum +{ + ST_UNBOUNDED, /**< Bound/constraint is unbounded. */ + ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */ + ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */ + ST_DISABLED, /**< Bound/constraint is disabled (i.e. ignored when solving QP). */ + ST_UNKNOWN /**< Type of bound/constraint unknown. */ +} SubjectToType; + + +/** Summarises all possible states of bounds and constraints. */ +typedef enum +{ + ST_LOWER = -1, /**< Bound/constraint is at its lower bound. */ + ST_INACTIVE, /**< Bound/constraint is inactive. */ + ST_UPPER, /**< Bound/constraint is at its upper bound. */ + ST_INFEASIBLE_LOWER, /**< (to be documented) */ + ST_INFEASIBLE_UPPER, /**< (to be documented) */ + ST_UNDEFINED /**< Status of bound/constraint undefined. */ +} SubjectToStatus; + + +/** + * \brief Stores internal information for tabular (debugging) output. + * + * Struct storing internal information for tabular (debugging) output + * when using the (S)QProblem(B) objects. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + int idxAddB; /**< Index of bound that has been added to working set. */ + int idxRemB; /**< Index of bound that has been removed from working set. */ + int idxAddC; /**< Index of constraint that has been added to working set. */ + int idxRemC; /**< Index of constraint that has been removed from working set. */ + int excAddB; /**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */ + int excRemB; /**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */ + int excAddC; /**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */ + int excRemC; /**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */ +} TabularOutput; + +/** + * \brief Struct containing the variable header for mat file. + * + * Struct storing the header of a variable to be stored in + * Matlab's binary format (using the outdated Level 4 variant + * for simplictiy). + * + * Note, this code snippet has been inspired from the document + * "Matlab(R) MAT-file Format, R2013b" by MathWorks + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + long numericFormat; /**< Flag indicating numerical format. */ + long nRows; /**< Number of rows. */ + long nCols; /**< Number of rows. */ + long imaginaryPart; /**< (to be documented) */ + long nCharName; /**< Number of character in name. */ +} MatMatrixHeader; + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_TYPES_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/UnitTesting.h b/third_party/acados/include/qpOASES_e/UnitTesting.h index dbff201039eb58..3fb31129a5752d 100644 --- a/third_party/acados/include/qpOASES_e/UnitTesting.h +++ b/third_party/acados/include/qpOASES_e/UnitTesting.h @@ -1,79 +1,79 @@ -/* - * This file is part of qpOASES. - * - * qpOASES -- An Implementation of the Online Active Set Strategy. - * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, - * Christian Kirches et al. All rights reserved. - * - * qpOASES is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * qpOASES is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with qpOASES; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - - -/** - * \file include/qpOASES_e/UnitTesting.h - * \author Hans Joachim Ferreau - * \version 3.1embedded - * \date 2014-2015 - * - * Definition of auxiliary functions/macros for unit testing. - */ - - -#ifndef QPOASES_UNIT_TESTING_H -#define QPOASES_UNIT_TESTING_H - - -#ifndef TEST_TOL_FACTOR -#define TEST_TOL_FACTOR 1 -#endif - - -/** Return value for tests that passed. */ -#define TEST_PASSED 0 - -/** Return value for tests that failed. */ -#define TEST_FAILED 1 - -/** Return value for tests that could not run due to missing external data. */ -#define TEST_DATA_NOT_FOUND 99 - - -/** Macro verifying that two numerical values are equal in order to pass unit test. */ -#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; } - -/** Macro verifying that two numerical values are close to each other in order to pass unit test. */ -#define QPOASES_TEST_FOR_NEAR( x,y ) if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; } - -/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */ -#define QPOASES_TEST_FOR_TOL( x,tol ) if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; } - -/** Macro verifying that a logical expression holds in order to pass unit test. */ -#define QPOASES_TEST_FOR_TRUE( x ) if ( (x) == 0 ) { return TEST_FAILED; } - - - -BEGIN_NAMESPACE_QPOASES - - -END_NAMESPACE_QPOASES - - -#endif /* QPOASES_UNIT_TESTING_H */ - - -/* - * end of file - */ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/UnitTesting.h + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2014-2015 + * + * Definition of auxiliary functions/macros for unit testing. + */ + + +#ifndef QPOASES_UNIT_TESTING_H +#define QPOASES_UNIT_TESTING_H + + +#ifndef TEST_TOL_FACTOR +#define TEST_TOL_FACTOR 1 +#endif + + +/** Return value for tests that passed. */ +#define TEST_PASSED 0 + +/** Return value for tests that failed. */ +#define TEST_FAILED 1 + +/** Return value for tests that could not run due to missing external data. */ +#define TEST_DATA_NOT_FOUND 99 + + +/** Macro verifying that two numerical values are equal in order to pass unit test. */ +#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; } + +/** Macro verifying that two numerical values are close to each other in order to pass unit test. */ +#define QPOASES_TEST_FOR_NEAR( x,y ) if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; } + +/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */ +#define QPOASES_TEST_FOR_TOL( x,tol ) if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; } + +/** Macro verifying that a logical expression holds in order to pass unit test. */ +#define QPOASES_TEST_FOR_TRUE( x ) if ( (x) == 0 ) { return TEST_FAILED; } + + + +BEGIN_NAMESPACE_QPOASES + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_UNIT_TESTING_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so index a081e31ab178fd..35f83d4159541b 100644 Binary files a/third_party/acados/larch64/lib/libacados.so and b/third_party/acados/larch64/lib/libacados.so differ diff --git a/third_party/acados/larch64/lib/libblasfeo.so b/third_party/acados/larch64/lib/libblasfeo.so index e2191897a10225..6b0e26de795ee1 100644 Binary files a/third_party/acados/larch64/lib/libblasfeo.so and b/third_party/acados/larch64/lib/libblasfeo.so differ diff --git a/third_party/acados/larch64/lib/libhpipm.so b/third_party/acados/larch64/lib/libhpipm.so index 002eda375a5265..bebd5e891145e2 100644 Binary files a/third_party/acados/larch64/lib/libhpipm.so and b/third_party/acados/larch64/lib/libhpipm.so differ diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 index d64e40ec607035..0611c8a9390013 100644 Binary files a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 and b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 differ diff --git a/third_party/acados/larch64/t_renderer b/third_party/acados/larch64/t_renderer index f9e7d16601d24a..b4ff8bc319ba99 100755 Binary files a/third_party/acados/larch64/t_renderer and b/third_party/acados/larch64/t_renderer differ diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so index 4e80f7c76bace2..b8a6280d68f175 100644 Binary files a/third_party/acados/x86_64/lib/libacados.so and b/third_party/acados/x86_64/lib/libacados.so differ diff --git a/third_party/acados/x86_64/lib/libblasfeo.so b/third_party/acados/x86_64/lib/libblasfeo.so index 26d5a3dbe917ef..e4a19caf136451 100644 Binary files a/third_party/acados/x86_64/lib/libblasfeo.so and b/third_party/acados/x86_64/lib/libblasfeo.so differ diff --git a/third_party/acados/x86_64/lib/libhpipm.so b/third_party/acados/x86_64/lib/libhpipm.so index 40e2e4e7d4775d..428c76a1178553 100644 Binary files a/third_party/acados/x86_64/lib/libhpipm.so and b/third_party/acados/x86_64/lib/libhpipm.so differ diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 index cf5e550faa95f7..cf3604688c846b 100644 Binary files a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 and b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 differ diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer index e995a209b79a11..5ced89c28dd1c6 100755 Binary files a/third_party/acados/x86_64/t_renderer and b/third_party/acados/x86_64/t_renderer differ diff --git a/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore deleted file mode 100644 index ac06c0cf85b3ae..00000000000000 --- a/third_party/bootstrap/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/icons/ diff --git a/third_party/bootstrap/bootstrap-icons.svg b/third_party/bootstrap/bootstrap-icons.svg deleted file mode 100644 index 692a27ea55c32c..00000000000000 --- a/third_party/bootstrap/bootstrap-icons.svg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d7ecfaac355e51c9b95319fdf4681cf4c423109fd477e961af588b92607a76da -size 1087239 diff --git a/third_party/bootstrap/pull.sh b/third_party/bootstrap/pull.sh deleted file mode 100755 index 0b03b4db9eff3e..00000000000000 --- a/third_party/bootstrap/pull.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash -set -e - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -cd $DIR - -if [ ! -d icons/ ]; then - git clone https://github.com/twbs/icons/ -fi - -cd icons -git fetch --all -git checkout d5aa187483a1b0b186f87adcfa8576350d970d98 -cp bootstrap-icons.svg ../ diff --git a/third_party/catch2/include/catch2/catch.hpp b/third_party/catch2/include/catch2/catch.hpp index 6a31e5b0ca8cd4..d2a12427b25819 100644 --- a/third_party/catch2/include/catch2/catch.hpp +++ b/third_party/catch2/include/catch2/catch.hpp @@ -1,3 +1,17970 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:27da57c7a06d09be8dd81fab7246b79e7892b6ae7e4e49ba8631f1d5a955e3fc -size 657276 +/* + * Catch v2.13.9 + * Generated: 2022-04-12 22:37:23.260201 + * ---------------------------------------------------------- + * This file has been merged from multiple headers. Please don't edit it directly + * Copyright (c) 2022 Two Blue Cubes Ltd. All rights reserved. + * + * Distributed under the Boost Software License, Version 1.0. (See accompanying + * file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) + */ +#ifndef TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +#define TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +// start catch.hpp + + +#define CATCH_VERSION_MAJOR 2 +#define CATCH_VERSION_MINOR 13 +#define CATCH_VERSION_PATCH 9 + +#ifdef __clang__ +# pragma clang system_header +#elif defined __GNUC__ +# pragma GCC system_header +#endif + +// start catch_suppress_warnings.h + +#ifdef __clang__ +# ifdef __ICC // icpc defines the __clang__ macro +# pragma warning(push) +# pragma warning(disable: 161 1682) +# else // __ICC +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wpadded" +# pragma clang diagnostic ignored "-Wswitch-enum" +# pragma clang diagnostic ignored "-Wcovered-switch-default" +# endif +#elif defined __GNUC__ + // Because REQUIREs trigger GCC's -Wparentheses, and because still + // supported version of g++ have only buggy support for _Pragmas, + // Wparentheses have to be suppressed globally. +# pragma GCC diagnostic ignored "-Wparentheses" // See #674 for details + +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-variable" +# pragma GCC diagnostic ignored "-Wpadded" +#endif +// end catch_suppress_warnings.h +#if defined(CATCH_CONFIG_MAIN) || defined(CATCH_CONFIG_RUNNER) +# define CATCH_IMPL +# define CATCH_CONFIG_ALL_PARTS +#endif + +// In the impl file, we want to have access to all parts of the headers +// Can also be used to sanely support PCHs +#if defined(CATCH_CONFIG_ALL_PARTS) +# define CATCH_CONFIG_EXTERNAL_INTERFACES +# if defined(CATCH_CONFIG_DISABLE_MATCHERS) +# undef CATCH_CONFIG_DISABLE_MATCHERS +# endif +# if !defined(CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER) +# define CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER +# endif +#endif + +#if !defined(CATCH_CONFIG_IMPL_ONLY) +// start catch_platform.h + +// See e.g.: +// https://opensource.apple.com/source/CarbonHeaders/CarbonHeaders-18.1/TargetConditionals.h.auto.html +#ifdef __APPLE__ +# include +# if (defined(TARGET_OS_OSX) && TARGET_OS_OSX == 1) || \ + (defined(TARGET_OS_MAC) && TARGET_OS_MAC == 1) +# define CATCH_PLATFORM_MAC +# elif (defined(TARGET_OS_IPHONE) && TARGET_OS_IPHONE == 1) +# define CATCH_PLATFORM_IPHONE +# endif + +#elif defined(linux) || defined(__linux) || defined(__linux__) +# define CATCH_PLATFORM_LINUX + +#elif defined(WIN32) || defined(__WIN32__) || defined(_WIN32) || defined(_MSC_VER) || defined(__MINGW32__) +# define CATCH_PLATFORM_WINDOWS +#endif + +// end catch_platform.h + +#ifdef CATCH_IMPL +# ifndef CLARA_CONFIG_MAIN +# define CLARA_CONFIG_MAIN_NOT_DEFINED +# define CLARA_CONFIG_MAIN +# endif +#endif + +// start catch_user_interfaces.h + +namespace Catch { + unsigned int rngSeed(); +} + +// end catch_user_interfaces.h +// start catch_tag_alias_autoregistrar.h + +// start catch_common.h + +// start catch_compiler_capabilities.h + +// Detect a number of compiler features - by compiler +// The following features are defined: +// +// CATCH_CONFIG_COUNTER : is the __COUNTER__ macro supported? +// CATCH_CONFIG_WINDOWS_SEH : is Windows SEH supported? +// CATCH_CONFIG_POSIX_SIGNALS : are POSIX signals supported? +// CATCH_CONFIG_DISABLE_EXCEPTIONS : Are exceptions enabled? +// **************** +// Note to maintainers: if new toggles are added please document them +// in configuration.md, too +// **************** + +// In general each macro has a _NO_ form +// (e.g. CATCH_CONFIG_NO_POSIX_SIGNALS) which disables the feature. +// Many features, at point of detection, define an _INTERNAL_ macro, so they +// can be combined, en-mass, with the _NO_ forms later. + +#ifdef __cplusplus + +# if (__cplusplus >= 201402L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201402L) +# define CATCH_CPP14_OR_GREATER +# endif + +# if (__cplusplus >= 201703L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +# define CATCH_CPP17_OR_GREATER +# endif + +#endif + +// Only GCC compiler should be used in this block, so other compilers trying to +// mask themselves as GCC should be ignored. +#if defined(__GNUC__) && !defined(__clang__) && !defined(__ICC) && !defined(__CUDACC__) && !defined(__LCC__) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic pop" ) + +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) + +#endif + +#if defined(__clang__) + +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic pop" ) + +// As of this writing, IBM XL's implementation of __builtin_constant_p has a bug +// which results in calls to destructors being emitted for each temporary, +// without a matching initialization. In practice, this can result in something +// like `std::string::~string` being called on an uninitialized value. +// +// For example, this code will likely segfault under IBM XL: +// ``` +// REQUIRE(std::string("12") + "34" == "1234") +// ``` +// +// Therefore, `CATCH_INTERNAL_IGNORE_BUT_WARN` is not implemented. +# if !defined(__ibmxl__) && !defined(__CUDACC__) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) /* NOLINT(cppcoreguidelines-pro-type-vararg, hicpp-vararg) */ +# endif + +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wexit-time-destructors\"" ) \ + _Pragma( "clang diagnostic ignored \"-Wglobal-constructors\"") + +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wparentheses\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-variable\"" ) + +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wgnu-zero-variadic-macro-arguments\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-template\"" ) + +#endif // __clang__ + +//////////////////////////////////////////////////////////////////////////////// +// Assume that non-Windows platforms support posix signals by default +#if !defined(CATCH_PLATFORM_WINDOWS) + #define CATCH_INTERNAL_CONFIG_POSIX_SIGNALS +#endif + +//////////////////////////////////////////////////////////////////////////////// +// We know some environments not to support full POSIX signals +#if defined(__CYGWIN__) || defined(__QNX__) || defined(__EMSCRIPTEN__) || defined(__DJGPP__) + #define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +#endif + +#ifdef __OS400__ +# define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +# define CATCH_CONFIG_COLOUR_NONE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Android somehow still does not support std::to_string +#if defined(__ANDROID__) +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING +# define CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Not all Windows environments support SEH properly +#if defined(__MINGW32__) +# define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH +#endif + +//////////////////////////////////////////////////////////////////////////////// +// PS4 +#if defined(__ORBIS__) +# define CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Cygwin +#ifdef __CYGWIN__ + +// Required for some versions of Cygwin to declare gettimeofday +// see: http://stackoverflow.com/questions/36901803/gettimeofday-not-declared-in-this-scope-cygwin +# define _BSD_SOURCE +// some versions of cygwin (most) do not support std::to_string. Use the libstd check. +// https://gcc.gnu.org/onlinedocs/gcc-4.8.2/libstdc++/api/a01053_source.html line 2812-2813 +# if !((__cplusplus >= 201103L) && defined(_GLIBCXX_USE_C99) \ + && !defined(_GLIBCXX_HAVE_BROKEN_VSWPRINTF)) + +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING + +# endif +#endif // __CYGWIN__ + +//////////////////////////////////////////////////////////////////////////////// +// Visual C++ +#if defined(_MSC_VER) + +// Universal Windows platform does not support SEH +// Or console colours (or console at all...) +# if defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_APP) +# define CATCH_CONFIG_COLOUR_NONE +# else +# define CATCH_INTERNAL_CONFIG_WINDOWS_SEH +# endif + +# if !defined(__clang__) // Handle Clang masquerading for msvc + +// MSVC traditional preprocessor needs some workaround for __VA_ARGS__ +// _MSVC_TRADITIONAL == 0 means new conformant preprocessor +// _MSVC_TRADITIONAL == 1 means old traditional non-conformant preprocessor +# if !defined(_MSVC_TRADITIONAL) || (defined(_MSVC_TRADITIONAL) && _MSVC_TRADITIONAL) +# define CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +# endif // MSVC_TRADITIONAL + +// Only do this if we're not using clang on Windows, which uses `diagnostic push` & `diagnostic pop` +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION __pragma( warning(push) ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION __pragma( warning(pop) ) +# endif // __clang__ + +#endif // _MSC_VER + +#if defined(_REENTRANT) || defined(_MSC_VER) +// Enable async processing, as -pthread is specified or no additional linking is required +# define CATCH_INTERNAL_CONFIG_USE_ASYNC +#endif // _MSC_VER + +//////////////////////////////////////////////////////////////////////////////// +// Check if we are compiled with -fno-exceptions or equivalent +#if defined(__EXCEPTIONS) || defined(__cpp_exceptions) || defined(_CPPUNWIND) +# define CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED +#endif + +//////////////////////////////////////////////////////////////////////////////// +// DJGPP +#ifdef __DJGPP__ +# define CATCH_INTERNAL_CONFIG_NO_WCHAR +#endif // __DJGPP__ + +//////////////////////////////////////////////////////////////////////////////// +// Embarcadero C++Build +#if defined(__BORLANDC__) + #define CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// Use of __COUNTER__ is suppressed during code analysis in +// CLion/AppCode 2017.2.x and former, because __COUNTER__ is not properly +// handled by it. +// Otherwise all supported compilers support COUNTER macro, +// but user still might want to turn it off +#if ( !defined(__JETBRAINS_IDE__) || __JETBRAINS_IDE__ >= 20170300L ) + #define CATCH_INTERNAL_CONFIG_COUNTER +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// RTX is a special version of Windows that is real time. +// This means that it is detected as Windows, but does not provide +// the same set of capabilities as real Windows does. +#if defined(UNDER_RTSS) || defined(RTX64_BUILD) + #define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH + #define CATCH_INTERNAL_CONFIG_NO_ASYNC + #define CATCH_CONFIG_COLOUR_NONE +#endif + +#if !defined(_GLIBCXX_USE_C99_MATH_TR1) +#define CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Various stdlib support checks that require __has_include +#if defined(__has_include) + // Check if string_view is available and usable + #if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW + #endif + + // Check if optional is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if byte is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # include + # if defined(__cpp_lib_byte) && (__cpp_lib_byte > 0) + # define CATCH_INTERNAL_CONFIG_CPP17_BYTE + # endif + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if variant is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # if defined(__clang__) && (__clang_major__ < 8) + // work around clang bug with libstdc++ https://bugs.llvm.org/show_bug.cgi?id=31852 + // fix should be in clang 8, workaround in libstdc++ 8.2 + # include + # if defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # define CATCH_CONFIG_NO_CPP17_VARIANT + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__clang__) && (__clang_major__ < 8) + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) +#endif // defined(__has_include) + +#if defined(CATCH_INTERNAL_CONFIG_COUNTER) && !defined(CATCH_CONFIG_NO_COUNTER) && !defined(CATCH_CONFIG_COUNTER) +# define CATCH_CONFIG_COUNTER +#endif +#if defined(CATCH_INTERNAL_CONFIG_WINDOWS_SEH) && !defined(CATCH_CONFIG_NO_WINDOWS_SEH) && !defined(CATCH_CONFIG_WINDOWS_SEH) && !defined(CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH) +# define CATCH_CONFIG_WINDOWS_SEH +#endif +// This is set by default, because we assume that unix compilers are posix-signal-compatible by default. +#if defined(CATCH_INTERNAL_CONFIG_POSIX_SIGNALS) && !defined(CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_POSIX_SIGNALS) +# define CATCH_CONFIG_POSIX_SIGNALS +#endif +// This is set by default, because we assume that compilers with no wchar_t support are just rare exceptions. +#if !defined(CATCH_INTERNAL_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_WCHAR) +# define CATCH_CONFIG_WCHAR +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_CPP11_TO_STRING) +# define CATCH_CONFIG_CPP11_TO_STRING +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_NO_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_CPP17_OPTIONAL) +# define CATCH_CONFIG_CPP17_OPTIONAL +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_NO_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_CPP17_STRING_VIEW) +# define CATCH_CONFIG_CPP17_STRING_VIEW +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_VARIANT) && !defined(CATCH_CONFIG_NO_CPP17_VARIANT) && !defined(CATCH_CONFIG_CPP17_VARIANT) +# define CATCH_CONFIG_CPP17_VARIANT +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_BYTE) && !defined(CATCH_CONFIG_NO_CPP17_BYTE) && !defined(CATCH_CONFIG_CPP17_BYTE) +# define CATCH_CONFIG_CPP17_BYTE +#endif + +#if defined(CATCH_CONFIG_EXPERIMENTAL_REDIRECT) +# define CATCH_INTERNAL_CONFIG_NEW_CAPTURE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_NEW_CAPTURE) && !defined(CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NEW_CAPTURE) +# define CATCH_CONFIG_NEW_CAPTURE +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED) && !defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +# define CATCH_CONFIG_DISABLE_EXCEPTIONS +#endif + +#if defined(CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_NO_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_POLYFILL_ISNAN) +# define CATCH_CONFIG_POLYFILL_ISNAN +#endif + +#if defined(CATCH_INTERNAL_CONFIG_USE_ASYNC) && !defined(CATCH_INTERNAL_CONFIG_NO_ASYNC) && !defined(CATCH_CONFIG_NO_USE_ASYNC) && !defined(CATCH_CONFIG_USE_ASYNC) +# define CATCH_CONFIG_USE_ASYNC +#endif + +#if defined(CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_NO_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_ANDROID_LOGWRITE) +# define CATCH_CONFIG_ANDROID_LOGWRITE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_NO_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_GLOBAL_NEXTAFTER) +# define CATCH_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Even if we do not think the compiler has that warning, we still have +// to provide a macro that can be used by the code. +#if !defined(CATCH_INTERNAL_START_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS +#endif + +// The goal of this macro is to avoid evaluation of the arguments, but +// still have the compiler warn on problems inside... +#if !defined(CATCH_INTERNAL_IGNORE_BUT_WARN) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) +#endif + +#if defined(__APPLE__) && defined(__apple_build_version__) && (__clang_major__ < 10) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#elif defined(__clang__) && (__clang_major__ < 5) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +#define CATCH_TRY if ((true)) +#define CATCH_CATCH_ALL if ((false)) +#define CATCH_CATCH_ANON(type) if ((false)) +#else +#define CATCH_TRY try +#define CATCH_CATCH_ALL catch (...) +#define CATCH_CATCH_ANON(type) catch (type) +#endif + +#if defined(CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_NO_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) +#define CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#endif + +// end catch_compiler_capabilities.h +#define INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) name##line +#define INTERNAL_CATCH_UNIQUE_NAME_LINE( name, line ) INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) +#ifdef CATCH_CONFIG_COUNTER +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __COUNTER__ ) +#else +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __LINE__ ) +#endif + +#include +#include +#include + +// We need a dummy global operator<< so we can bring it into Catch namespace later +struct Catch_global_namespace_dummy {}; +std::ostream& operator<<(std::ostream&, Catch_global_namespace_dummy); + +namespace Catch { + + struct CaseSensitive { enum Choice { + Yes, + No + }; }; + + class NonCopyable { + NonCopyable( NonCopyable const& ) = delete; + NonCopyable( NonCopyable && ) = delete; + NonCopyable& operator = ( NonCopyable const& ) = delete; + NonCopyable& operator = ( NonCopyable && ) = delete; + + protected: + NonCopyable(); + virtual ~NonCopyable(); + }; + + struct SourceLineInfo { + + SourceLineInfo() = delete; + SourceLineInfo( char const* _file, std::size_t _line ) noexcept + : file( _file ), + line( _line ) + {} + + SourceLineInfo( SourceLineInfo const& other ) = default; + SourceLineInfo& operator = ( SourceLineInfo const& ) = default; + SourceLineInfo( SourceLineInfo&& ) noexcept = default; + SourceLineInfo& operator = ( SourceLineInfo&& ) noexcept = default; + + bool empty() const noexcept { return file[0] == '\0'; } + bool operator == ( SourceLineInfo const& other ) const noexcept; + bool operator < ( SourceLineInfo const& other ) const noexcept; + + char const* file; + std::size_t line; + }; + + std::ostream& operator << ( std::ostream& os, SourceLineInfo const& info ); + + // Bring in operator<< from global namespace into Catch namespace + // This is necessary because the overload of operator<< above makes + // lookup stop at namespace Catch + using ::operator<<; + + // Use this in variadic streaming macros to allow + // >> +StreamEndStop + // as well as + // >> stuff +StreamEndStop + struct StreamEndStop { + std::string operator+() const; + }; + template + T const& operator + ( T const& value, StreamEndStop ) { + return value; + } +} + +#define CATCH_INTERNAL_LINEINFO \ + ::Catch::SourceLineInfo( __FILE__, static_cast( __LINE__ ) ) + +// end catch_common.h +namespace Catch { + + struct RegistrarForTagAliases { + RegistrarForTagAliases( char const* alias, char const* tag, SourceLineInfo const& lineInfo ); + }; + +} // end namespace Catch + +#define CATCH_REGISTER_TAG_ALIAS( alias, spec ) \ + CATCH_INTERNAL_START_WARNINGS_SUPPRESSION \ + CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + namespace{ Catch::RegistrarForTagAliases INTERNAL_CATCH_UNIQUE_NAME( AutoRegisterTagAlias )( alias, spec, CATCH_INTERNAL_LINEINFO ); } \ + CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION + +// end catch_tag_alias_autoregistrar.h +// start catch_test_registry.h + +// start catch_interfaces_testcase.h + +#include + +namespace Catch { + + class TestSpec; + + struct ITestInvoker { + virtual void invoke () const = 0; + virtual ~ITestInvoker(); + }; + + class TestCase; + struct IConfig; + + struct ITestCaseRegistry { + virtual ~ITestCaseRegistry(); + virtual std::vector const& getAllTests() const = 0; + virtual std::vector const& getAllTestsSorted( IConfig const& config ) const = 0; + }; + + bool isThrowSafe( TestCase const& testCase, IConfig const& config ); + bool matchTest( TestCase const& testCase, TestSpec const& testSpec, IConfig const& config ); + std::vector filterTests( std::vector const& testCases, TestSpec const& testSpec, IConfig const& config ); + std::vector const& getAllTestCasesSorted( IConfig const& config ); + +} + +// end catch_interfaces_testcase.h +// start catch_stringref.h + +#include +#include +#include +#include + +namespace Catch { + + /// A non-owning string class (similar to the forthcoming std::string_view) + /// Note that, because a StringRef may be a substring of another string, + /// it may not be null terminated. + class StringRef { + public: + using size_type = std::size_t; + using const_iterator = const char*; + + private: + static constexpr char const* const s_empty = ""; + + char const* m_start = s_empty; + size_type m_size = 0; + + public: // construction + constexpr StringRef() noexcept = default; + + StringRef( char const* rawChars ) noexcept; + + constexpr StringRef( char const* rawChars, size_type size ) noexcept + : m_start( rawChars ), + m_size( size ) + {} + + StringRef( std::string const& stdString ) noexcept + : m_start( stdString.c_str() ), + m_size( stdString.size() ) + {} + + explicit operator std::string() const { + return std::string(m_start, m_size); + } + + public: // operators + auto operator == ( StringRef const& other ) const noexcept -> bool; + auto operator != (StringRef const& other) const noexcept -> bool { + return !(*this == other); + } + + auto operator[] ( size_type index ) const noexcept -> char { + assert(index < m_size); + return m_start[index]; + } + + public: // named queries + constexpr auto empty() const noexcept -> bool { + return m_size == 0; + } + constexpr auto size() const noexcept -> size_type { + return m_size; + } + + // Returns the current start pointer. If the StringRef is not + // null-terminated, throws std::domain_exception + auto c_str() const -> char const*; + + public: // substrings and searches + // Returns a substring of [start, start + length). + // If start + length > size(), then the substring is [start, size()). + // If start > size(), then the substring is empty. + auto substr( size_type start, size_type length ) const noexcept -> StringRef; + + // Returns the current start pointer. May not be null-terminated. + auto data() const noexcept -> char const*; + + constexpr auto isNullTerminated() const noexcept -> bool { + return m_start[m_size] == '\0'; + } + + public: // iterators + constexpr const_iterator begin() const { return m_start; } + constexpr const_iterator end() const { return m_start + m_size; } + }; + + auto operator += ( std::string& lhs, StringRef const& sr ) -> std::string&; + auto operator << ( std::ostream& os, StringRef const& sr ) -> std::ostream&; + + constexpr auto operator "" _sr( char const* rawChars, std::size_t size ) noexcept -> StringRef { + return StringRef( rawChars, size ); + } +} // namespace Catch + +constexpr auto operator "" _catch_sr( char const* rawChars, std::size_t size ) noexcept -> Catch::StringRef { + return Catch::StringRef( rawChars, size ); +} + +// end catch_stringref.h +// start catch_preprocessor.hpp + + +#define CATCH_RECURSION_LEVEL0(...) __VA_ARGS__ +#define CATCH_RECURSION_LEVEL1(...) CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL2(...) CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL3(...) CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL4(...) CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL5(...) CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(__VA_ARGS__))) + +#ifdef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_EXPAND_VARGS(...) __VA_ARGS__ +// MSVC needs more evaluations +#define CATCH_RECURSION_LEVEL6(...) CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(__VA_ARGS__))) +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL6(CATCH_RECURSION_LEVEL6(__VA_ARGS__)) +#else +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL5(__VA_ARGS__) +#endif + +#define CATCH_REC_END(...) +#define CATCH_REC_OUT + +#define CATCH_EMPTY() +#define CATCH_DEFER(id) id CATCH_EMPTY() + +#define CATCH_REC_GET_END2() 0, CATCH_REC_END +#define CATCH_REC_GET_END1(...) CATCH_REC_GET_END2 +#define CATCH_REC_GET_END(...) CATCH_REC_GET_END1 +#define CATCH_REC_NEXT0(test, next, ...) next CATCH_REC_OUT +#define CATCH_REC_NEXT1(test, next) CATCH_DEFER ( CATCH_REC_NEXT0 ) ( test, next, 0) +#define CATCH_REC_NEXT(test, next) CATCH_REC_NEXT1(CATCH_REC_GET_END test, next) + +#define CATCH_REC_LIST0(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2(f, x, peek, ...) f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) + +#define CATCH_REC_LIST0_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2_UD(f, userdata, x, peek, ...) f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) + +// Applies the function macro `f` to each of the remaining parameters, inserts commas between the results, +// and passes userdata as the first parameter to each invocation, +// e.g. CATCH_REC_LIST_UD(f, x, a, b, c) evaluates to f(x, a), f(x, b), f(x, c) +#define CATCH_REC_LIST_UD(f, userdata, ...) CATCH_RECURSE(CATCH_REC_LIST2_UD(f, userdata, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define CATCH_REC_LIST(f, ...) CATCH_RECURSE(CATCH_REC_LIST2(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define INTERNAL_CATCH_EXPAND1(param) INTERNAL_CATCH_EXPAND2(param) +#define INTERNAL_CATCH_EXPAND2(...) INTERNAL_CATCH_NO## __VA_ARGS__ +#define INTERNAL_CATCH_DEF(...) INTERNAL_CATCH_DEF __VA_ARGS__ +#define INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE(...) INTERNAL_CATCH_STRINGIZE2(__VA_ARGS__) +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_STRINGIZE2(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) +#else +// MSVC is adding extra space and needs another indirection to expand INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE2(...) INTERNAL_CATCH_STRINGIZE3(__VA_ARGS__) +#define INTERNAL_CATCH_STRINGIZE3(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) (INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) + 1) +#endif + +#define INTERNAL_CATCH_MAKE_NAMESPACE2(...) ns_##__VA_ARGS__ +#define INTERNAL_CATCH_MAKE_NAMESPACE(name) INTERNAL_CATCH_MAKE_NAMESPACE2(name) + +#define INTERNAL_CATCH_REMOVE_PARENS(...) INTERNAL_CATCH_EXPAND1(INTERNAL_CATCH_DEF __VA_ARGS__) + +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) decltype(get_wrapper()) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__)) +#else +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) INTERNAL_CATCH_EXPAND_VARGS(decltype(get_wrapper())) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_EXPAND_VARGS(INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__))) +#endif + +#define INTERNAL_CATCH_MAKE_TYPE_LISTS_FROM_TYPES(...)\ + CATCH_REC_LIST(INTERNAL_CATCH_MAKE_TYPE_LIST,__VA_ARGS__) + +#define INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_0) INTERNAL_CATCH_REMOVE_PARENS(_0) +#define INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_0, _1) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_1) +#define INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_0, _1, _2) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_1, _2) +#define INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_0, _1, _2, _3) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_1, _2, _3) +#define INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_0, _1, _2, _3, _4) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_1, _2, _3, _4) +#define INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_0, _1, _2, _3, _4, _5) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_1, _2, _3, _4, _5) +#define INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_0, _1, _2, _3, _4, _5, _6) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_1, _2, _3, _4, _5, _6) +#define INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_0, _1, _2, _3, _4, _5, _6, _7) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_1, _2, _3, _4, _5, _6, _7) +#define INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_1, _2, _3, _4, _5, _6, _7, _8) +#define INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9) +#define INTERNAL_CATCH_REMOVE_PARENS_11_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10) + +#define INTERNAL_CATCH_VA_NARGS_IMPL(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, N, ...) N + +#define INTERNAL_CATCH_TYPE_GEN\ + template struct TypeList {};\ + template\ + constexpr auto get_wrapper() noexcept -> TypeList { return {}; }\ + template class...> struct TemplateTypeList{};\ + template class...Cs>\ + constexpr auto get_wrapper() noexcept -> TemplateTypeList { return {}; }\ + template\ + struct append;\ + template\ + struct rewrap;\ + template class, typename...>\ + struct create;\ + template class, typename>\ + struct convert;\ + \ + template \ + struct append { using type = T; };\ + template< template class L1, typename...E1, template class L2, typename...E2, typename...Rest>\ + struct append, L2, Rest...> { using type = typename append, Rest...>::type; };\ + template< template class L1, typename...E1, typename...Rest>\ + struct append, TypeList, Rest...> { using type = L1; };\ + \ + template< template class Container, template class List, typename...elems>\ + struct rewrap, List> { using type = TypeList>; };\ + template< template class Container, template class List, class...Elems, typename...Elements>\ + struct rewrap, List, Elements...> { using type = typename append>, typename rewrap, Elements...>::type>::type; };\ + \ + template